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>();
12
Matrix<float, n_y, n_x>
C;
13
C.
setIdentity
();
14
float
data
[] = {1,2,3,4,5};
15
Vector<float, n_y>
r(data);
16
17
Vector<float, n_x>
dx;
18
SquareMatrix<float, n_x>
dP;
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 : */
matrix::Vector
Definition:
Dual.hpp:22
matrix::SquareMatrix
Definition:
SquareMatrix.hpp:26
matrix
Definition:
AxisAngle.hpp:11
matrix::Matrix::setIdentity
void setIdentity()
Definition:
Matrix.hpp:447
matrix::isEqual
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition:
Matrix.hpp:571
data
uint8_t * data
Definition:
dataman.cpp:149
test_macros.hpp
TEST
#define TEST(X)
Definition:
test_macros.hpp:14
matrix::Matrix
Definition:
Matrix.hpp:36
main
int main()
Definition:
filter.cpp:6
P
P[0][0]
Definition:
quatCovMat.c:44
filter.hpp
src
lib
matrix
test
filter.cpp
Generated by
1.8.13