Browse Source

lpe: small fixes

sbg
TSC21 7 years ago committed by Lorenz Meier
parent
commit
691c35f47e
  1. 4
      src/modules/local_position_estimator/sensors/flow.cpp
  2. 4
      src/modules/local_position_estimator/sensors/gps.cpp
  3. 9
      src/modules/local_position_estimator/sensors/lidar.cpp
  4. 4
      src/modules/local_position_estimator/sensors/mocap.cpp
  5. 4
      src/modules/local_position_estimator/sensors/sonar.cpp

4
src/modules/local_position_estimator/sensors/flow.cpp

@ -170,7 +170,7 @@ void BlockLocalPositionEstimator::flowCorrect()
Vector<float, 2> r = y - C * _x; Vector<float, 2> r = y - C * _x;
// residual covariance // residual covariance
Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose()) + R; Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose() + R;
// publish innovations // publish innovations
_pub_innov.get().flow_innov[0] = r(0); _pub_innov.get().flow_innov[0] = r(0);
@ -179,7 +179,7 @@ void BlockLocalPositionEstimator::flowCorrect()
_pub_innov.get().flow_innov_var[1] = S(1, 1); _pub_innov.get().flow_innov_var[1] = S(1, 1);
// residual covariance, (inverse) // residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I = inv(C); Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);
// fault detection // fault detection
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);

4
src/modules/local_position_estimator/sensors/gps.cpp

@ -185,7 +185,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
Vector<float, n_y_gps> r = y - C * x0; Vector<float, n_y_gps> r = y - C * x0;
// residual covariance // 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 // publish innovations
for (int i = 0; i < 6; i ++) { for (int i = 0; i < 6; i ++) {
@ -194,7 +194,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
} }
// residual covariance, (inverse) // 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 // fault detection
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);

9
src/modules/local_position_estimator/sensors/lidar.cpp

@ -85,15 +85,18 @@ void BlockLocalPositionEstimator::lidarCorrect()
R(0, 0) = cov; R(0, 0) = cov;
} }
// residual
Vector<float, n_y_lidar> r = y - C * _x;
// residual covariance // residual covariance
Matrix<float, n_y_lidar, n_y_lidar> S = C * _P * C.transpose()) + R; Matrix<float, n_y_lidar, n_y_lidar> S = C * _P * C.transpose() + R;
// residual covariance, (inverse)
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv(C);
// publish innovations // publish innovations
_pub_innov.get().hagl_innov = r(0); _pub_innov.get().hagl_innov = r(0);
_pub_innov.get().hagl_innov_var = S(0, 0); _pub_innov.get().hagl_innov_var = S(0, 0);
// residual covariance, (inverse)
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);
// fault detection // fault detection
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);

4
src/modules/local_position_estimator/sensors/mocap.cpp

@ -76,7 +76,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
// residual // residual
Vector<float, n_y_mocap> r = y - C * _x; Vector<float, n_y_mocap> r = y - C * _x;
// residual covariance // residual covariance
Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose()) + R; Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
// publish innovations // publish innovations
for (int i = 0; i < 3; i ++) { for (int i = 0; i < 3; i ++) {
@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
} }
// residual covariance, (inverse) // residual covariance, (inverse)
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv(C); Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);
// fault detection // fault detection
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);

4
src/modules/local_position_estimator/sensors/sonar.cpp

@ -106,14 +106,14 @@ void BlockLocalPositionEstimator::sonarCorrect()
// residual // residual
Vector<float, n_y_sonar> r = y - C * _x; Vector<float, n_y_sonar> r = y - C * _x;
// residual covariance // residual covariance
Matrix<float, n_y_sonar, n_y_sonar> S = C * _P * C.transpose()) + R; Matrix<float, n_y_sonar, n_y_sonar> S = C * _P * C.transpose() + R;
// publish innovations // publish innovations
_pub_innov.get().hagl_innov = r(0); _pub_innov.get().hagl_innov = r(0);
_pub_innov.get().hagl_innov_var = S(0, 0); _pub_innov.get().hagl_innov_var = S(0, 0);
// residual covariance, (inverse) // residual covariance, (inverse)
Matrix<float, n_y_sonar, n_y_sonar> S_I = inv(C); Matrix<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);
// fault detection // fault detection
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);

Loading…
Cancel
Save