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

lpe: fix innovation covariance publishing

Browse Source
This commit is contained in:
TSC21
2017-11-03 23:45:43 +01:00
committed by Lorenz Meier
parent 76fcd99241
commit 392999c2df
5 changed files with 44 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+8 -4
View File
@@ -168,14 +168,18 @@ void BlockLocalPositionEstimator::flowCorrect()
// residual
Vector<float, 2> r = y - C * _x;
// residual covariance
Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose()) + R;
// publish innovations
_pub_innov.get().flow_innov[0] = r(0);
_pub_innov.get().flow_innov[1] = r(1);
_pub_innov.get().flow_innov_var[0] = R(0, 0);
_pub_innov.get().flow_innov_var[1] = R(1, 1);
_pub_innov.get().flow_innov_var[0] = S(0, 0);
_pub_innov.get().flow_innov_var[1] = S(1, 1);
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
inv<float, n_y_flow>(C * _P * C.transpose() + R);
Matrix<float, n_y_flow, n_y_flow> S_I = inv(C);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);