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

@@ -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

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

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -872,7 +872,6 @@ void BlockLocalPositionEstimator::predict()
 	}
 
 	_P += dP;
-
 	_xLowPass.update(_x);
 	_aglLowPass.update(agl());
 }

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
+	// kalman filter correction always
-	if (!(_sensorFault & SENSOR_BARO)) {
+	Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
-		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
+	Vector<float, n_x> dx = K * r;
-		Vector<float, n_x> dx = K * r;
+	_x += dx;
-		_x += dx;
+	_P -= K * C * _P;
-		_P -= K * C * _P;
-	}
 }
 
 void BlockLocalPositionEstimator::baroCheckTimeout()

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
+	// kalman filter correction always for GPS
-	if (!(_sensorFault & SENSOR_GPS)) {
+	Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
-		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
+	Vector<float, n_x> dx = K * r;
-		Vector<float, n_x> dx = K * r;
+	_x += dx;
-		_x += dx;
+	_P -= K * C * _P;
-		_P -= K * C * _P;
-	}
 }
 
 void BlockLocalPositionEstimator::gpsCheckTimeout()

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
+	// kalman filter correction always for land detector
-	if (!(_sensorFault & SENSOR_LAND)) {
+	Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
-		Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
+	Vector<float, n_x> dx = K * r;
-		Vector<float, n_x> dx = K * r;
+	_x += dx;
-		_x += dx;
+	_P -= K * C * _P;
-		_P -= K * C * _P;
-	}
 }
 
 void BlockLocalPositionEstimator::landCheckTimeout()

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
+	// kalman filter correction always
-	if (!(_sensorFault & SENSOR_LIDAR)) {
+	Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
-		Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
+	Vector<float, n_x> dx = K * r;
-		Vector<float, n_x> dx = K * r;
+	_x += dx;
-		_x += dx;
+	_P -= K * C * _P;
-		_P -= K * C * _P;
-	}
 }
 
 void BlockLocalPositionEstimator::lidarCheckTimeout()

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
+	// kalman filter correction always
-	if (!(_sensorFault & SENSOR_MOCAP)) {
+	Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
-		Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
+	Vector<float, n_x> dx = K * r;
-		Vector<float, n_x> dx = K * r;
+	_x += dx;
-		_x += dx;
+	_P -= K * C * _P;
-		_P -= K * C * _P;
-	}
 }
 
 void BlockLocalPositionEstimator::mocapCheckTimeout()

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -137,7 +137,6 @@ void BlockLocalPositionEstimator::sonarCorrect()
 		_x += dx;
 		_P -= K * C * _P;
 	}
-
 }
 
 void BlockLocalPositionEstimator::sonarCheckTimeout()