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

Added innovation logging to LPE. (#5124)

Browse Source
This commit is contained in:
James Goppert
2016-07-22 19:19:30 -04:00
committed by GitHub
parent 0730e681bc
commit a4ef364f80
7 changed files with 28 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+5 -1
View File
@@ -122,7 +122,7 @@ void BlockLocalPositionEstimator::flowCorrect()
C(Y_flow_x, X_x) = 1;
C(Y_flow_y, X_y) = 1;
Matrix<float, n_y_flow, n_y_flow> R;
SquareMatrix<float, n_y_flow> R;
R.setZero();
R(Y_flow_x, Y_flow_x) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
@@ -131,6 +131,10 @@ void BlockLocalPositionEstimator::flowCorrect()
// residual
Vector<float, 2> r = y - C * _x;
_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);
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =