Added fake landing xy velocity measurement to lpe. (#5820)

Flight tested and is working.

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -408,12 +408,16 @@ void BlockLocalPositionEstimator::update()
 	for (int i = 0; i < n_x; i++) {
 		for (int j = 0; j <= i; j++) {
 			if (!PX4_ISFINITE(_P(i, j))) {
+				mavlink_and_console_log_info(&mavlink_log_pub,
+							     "[lpe] reinit P (%d, %d) not finite", i, j);
 				reinit_P = true;
 			}
 
 			if (i == j) {
 				// make sure diagonal elements are positive
 				if (_P(i, i) <= 0) {
+					mavlink_and_console_log_info(&mavlink_log_pub,
+								     "[lpe] reinit P (%d, %d) negative", i, j);
 					reinit_P = true;
 				}
 
@@ -430,7 +434,6 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (reinit_P) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
 		initP();
 	}
 
@@ -837,16 +840,19 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 void BlockLocalPositionEstimator::initP()
 {
 	_P.setZero();
-	_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID; // initialize to twice valid condition
-	_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID;
-	_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID;
-	_P(X_vx, X_vx) = 1;
-	_P(X_vy, X_vy) = 1;
-	_P(X_vz, X_vz) = 1;
+	// initialize to twice valid condition
+	_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
+	_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
+	_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
+	_P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
+	_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
+	// use vxy thresh for vz init as well
+	_P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
+	// initialize bias uncertainty to small values to keep them stable
 	_P(X_bx, X_bx) = 1e-6;
 	_P(X_by, X_by) = 1e-6;
 	_P(X_bz, X_bz) = 1e-6;
-	_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID;
+	_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
 }
 
 void BlockLocalPositionEstimator::initSS()

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -135,7 +135,7 @@ public:
 	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
 	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
 	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
-	enum {Y_land_z = 0, n_y_land};
+	enum {Y_land_vx, Y_land_vy, Y_land_agl = 0, n_y_land};
 	enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};
 
 	BlockLocalPositionEstimator();

src/modules/local_position_estimator/sensors/land.cpp

@@ -46,19 +46,23 @@ void BlockLocalPositionEstimator::landCorrect()
 	Matrix<float, n_y_land, n_x> C;
 	C.setZero();
 	// y = -(z - tz)
-	C(Y_land_z, X_z) = -1; // measured altitude, negative down dir.
-	C(Y_land_z, X_tz) = 1; // measured altitude, negative down dir.
+	C(Y_land_vx, X_vx) = 1;
+	C(Y_land_vy, X_vy) = 1;
+	C(Y_land_agl, X_z) = -1; // measured altitude, negative down dir.
+	C(Y_land_agl, X_tz) = 1; // measured altitude, negative down dir.
 
 	// use parameter covariance
 	SquareMatrix<float, n_y_land> R;
 	R.setZero();
-	R(0, 0) = _land_z_stddev.get() * _land_z_stddev.get();
+	R(Y_land_vx, Y_land_vx) = 0.1;
+	R(Y_land_vy, Y_land_vy) = 0.1;
+	R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();
 
 	// residual
 	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
 	Vector<float, n_y_land> r = y - C * _x;
-	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = R(0, 0);
+	_pub_innov.get().hagl_innov = r(Y_land_agl);
+	_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);