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

lpe: small fixes

Browse Source
This commit is contained in:
TSC21
2017-11-03 23:45:43 +01:00
committed by Lorenz Meier
parent e6efb5ec0a
commit 691c35f47e
5 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+2 -2
View File
@@ -185,7 +185,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
Vector<float, n_y_gps> r = y - C * x0;
// residual covariance
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose()) + R;
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
// publish innovations
for (int i = 0; i < 6; i ++) {
@@ -194,7 +194,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
}
// residual covariance, (inverse)
Matrix<float, n_y_gps, n_y_gps> S_I = inv(C);
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);