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

More filter testing.

Browse Source
This commit is contained in:
jgoppert
2015-11-04 20:58:56 -05:00
parent 76cf91c5fd
commit eddc55827a
2 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/filter.hpp
+10 -10
View File
@@ -6,17 +6,17 @@ namespace matrix {
template<typename Type, size_t M, size_t N>
void kalman_correct(
const Matrix<Type, M, M> & P,
const Matrix<Type, N, M> & C,
const Matrix<Type, N, N> & R,
const Vector<Type, N> &r,
Vector<Type, M> & dx,
float & beta
)
const Matrix<Type, M, M> & P,
const Matrix<Type, N, M> & C,
const Matrix<Type, N, N> & R,
const Vector<Type, N> &r,
Vector<Type, M> & dx,
float & beta
)
{
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
dx = P*C.T()*S_I*r;
beta = Scalar<Type>(r.T()*S_I*r);
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
dx = P*C.T()*S_I*r;
beta = Scalar<Type>(r.T()*S_I*r);
}
}; // namespace matrix
test/filter.cpp
+6
View File
@@ -22,6 +22,12 @@ int main()
kalman_correct<float, 6, 5>(P, C, R, r, dx, beta);
dx.print();
printf("beta: %g\n", beta);
float data_check[] = {0.5,1,1.5,2,2.5,0};
Vector<float, n_x> dx_check(data_check);
assert(dx == dx_check);
return 0;
}