fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 19:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Made kalman filter correction function usable.

Browse Source
This commit is contained in:
jgoppert
2015-11-09 19:55:20 -05:00
parent 222a97e73f
commit e972a0a111
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/filter.hpp
+6 -3
View File
@@ -6,17 +6,20 @@ namespace matrix {
template<typename Type, size_t M, size_t N> template<typename Type, size_t M, size_t N>
void kalman_correct( void kalman_correct(
const Matrix<Type, M, M> & P, const SquareMatrix<Type, M> & P,
const Matrix<Type, N, M> & C, const Matrix<Type, N, M> & C,
const Matrix<Type, N, N> & R, const SquareMatrix<Type, N> & R,
const Vector<Type, N> &r, const Vector<Type, N> &r,
Vector<Type, M> & dx, Vector<Type, M> & dx,
SquareMatrix<Type, M> & dP,
float & beta float & beta
) )
{ {
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I(); SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
dx = P*C.T()*S_I*r; Matrix<Type, M, N> K = P*C.T()*S_I;
dx = K*r;
beta = Scalar<Type>(r.T()*S_I*r); beta = Scalar<Type>(r.T()*S_I*r);
dP = K*C*P*(-1);
} }
}; // namespace matrix }; // namespace matrix