PX4 Firmware
PX4 Autopilot Software http://px4.io
filter.hpp
Go to the documentation of this file.
1
#pragma once
2
3
#include "
math.hpp
"
4
5
namespace
matrix
{
6
7
template
<
typename
Type,
size_t
M,
size_t
N>
8
int
kalman_correct
(
9
const
Matrix<Type, M, M>
&
P
,
10
const
Matrix<Type, N, M>
& C,
11
const
Matrix<Type, N, N>
& R,
12
const
Matrix<Type, N, 1>
&r,
13
Matrix<Type, M, 1>
& dx,
14
Matrix<Type, M, M>
& dP,
15
Type & beta
16
)
17
{
18
SquareMatrix<Type, N>
S_I =
SquareMatrix<Type, N>
(C*P*C.
T
() + R).I();
19
Matrix<Type, M, N>
K = P*C.
T
()*S_I;
20
dx = K*r;
21
beta =
Scalar<Type>
(r.T()*S_I*r);
22
dP = K*C*P*(-1);
23
return
0;
24
}
25
26
}
// namespace matrix
matrix::Matrix::T
Matrix< Type, N, M > T() const
Definition:
Matrix.hpp:368
matrix::SquareMatrix
Definition:
SquareMatrix.hpp:26
matrix::Scalar
Definition:
Scalar.hpp:17
matrix
Definition:
AxisAngle.hpp:11
math.hpp
matrix::kalman_correct
int kalman_correct(const Matrix< Type, M, M > &P, const Matrix< Type, N, M > &C, const Matrix< Type, N, N > &R, const Matrix< Type, N, 1 > &r, Matrix< Type, M, 1 > &dx, Matrix< Type, M, M > &dP, Type &beta)
Definition:
filter.hpp:8
matrix::Matrix< Type, M, M >
P
P[0][0]
Definition:
quatCovMat.c:44
src
lib
matrix
matrix
filter.hpp
Generated by
1.8.13