LPE Variance Dependent Publication (#4914)

* Use variance to control publishing for LPE. * Don't stop publishing if we have gps/ baro. * LPE tuning and cleanup. * Added bias saturation to LPE. * Added vector enabled low pass filter block. * Added rk4 integration and pub lowpass to LPE. * Fix std::abs issue on mac/ reset lowpass on state reset. * Don't estimate gyro bias when rotating at high speed att_est_q. * Lowered low pass on position to 5 Hz for LPE. * Streamline state space update for LPE. * Added health flags to est2 log. * Revert to old tuning, more conservative, less faults. * Formatting. * Fix for fault message on LPE. * Added subscription throttling to LPE. * Formatting.
2026-06-04 21:23:57 +08:00 · 2016-07-01 11:43:09 -04:00
parent 2a395c3fec
commit 00dfc99e08
17 changed files with 417 additions and 247 deletions
@@ -84,6 +84,7 @@ float BlockLowPass::update(float input)
 	return getState();
 }

+
 float BlockHighPass::update(float input)
 {
 	float b = 2 * float(M_PI) * getFCut() * getDt();
@@ -127,6 +127,45 @@ protected:
 	control::BlockParamFloat _fCut;
 };

+template<class Type, size_t M>
+class __EXPORT BlockLowPassVector: public Block
+{
+public:
+// methods
+	BlockLowPassVector(SuperBlock *parent,
+			   const char *name) :
+		Block(parent, name),
+		_state(),
+		_fCut(this, "") // only one parameter, no need to name
+	{
+		for (int i = 0; i < M; i++) {
+			_state(i) = 0.0f / 0.0f;
+		}
+	};
+	virtual ~BlockLowPassVector() {};
+	matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
+	{
+		for (int i = 0; i < M; i++) {
+			if (!PX4_ISFINITE(getState()(i))) {
+				setState(input);
+			}
+		}
+
+		float b = 2 * float(M_PI) * getFCut() * getDt();
+		float a = b / (1 + b);
+		setState(input * a + getState() * (1 - a));
+		return getState();
+	}
+// accessors
+	matrix::Vector<Type, M> getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(const matrix::Vector<Type, M> &state) { _state = state; }
+private:
+// attributes
+	matrix::Vector<Type, M> _state;
+	control::BlockParamFloat _fCut;
+};
+
 /**
 * A high pass filter as described here:
 * http://en.wikipedia.org/wiki/High-pass_filter.
@@ -797,6 +797,7 @@ bool AttitudeEstimatorQ::update(float dt)

 	_q.normalize();

+
 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame
 	// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
@@ -810,7 +811,9 @@ bool AttitudeEstimatorQ::update(float dt)
 	corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

 	// Gyro bias estimation
-	_gyro_bias += corr * (_w_gyro_bias * dt);
+	if (_gyro.length() < 1.0f) {
+		_gyro_bias += corr * (_w_gyro_bias * dt);
+	}

 	for (int i = 0; i < 3; i++) {
 		_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
@@ -33,8 +33,9 @@
 using namespace matrix;
 using namespace control;

-static const float GPS_DELAY_MAX = 0.5; // seconds
-static const float HIST_STEP = 0.05; // 20 hz
+static const float GPS_DELAY_MAX = 0.5f; // seconds
+static const float HIST_STEP = 0.05f; // 20 hz
+static const float BIAS_MAX = 1e-1f;
 static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP;
 static const size_t N_DIST_SUBS = 4;

@@ -134,6 +135,10 @@ public:

 	BlockLocalPositionEstimator();
 	void update();
+	Vector<float, n_x> dynamics(
+		float t,
+		const Vector<float, n_x> &x,
+		const Vector<float, n_u> &u);
 	virtual ~BlockLocalPositionEstimator();

 private:
@@ -144,6 +149,9 @@ private:
 	// methods
 	// ----------------------------
 	void initP();
+	void initSS();
+	void updateSSStates();
+	void updateSSParams();

 	// predict the next state
 	void predict();
@@ -195,6 +203,7 @@ private:

 	// misc
 	float agl();
+	void correctionLogic(Vector<float, n_x> &dx);
 	void detectDistanceSensors();
 	void updateHome();

@@ -207,11 +216,8 @@ private:
 	// ----------------------------

 	// subscriptions
-	uORB::Subscription<vehicle_status_s> _sub_status;
 	uORB::Subscription<actuator_armed_s> _sub_armed;
-	uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
 	uORB::Subscription<vehicle_attitude_s> _sub_att;
-	uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
 	uORB::Subscription<optical_flow_s> _sub_flow;
 	uORB::Subscription<sensor_combined_s> _sub_sensor;
 	uORB::Subscription<parameter_update_s> _sub_param_update;
@@ -302,6 +308,9 @@ private:
 	BlockStats<float, n_y_mocap> _mocapStats;
 	BlockStats<double, n_y_gps> _gpsStats;

+	// low pass
+	BlockLowPassVector<float, n_x> _xLowPass;
+
 	// delay blocks
 	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
 	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
@@ -346,9 +355,9 @@ private:
 	float _flowMeanQual;

 	// status
-	bool _canEstimateXY;
-	bool _canEstimateZ;
-	bool _canEstimateT;
+	bool _validXY;
+	bool _validZ;
+	bool _validTZ;
 	bool _xyTimeout;
 	bool _zTimeout;
 	bool _tzTimeout;
@@ -372,4 +381,8 @@ private:
 	Vector<float, n_x>  _x; // state vector
 	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
+	Matrix<float, n_x, n_x>  _A; // dynamics matrix
+	Matrix<float, n_x, n_u>  _B; // input matrix
+	Matrix<float, n_u, n_u>  _R; // input covariance
+	Matrix<float, n_x, n_x>  _Q; // process noise covariance
 };
@@ -35,7 +35,7 @@ px4_add_module(
 	MAIN local_position_estimator
 	COMPILE_FLAGS -Os
 	STACK_MAIN 5700
-	STACK_MAX 10000
+	STACK_MAX 13000
 	SRCS
 		local_position_estimator_main.cpp
 		BlockLocalPositionEstimator.cpp
@@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[])
 		deamon_task = px4_task_spawn_cmd("lp_estimator",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_MAX - 5,
-						 12000,
+						 13000,
 						 local_position_estimator_thread_main,
 						 (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
 		return 0;
@@ -88,34 +88,32 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
 PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);

 /**
- * Accelerometer xy standard deviation
+ * Accelerometer xy noise density
 *
- * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
- * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
- * Since accels sampled at 1000 Hz.
+ * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
 *
- * should be 0.0464
+ * Larger than data sheet to account for tilt error.
 *
 * @group Local Position Estimator
- * @unit m/s^2
+ * @unit m/s^2/srqt(Hz)
 * @min 0.00001
 * @max 2
 * @decimal 4
 */
-PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
+PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);

 /**
- * Accelerometer z standard deviation
+ * Accelerometer z noise density
 *
- * (see Accel x comments)
+ * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
 *
 * @group Local Position Estimator
- * @unit m/s^2
+ * @unit m/s^2/srqt(Hz)
 * @min 0.00001
 * @max 2
 * @decimal 4
 */
-PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
+PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);

 /**
 * Barometric presssure altitude z standard deviation.
@@ -145,11 +143,11 @@ PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
 * @max 0.4
 * @decimal 2
 */
-PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
+PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f);


 /**
- * GPS xy standard deviation.
+ * Minimum GPS xy standard deviation, uses reported EPH if greater.
 *
 * @group Local Position Estimator
 * @unit m
@@ -157,10 +155,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
 * @max 5
 * @decimal 2
 */
-PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f);

 /**
- * GPS z standard deviation.
+ * Minimum GPS z standard deviation, uses reported EPV if greater.
 *
 * @group Local Position Estimator
 * @unit m
@@ -168,10 +166,11 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
 * @max 200
 * @decimal 2
 */
-PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f);

 /**
 * GPS xy velocity standard deviation.
+ * EPV used if greater than this value.
 *
 * @group Local Position Estimator
 * @unit m/s
@@ -193,7 +192,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f);
 PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);

 /**
- * GPS max eph
+ * Max EPH allowed for GPS initialization
 *
 * @group Local Position Estimator
 * @unit m
@@ -204,7 +203,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
 PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);

 /**
- * GPS max epv
+ * Max EPV allowed for GPS initialization
 *
 * @group Local Position Estimator
 * @unit m
@@ -258,24 +257,30 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
 /**
 * Position propagation noise density
 *
+ * Increase to trust measurements more.
+ * Decrease to trust model more.
+ *
 * @group Local Position Estimator
 * @unit m/s/sqrt(Hz)
 * @min 0
 * @max 1
 * @decimal 8
 */
-PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);

 /**
 * Velocity propagation noise density
 *
+ * Increase to trust measurements more.
+ * Decrease to trust model more.
+ *
 * @group Local Position Estimator
 * @unit (m/s)/s/sqrt(Hz)
 * @min 0
 * @max 1
 * @decimal 8
 */
-PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);

 /**
 * Accel bias propagation noise density
@@ -332,3 +337,14 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
 */
 PARAM_DEFINE_FLOAT(LPE_LON, -86.929);

+/**
+ * Cut frequency for state publication
+ *
+ * @group Local Position Estimator
+ * @unit Hz
+ * @min 5
+ * @max 1000
+ * @decimal 0
+ */
+PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);
+
@@ -76,8 +76,8 @@ void BlockLocalPositionEstimator::baroCorrect()

 	if (beta > BETA_TABLE[n_y_baro]) {
 		if (_baroFault < FAULT_MINOR) {
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
-			//double(r(0)), double(beta));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
+						     double(r(0)), double(beta));
 			_baroFault = FAULT_MINOR;
 		}

@@ -90,14 +90,7 @@ void BlockLocalPositionEstimator::baroCorrect()
 	if (_baroFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-
-		if (!_canEstimateXY) {
-			dx(X_x) = 0;
-			dx(X_y) = 0;
-			dx(X_vx) = 0;
-			dx(X_vy) = 0;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -49,7 +49,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	}

 	// calculate range to center of image for flow
-	if (!_canEstimateT) {
+	if (!_validTZ) {
 		return -1;
 	}

@@ -153,7 +153,9 @@ void BlockLocalPositionEstimator::flowCorrect()
 	if (_flowFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_flow> K =
 			_P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;

 	} else {
@@ -8,11 +8,25 @@ extern orb_advert_t mavlink_log_pub;
 // to initialize
 static const uint32_t 		REQ_GPS_INIT_COUNT = 10;
 static const uint32_t 		GPS_TIMEOUT =      1000000; // 1.0 s
-static const float 			GPS_EPH_MIN =      2.0; // m, min allowed by gps self reporting
-static const float 			GPS_EPV_MIN =      2.0; // m, min allowed by gps self reporting

 void BlockLocalPositionEstimator::gpsInit()
 {
+	// check for good gps signal
+	uint8_t nSat = _sub_gps.get().satellites_used;
+	float eph = _sub_gps.get().eph;
+	float epv = _sub_gps.get().epv;
+	uint8_t fix_type = _sub_gps.get().fix_type;
+
+	if (
+		nSat < 6 ||
+		eph > _gps_eph_max.get() ||
+		epv > _gps_epv_max.get() ||
+		fix_type < 3
+	) {
+		_gpsStats.reset();
+		return;
+	}
+
 	// measure
 	Vector<double, n_y_gps> y;

@@ -50,21 +64,6 @@ void BlockLocalPositionEstimator::gpsInit()

 int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
 {
-	// check for good gps signal
-	uint8_t nSat = _sub_gps.get().satellites_used;
-	float eph = _sub_gps.get().eph;
-	float epv = _sub_gps.get().epv;
-	uint8_t fix_type = _sub_gps.get().fix_type;
-
-	if (
-		nSat < 6 ||
-		eph > _gps_eph_max.get() ||
-		epv > _gps_epv_max.get() ||
-		fix_type < 3
-	) {
-		return -1;
-	}
-
 	// gps measurement
 	y.setZero();
 	y(0) = _sub_gps.get().lat * 1e-7;
@@ -125,11 +124,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();

 	// if field is not below minimum, set it to the value provided
-	if (_sub_gps.get().eph > GPS_EPH_MIN) {
+	if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
 		var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
 	}

-	if (_sub_gps.get().epv > GPS_EPV_MIN) {
+	if (_sub_gps.get().epv > _gps_z_stddev.get()) {
 		var_z = _sub_gps.get().epv * _sub_gps.get().epv;
 	}

@@ -170,13 +169,9 @@ void BlockLocalPositionEstimator::gpsCorrect()

 	if (beta > BETA_TABLE[n_y_gps]) {
 		if (_gpsFault < FAULT_MINOR) {
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
-			//double(r(0)),  double(r(1)), double(r(2)),
-			//double(r(3)), double(r(4)), double(r(5)));
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
-			//double(S_I(0, 0)),  double(S_I(1, 1)), double(S_I(2, 2)),
-			//double(S_I(3, 3)),  double(S_I(4, 4)), double(S_I(5, 5)));
+			mavlink_and_console_log_info(&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)),
+						     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
 			_gpsFault = FAULT_MINOR;
 		}

@@ -188,7 +183,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	// kalman filter correction if no hard fault
 	if (_gpsFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
@@ -109,14 +109,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	if (_lidarFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-
-		if (!_canEstimateXY) {
-			dx(X_x) = 0;
-			dx(X_y) = 0;
-			dx(X_vx) = 0;
-			dx(X_vy) = 0;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -98,7 +98,9 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// kalman filter correction if no fault
 	if (_mocapFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
@@ -127,14 +127,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
 		Matrix<float, n_x, n_y_sonar> K =
 			_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-
-		if (!_canEstimateXY) {
-			dx(X_x) = 0;
-			dx(X_y) = 0;
-			dx(X_vx) = 0;
-			dx(X_vy) = 0;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -96,7 +96,9 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// kalman filter correction if no fault
 	if (_visionFault <  fault_lvl_disable) {
 		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
@@ -2138,6 +2138,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2);
 				log_msg.body.log_EST2.gps_check_fail_flags = buf.estimator_status.gps_check_fail_flags;
 				log_msg.body.log_EST2.control_mode_flags = buf.estimator_status.control_mode_flags;
+				log_msg.body.log_EST2.health_flags = buf.estimator_status.health_flags;
 				LOGBUFFER_WRITE_AND_COUNT(EST2);

 				log_msg.msg_type = LOG_EST3_MSG;
@@ -410,9 +410,10 @@ struct log_EST1_s {
 /* --- EST2 - ESTIMATOR STATUS --- */
 #define LOG_EST2_MSG 34
 struct log_EST2_s {
-    float cov[12];
-    uint16_t gps_check_fail_flags;
-    uint16_t control_mode_flags;
+	float cov[12];
+	uint16_t gps_check_fail_flags;
+	uint16_t control_mode_flags;
+	uint8_t health_flags;
 };

 /* --- EST3 - ESTIMATOR STATUS --- */
@@ -687,7 +688,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT(EST0, "ffffffffffffBBHB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
-	LOG_FORMAT(EST2, "ffffffffffffHH",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL"),
+	LOG_FORMAT(EST2, "ffffffffffffHHB",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL,fHealth"),
 	LOG_FORMAT(EST3, "ffffffffffffffff",    "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
 	LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
 	LOG_FORMAT(EST5, "ffffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV,AirI,AirIV"),