1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
| #pragma once
|
| #include "math.hpp"
|
| namespace matrix {
|
| template<typename Type, size_t M, size_t N>
| 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
| )
| {
| SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
| Matrix<Type, M, N> K = P*C.T()*S_I;
| dx = K*r;
| beta = Scalar<Type>(r.T()*S_I*r);
| dP = K*C*P*(-1);
| return 0;
| }
|
| } // namespace matrix
|
|
