LPE tuning for GPS delay in sim.

posix-configs/SITL/init/rcS_lpe_jmavsim_iris

@@ -32,12 +32,6 @@ param set COM_RC_IN_MODE 1
 param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
-param set MIS_TAKEOFF_ALT 5.0
-param set MPC_HOLD_MAX_Z 2.0
-param set MPC_HOLD_XY_DZ 0.1
-param set MPC_HOLD_Z_DZ 0.1
-param set MPC_Z_VEL_MAX 2.0
-param set MPC_Z_VEL_P 0.4
 
 # changes for LPE
 param set COM_RC_IN_MODE 1
@@ -55,13 +49,10 @@ sensors start
 commander start
 land_detector start multicopter
 navigator start
-attitude_estimator_q start
-local_position_estimator start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -70,6 +61,7 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
 mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
+attitude_estimator_q start
+local_position_estimator start

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -279,15 +279,15 @@ void BlockLocalPositionEstimator::update()
 
 	// determine if we should start estimating
 	_canEstimateZ =
-		(_baroInitialized && !_baroFault);
+		(_baroInitialized && _baroFault < FAULT_SEVERE);
 	_canEstimateXY =
-		(_gpsInitialized && !_gpsFault) ||
-		(_flowInitialized && !_flowFault) ||
-		(_visionInitialized && !_visionFault) ||
-		(_mocapInitialized && !_mocapFault);
+		(_gpsInitialized && _gpsFault < FAULT_SEVERE) ||
+		(_flowInitialized && _flowFault < FAULT_SEVERE) ||
+		(_visionInitialized && _visionFault < FAULT_SEVERE) ||
+		(_mocapInitialized && _mocapFault < FAULT_SEVERE);
 	_canEstimateT =
-		(_lidarInitialized && !_lidarFault) ||
-		(_sonarInitialized && !_sonarFault);
+		(_lidarInitialized && _lidarFault < FAULT_SEVERE) ||
+		(_sonarInitialized && _sonarFault < FAULT_SEVERE);
 
 	if (_canEstimateXY) {
 		_time_last_xy = _timeStamp;
@@ -1164,7 +1164,7 @@ void BlockLocalPositionEstimator::correctFlow()
 	}
 
 	// kalman filter correction if no fault
-	if (_flowFault == FAULT_NONE) {
+	if (_flowFault < FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_flow> K =
 			_P * C.transpose() * S_I;
 		_x += K * r;
@@ -1260,7 +1260,7 @@ void BlockLocalPositionEstimator::correctSonar()
 	}
 
 	// kalman filter correction if no fault
-	if (_sonarFault == FAULT_NONE) {
+	if (_sonarFault < FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_sonar> K =
 			_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
@@ -1319,7 +1319,7 @@ void BlockLocalPositionEstimator::correctBaro()
 	}
 
 	// kalman filter correction if no fault
-	if (_baroFault == FAULT_NONE) {
+	if (_baroFault < FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 
@@ -1402,7 +1402,7 @@ void BlockLocalPositionEstimator::correctLidar()
 	}
 
 	// kalman filter correction if no fault
-	if (_lidarFault == FAULT_NONE) {
+	if (_lidarFault < FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 
@@ -1525,9 +1525,6 @@ void BlockLocalPositionEstimator::correctGps()
 			_gpsFault = FAULT_MINOR;
 		}
 
-		// trust GPS less
-		S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
-
 	} else if (_gpsFault) {
 		_gpsFault = FAULT_NONE;
 		mavlink_log_info(_mavlink_fd, "[lpe] GPS OK");
@@ -1535,7 +1532,7 @@ void BlockLocalPositionEstimator::correctGps()
 	}
 
 	// kalman filter correction if no hard fault
-	if (_gpsFault == FAULT_NONE) {
+	if (_gpsFault < FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
 		_x += K * r;
 		_P -= K * C * _P;
@@ -1590,7 +1587,7 @@ void BlockLocalPositionEstimator::correctVision()
 	}
 
 	// kalman filter correction if no fault
-	if (_visionFault == FAULT_NONE) {
+	if (_visionFault <  FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
 		_x += K * r;
 		_P -= K * C * _P;
@@ -1647,7 +1644,7 @@ void BlockLocalPositionEstimator::correctMocap()
 	}
 
 	// kalman filter correction if no fault
-	if (_mocapFault == FAULT_NONE) {
+	if (_mocapFault <  FAULT_SEVERE) {
 		Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
 		_x += K * r;
 		_P -= K * C * _P;

src/modules/local_position_estimator/params.c

@@ -171,7 +171,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
  * @max 2
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 1.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f);
 
 /**
  * GPS z velocity standard deviation.
@@ -182,7 +182,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 1.0f);
  * @max 2
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 1.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
 
 /**
  * GPS max eph