mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
36 lines
749 B
C++
36 lines
749 B
C++
#include <assert.h>
|
|
#include <stdio.h>
|
|
|
|
#include <matrix/integration.hpp>
|
|
|
|
using namespace matrix;
|
|
|
|
// instantiate template to ensure coverage check
|
|
template int integrate_rk4<float, 6>(
|
|
Vector<float, 6> (*f)(float, Vector<float, 6>),
|
|
const Vector<float, 6> & y0,
|
|
float t0,
|
|
float h,
|
|
Vector<float, 6> & y1
|
|
);
|
|
|
|
Vector<float, 6> f(float t, Vector<float, 6> y);
|
|
|
|
Vector<float, 6> f(float t, Vector<float, 6> y) {
|
|
return ones<float, 6, 1>();
|
|
}
|
|
|
|
int main()
|
|
{
|
|
Vector<float, 6> y = ones<float, 6, 1>();
|
|
float t = 1;
|
|
float h = 0.1f;
|
|
y.T().print();
|
|
integrate_rk4(f, y, t, h, y);
|
|
y.T().print();
|
|
assert(y == (ones<float, 6, 1>()*1.1f));
|
|
return 0;
|
|
}
|
|
|
|
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|