PX4 Firmware
PX4 Autopilot Software http://px4.io
filter.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 #include <matrix/filter.hpp>
3 
4 using namespace matrix;
5 
6 int main()
7 {
8  const size_t n_x = 6;
9  const size_t n_y = 5;
10  SquareMatrix<float, n_x> P = eye<float, n_x>();
11  SquareMatrix<float, n_y> R = eye<float, n_y>();
13  C.setIdentity();
14  float data[] = {1,2,3,4,5};
15  Vector<float, n_y> r(data);
16 
19  float beta = 0;
20  kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
21 
22  float data_check[] = {0.5,1,1.5,2,2.5,0};
23  Vector<float, n_x> dx_check(data_check);
24  TEST(isEqual(dx, dx_check));
25 
26  return 0;
27 }
28 
29 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
void setIdentity()
Definition: Matrix.hpp:447
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
uint8_t * data
Definition: dataman.cpp:149
#define TEST(X)
Definition: test_macros.hpp:14
int main()
Definition: filter.cpp:6
P[0][0]
Definition: quatCovMat.c:44