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>
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< Type, N, M > T() const
Definition: Matrix.hpp:368
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
P[0][0]
Definition: quatCovMat.c:44