mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
27 lines
558 B
C++
27 lines
558 B
C++
#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
|