fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 15:43: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/sonar.cpp
+3 -1
View File
@@ -99,12 +99,14 @@ void BlockLocalPositionEstimator::sonarCorrect()
C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.
// covariance matrix
Matrix<float, n_y_sonar, n_y_sonar> R;
SquareMatrix<float, n_y_sonar> R;
R.setZero();
R(0, 0) = cov;
// residual
Vector<float, n_y_sonar> r = y - C * _x;
_pub_innov.get().hagl_innov = r(0);
_pub_innov.get().hagl_innov_var = R(0, 0);
// residual covariance, (inverse)
Matrix<float, n_y_sonar, n_y_sonar> S_I =