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

Reorder innovations in LPE

Browse Source
This commit is contained in:
kamilritz
2019-12-05 11:29:29 +01:00
committed by Julian Kent
parent b73c80428e
commit 61b569245b
3 changed files with 18 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+6 -6
View File
@@ -189,20 +189,20 @@ void BlockLocalPositionEstimator::gpsCorrect()
Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().gps_hvel[0] = r(3);
_pub_innov.get().gps_hvel[1] = r(4);
_pub_innov.get().gps_vvel = r(5);
_pub_innov.get().gps_hpos[0] = r(0);
_pub_innov.get().gps_hpos[1] = r(1);
_pub_innov.get().gps_vpos = r(2);
_pub_innov.get().gps_hvel[0] = r(3);
_pub_innov.get().gps_hvel[1] = r(4);
_pub_innov.get().gps_vvel = r(5);
// publish innovation variances
_pub_innov_var.get().gps_hvel[0] = S(3, 3);
_pub_innov_var.get().gps_hvel[1] = S(4, 4);
_pub_innov_var.get().gps_vvel = S(5, 5);
_pub_innov_var.get().gps_hpos[0] = S(0, 0);
_pub_innov_var.get().gps_hpos[1] = S(1, 1);
_pub_innov_var.get().gps_vpos = S(2, 2);
_pub_innov_var.get().gps_hvel[0] = S(3, 3);
_pub_innov_var.get().gps_hvel[1] = S(4, 4);
_pub_innov_var.get().gps_vvel = S(5, 5);
// residual covariance, (inverse)
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
src/modules/local_position_estimator/sensors/mocap.cpp
+6 -6
View File
@@ -145,20 +145,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
Matrix<float, n_y_mocap, n_y_mocap> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().ev_hvel[0] = NAN;
_pub_innov.get().ev_hvel[1] = NAN;
_pub_innov.get().ev_vvel = NAN;
_pub_innov.get().ev_hpos[0] = r(0);
_pub_innov.get().ev_hpos[1] = r(1);
_pub_innov.get().ev_vpos = r(2);
_pub_innov.get().ev_hvel[0] = NAN;
_pub_innov.get().ev_hvel[1] = NAN;
_pub_innov.get().ev_vvel = NAN;
// publish innovation variances
_pub_innov_var.get().ev_hvel[0] = NAN;
_pub_innov_var.get().ev_hvel[1] = NAN;
_pub_innov_var.get().ev_vvel = NAN;
_pub_innov_var.get().ev_hpos[0] = S(0, 0);
_pub_innov_var.get().ev_hpos[1] = S(1, 1);
_pub_innov_var.get().ev_vpos = S(2, 2);
_pub_innov_var.get().ev_hvel[0] = NAN;
_pub_innov_var.get().ev_hvel[1] = NAN;
_pub_innov_var.get().ev_vvel = NAN;
// residual covariance, (inverse)
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);
src/modules/local_position_estimator/sensors/vision.cpp
+6 -6
View File
@@ -162,20 +162,20 @@ void BlockLocalPositionEstimator::visionCorrect()
Matrix<float, n_y_vision, n_y_vision> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().ev_hvel[0] = NAN;
_pub_innov.get().ev_hvel[1] = NAN;
_pub_innov.get().ev_vvel = NAN;
_pub_innov.get().ev_hpos[0] = r(0, 0);
_pub_innov.get().ev_hpos[1] = r(1, 0);
_pub_innov.get().ev_vpos = r(2, 0);
_pub_innov.get().ev_hvel[0] = NAN;
_pub_innov.get().ev_hvel[1] = NAN;
_pub_innov.get().ev_vvel = NAN;
// publish innovation variances
_pub_innov_var.get().ev_hvel[0] = NAN;
_pub_innov_var.get().ev_hvel[1] = NAN;
_pub_innov_var.get().ev_vvel = NAN;
_pub_innov_var.get().ev_hpos[0] = S(0, 0);
_pub_innov_var.get().ev_hpos[1] = S(1, 1);
_pub_innov_var.get().ev_vpos = S(2, 2);
_pub_innov_var.get().ev_hvel[0] = NAN;
_pub_innov_var.get().ev_hvel[1] = NAN;
_pub_innov_var.get().ev_vvel = NAN;
// residual covariance, (inverse)
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);