From c28cd76e5f9fd3c10b2e682312acddb42f44e576 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 23 Dec 2016 15:08:37 -0500 Subject: [PATCH] LPE fault relaxation and sitl fix (#6146) * Set LPE FUSE for standard iris sitl config. * Relax fault detection handling. * Always correct lidar. --- posix-configs/SITL/init/lpe/iris | 3 +++ posix-configs/SITL/init/lpe/iris_opt_flow | 2 +- .../BlockLocalPositionEstimator.cpp | 1 - .../local_position_estimator/sensors/baro.cpp | 12 +++++------- .../local_position_estimator/sensors/gps.cpp | 17 +++++++++-------- .../local_position_estimator/sensors/land.cpp | 17 +++++++++-------- .../local_position_estimator/sensors/lidar.cpp | 12 +++++------- .../local_position_estimator/sensors/mocap.cpp | 12 +++++------- .../local_position_estimator/sensors/sonar.cpp | 1 - 9 files changed, 37 insertions(+), 40 deletions(-) diff --git a/posix-configs/SITL/init/lpe/iris b/posix-configs/SITL/init/lpe/iris index c2b37e76e3..d212110a96 100644 --- a/posix-configs/SITL/init/lpe/iris +++ b/posix-configs/SITL/init/lpe/iris @@ -42,6 +42,9 @@ param set MPC_Z_VEL_P 0.6 param set MPC_Z_VEL_I 0.15 param set EKF2_GBIAS_INIT 0.01 param set EKF2_ANGERR_INIT 0.01 +param set LPE_FUSION 247 +# 11110111 no vis yaw (1 << 3) + replay tryapplyparams simulator start -s rgbledsim start diff --git a/posix-configs/SITL/init/lpe/iris_opt_flow b/posix-configs/SITL/init/lpe/iris_opt_flow index 4e82e16282..f6fc727df0 100644 --- a/posix-configs/SITL/init/lpe/iris_opt_flow +++ b/posix-configs/SITL/init/lpe/iris_opt_flow @@ -61,7 +61,7 @@ param set MIS_TAKEOFF_ALT 2 param set NAV_ACC_RAD 1.0 param set CBRK_GPSFAIL 240024 param set LPE_FUSION 246 -# 11110110 no vis (1 << 3) yaw and no gps (1 << 0) +# 11110110 no vis yaw (1 << 3) and no gps (1 << 0) replay tryapplyparams simulator start -s diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index d15b21f06c..cefb1db899 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -872,7 +872,6 @@ void BlockLocalPositionEstimator::predict() } _P += dP; - _xLowPass.update(_x); _aglLowPass.update(agl()); } diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index 13ab3939fc..f74c93f381 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -85,13 +85,11 @@ void BlockLocalPositionEstimator::baroCorrect() mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK"); } - // kalman filter correction if no fault - if (!(_sensorFault & SENSOR_BARO)) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - _x += dx; - _P -= K * C * _P; - } + // kalman filter correction always + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + _P -= K * C * _P; } void BlockLocalPositionEstimator::baroCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 6dad1e972f..d7beab2fb2 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -190,7 +190,10 @@ void BlockLocalPositionEstimator::gpsCorrect() // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); - if (beta > BETA_TABLE[n_y_gps]) { + // artifically increase beta threshhold to prevent fault during landing + float beta_thresh = 1e2f; + + if (beta / BETA_TABLE[n_y_gps] > beta_thresh) { if (!(_sensorFault & SENSOR_GPS)) { mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), @@ -203,13 +206,11 @@ void BlockLocalPositionEstimator::gpsCorrect() mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK"); } - // kalman filter correction if no hard fault - if (!(_sensorFault & SENSOR_GPS)) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - _x += dx; - _P -= K * C * _P; - } + // kalman filter correction always for GPS + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + _P -= K * C * _P; } void BlockLocalPositionEstimator::gpsCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp index 8b951edc5f..5a0fd47fab 100644 --- a/src/modules/local_position_estimator/sensors/land.cpp +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -67,7 +67,10 @@ void BlockLocalPositionEstimator::landCorrect() // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); - if (beta > BETA_TABLE[n_y_land]) { + // artifically increase beta threshhold to prevent fault during landing + float beta_thresh = 1e2f; + + if (beta / BETA_TABLE[n_y_land] > beta_thresh) { if (!(_sensorFault & SENSOR_LAND)) { _sensorFault |= SENSOR_LAND; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta)); @@ -81,13 +84,11 @@ void BlockLocalPositionEstimator::landCorrect() mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK"); } - // kalman filter correction if no fault - if (!(_sensorFault & SENSOR_LAND)) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - _x += dx; - _P -= K * C * _P; - } + // kalman filter correction always for land detector + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + _P -= K * C * _P; } void BlockLocalPositionEstimator::landCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index 8100d38dbf..fac18d33de 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -113,13 +113,11 @@ void BlockLocalPositionEstimator::lidarCorrect() mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK"); } - // kalman filter correction if no fault - if (!(_sensorFault & SENSOR_LIDAR)) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - _x += dx; - _P -= K * C * _P; - } + // kalman filter correction always + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + _P -= K * C * _P; } void BlockLocalPositionEstimator::lidarCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index c7b729ac6e..027af444b8 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -91,13 +91,11 @@ void BlockLocalPositionEstimator::mocapCorrect() //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK"); } - // kalman filter correction if no fault - if (!(_sensorFault & SENSOR_MOCAP)) { - Matrix K = _P * C.transpose() * S_I; - Vector dx = K * r; - _x += dx; - _P -= K * C * _P; - } + // kalman filter correction always + Matrix K = _P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + _P -= K * C * _P; } void BlockLocalPositionEstimator::mocapCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 6ea193e758..2eaebcc69d 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -137,7 +137,6 @@ void BlockLocalPositionEstimator::sonarCorrect() _x += dx; _P -= K * C * _P; } - } void BlockLocalPositionEstimator::sonarCheckTimeout()