PX4 Firmware
PX4 Autopilot Software http://px4.io
integration.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 #include <matrix/integration.hpp>
3 
4 using namespace matrix;
5 
6 Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/);
7 
8 Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/) {
9  float v = -sin(t);
10  return v*ones<float, 6, 1>();
11 }
12 
13 int main()
14 {
15  Vector<float, 6> y = ones<float, 6, 1>();
16  Vector<float, 3> u = ones<float, 3, 1>();
17  float t0 = 0;
18  float tf = 2;
19  float h = 0.001f;
20  integrate_rk4(f, y, u, t0, tf, h, y);
21  float v = 1 + cos(tf) - cos(t0);
22  TEST(isEqual(y, (ones<float, 6, 1>()*v)));
23  return 0;
24 }
25 
26 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
int integrate_rk4(Vector< Type, M >(*f)(Type, const Matrix< Type, M, 1 > &x, const Matrix< Type, N, 1 > &u), const Matrix< Type, M, 1 > &y0, const Matrix< Type, N, 1 > &u, Type t0, Type tf, Type h0, Matrix< Type, M, 1 > &y1)
Definition: integration.hpp:8
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:286
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define TEST(X)
Definition: test_macros.hpp:14
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:279
int main()
Definition: integration.cpp:13