ekf2: gravity fusion updates

- only fuse gravity if no horizontal aiding source is enabled
 - add estimator_aid_src_gravity for inspection and logging
 - estimator_aid_src_gravity is always updated and published, even if not fused
 - add innovation gate with conservative default (could be made configurable)
 - use new EKF2_IMU_CTRL parameter to enable/disable gravity vector fusion

msg/EstimatorAidSource3d.msg

@@ -18,6 +18,4 @@ bool fusion_enabled          # true when measurements are being fused
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
-# TOPICS estimator_aid_src_ev_vel
-# TOPICS estimator_aid_src_gnss_vel
-# TOPICS estimator_aid_src_mag
+# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag

msg/EstimatorStatusFlags.msg

@@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on win
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
 bool cs_fake_pos                # 32 - true when fake position measurements are being fused
 bool cs_fake_hgt                # 33 - true when fake height measurements are being fused
+bool cs_gravity_vector          # 34 - true when gravity vector measurements are being fused
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/EKF/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(ecl_EKF
 	gps_checks.cpp
 	gps_control.cpp
 	gps_yaw_fusion.cpp
+	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
 	mag_control.cpp

src/modules/ekf2/EKF/common.h

@@ -127,8 +127,9 @@ enum class PositionSensor : uint8_t {
 };
 
 enum class ImuCtrl : uint8_t {
-	GyroBias  = (1<<0),
-	AccelBias = (1<<1),
+	GyroBias      = (1<<0),
+	AccelBias     = (1<<1),
+	GravityVector = (1<<2),
 };
 
 enum GnssCtrl : uint8_t {
@@ -394,7 +395,7 @@ struct parameters {
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
 
 	// gravity fusion
-	float gravity_noise{1.0f};		///< accelerometer measurement gaussian noise (m/s**2)
+	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)
 
 	// optical flow fusion
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
@@ -567,6 +568,7 @@ union filter_control_status_u {
 		uint64_t rng_kin_consistent      : 1; ///< 31 - true when the range finder kinematic consistency check is passing
 		uint64_t fake_pos                : 1; ///< 32 - true when fake position measurements are being fused
 		uint64_t fake_hgt                : 1; ///< 33 - true when fake height measurements are being fused
+		uint64_t gravity_vector          : 1; ///< 34 - true when gravity vector measurements are being fused
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -198,7 +198,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlBetaFusion(imu_delayed);
 	controlDragFusion();
 	controlHeightFusion(imu_delayed);
-	controlGravityFusion();
+	controlGravityFusion(imu_delayed);
 
 	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
@@ -429,12 +429,3 @@ void Ekf::controlAuxVelFusion()
 		}
 	}
 }
-
-void Ekf::controlGravityFusion()
-{
-	// fuse gravity observation if our overall acceleration isn't too big
-	const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G;
-	if (gravity_scale >= 0.9f && gravity_scale <= 1.1f) {
-		fuseGravity();
-	}
-}

src/modules/ekf2/EKF/ekf.h

@@ -191,7 +191,9 @@ public:
 	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
 	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
 
-	void getGravInnov(float grav_innov[3]) const { _gravity_innov.copyTo(grav_innov); }
+	void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
+	void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
+	void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
 
 	// get the state vector at the delayed time horizon
 	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
@@ -435,6 +437,8 @@ public:
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }
 
+	const auto &aid_src_gravity() const { return _aid_src_gravity; }
+
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
@@ -543,8 +547,6 @@ private:
 	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
 
-	Vector3f _gravity_innov{};	///< innovation of the last gravity fusion observation (m/s**2)
-
 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
 	Vector2f _flow_vel_body{};	///< velocity from corrected flow measurement (body frame)(m/s)
@@ -577,6 +579,8 @@ private:
 	estimator_aid_source1d_s _aid_src_mag_heading{};
 	estimator_aid_source3d_s _aid_src_mag{};
 
+	estimator_aid_source3d_s _aid_src_gravity{};
+
 	estimator_aid_source2d_s _aid_src_aux_vel{};
 
 	estimator_aid_source2d_s _aid_src_optical_flow{};
@@ -931,10 +935,8 @@ private:
 
 	void updateGroundEffect();
 
-	// gravity fusion
-	void fuseGravity();
-	// heuristically enable / disable gravity fusion
-	void controlGravityFusion();
+	// gravity fusion: heuristically enable / disable gravity fusion
+	void controlGravityFusion(const imuSample &imu_delayed);
 
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar(const flowSample &flow_sample);

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +35,7 @@
  * @file gravity_fusion.cpp
  * Fuse observations from the gravity vector to constrain roll
  * and pitch (a la complementary filter).
- * 
+ *
  * @author Daniel M. Sahu <danielmohansahu@gmail.com>
  */
 
@@ -44,30 +44,52 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::fuseGravity()
+void Ekf::controlGravityFusion(const imuSample &imu)
 {
-	// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
-	const imuSample imu = get_imu_sample_delayed();
-	const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias();
-	const float acc_measurement_noise = sq(_params.gravity_noise);
+	// fuse gravity observation if our overall acceleration isn't too big
+	const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G;
 
-	// initialize fusion variables
-	Vector24f Kx, Ky, Kz; // Kalman gain vectors
-	Vector3f innovation_variance;
+	_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
+					       && (((gravity_scale >= 0.9f && gravity_scale <= 1.1f)) || _control_status.flags.vehicle_at_rest)
+					       && !isHorizontalAidingActive();
+
+	// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
+	const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias();
+	const float measurement_var = sq(_params.gravity_noise);
 
 	// calculate kalman gains and innovation variances
+	Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2)
+	Vector3f innovation_variance;
+	Vector24f Kx, Ky, Kz; // Kalman gain vectors
 	sym::ComputeGravityInnovVarAndKAndH(
-		getStateAtFusionHorizonAsVector(), P, measurement, acc_measurement_noise, FLT_EPSILON,
-		&_gravity_innov, &innovation_variance, &Kx, &Ky, &Kz);
+		getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON,
+		&innovation, &innovation_variance, &Kx, &Ky, &Kz);
 
-	// perform fusion for each axis
-	if (!measurementUpdate(Kx, innovation_variance(0), _gravity_innov(0))) {
-		PX4_ERR("Gravity fusion (X axis) unsuccessful.");
+	// fill estimator aid source status
+	resetEstimatorAidStatus(_aid_src_gravity);
+	_aid_src_gravity.timestamp_sample = imu.time_us;
+	measurement.copyTo(_aid_src_gravity.observation);
+
+	for (auto &var : _aid_src_gravity.observation_variance) {
+		var = measurement_var;
 	}
-	if (!measurementUpdate(Ky, innovation_variance(1), _gravity_innov(1))) {
-		PX4_ERR("Gravity fusion (y axis) unsuccessful.");
-	}
-	if (!measurementUpdate(Kz, innovation_variance(2), _gravity_innov(2))) {
-		PX4_ERR("Gravity fusion (Z axis) unsuccessful.");
+
+	innovation.copyTo(_aid_src_gravity.innovation);
+	innovation_variance.copyTo(_aid_src_gravity.innovation_variance);
+
+	float innovation_gate = 1.f;
+	setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
+
+	_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
+
+	if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) {
+		// perform fusion for each axis
+		_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
+					 && measurementUpdate(Ky, innovation_variance(1), innovation(1))
+					 && measurementUpdate(Kz, innovation_variance(2), innovation(2));
+
+		if (_aid_src_gravity.fused) {
+			_aid_src_gravity.time_last_fuse = imu.time_us;
+		}
 	}
 }

src/modules/ekf2/EKF2.cpp

@@ -805,6 +805,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	// mag 3d
 	PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
 
+	// gravity
+	PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
+
 	// aux velocity
 	PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
 
@@ -1099,7 +1102,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getHaglInnov(innovations.hagl);
 	_ekf.getHaglRateInnov(innovations.hagl_rate);
 	_ekf.getTerrainFlowInnov(innovations.terr_flow);
-	_ekf.getGravInnov(innovations.gravity);
+	_ekf.getGravityInnov(innovations.gravity);
 	// Not yet supported
 	innovations.aux_vvel = NAN;
 
@@ -1150,6 +1153,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getHaglInnovRatio(test_ratios.hagl);
 	_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
 	_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
+	_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
 	// Not yet supported
 	test_ratios.aux_vvel = NAN;
 
@@ -1176,6 +1180,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getHaglInnovVar(variances.hagl);
 	_ekf.getHaglRateInnovVar(variances.hagl_rate);
 	_ekf.getTerrainFlowInnovVar(variances.terr_flow);
+	_ekf.getGravityInnovVar(variances.gravity);
 	// Not yet supported
 	variances.aux_vvel = NAN;
 
@@ -1509,6 +1514,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
 		status_flags.cs_fake_pos                = _ekf.control_status_flags().fake_pos;
 		status_flags.cs_fake_hgt                = _ekf.control_status_flags().fake_hgt;
+		status_flags.cs_gravity_vector          = _ekf.control_status_flags().gravity_vector;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/EKF2.hpp

@@ -285,6 +285,8 @@ private:
 	hrt_abstime _status_mag_pub_last{0};
 	hrt_abstime _status_mag_heading_pub_last{0};
 
+	hrt_abstime _status_gravity_pub_last{0};
+
 	hrt_abstime _status_aux_vel_pub_last{0};
 
 	hrt_abstime _status_optical_flow_pub_last{0};
@@ -376,6 +378,8 @@ private:
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
 	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
 
+	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
+
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
 
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};

src/modules/ekf2/ekf2_params.c

@@ -57,11 +57,12 @@ PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000);
  *
  * @group EKF2
  * @min 0
- * @max 3
+ * @max 7
  * @bit 0 Gyro Bias
  * @bit 1 Accel Bias
+ * @bit 2 Gravity vector fusion
  */
-PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3);
+PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 7);
 
 /**
  * Magnetometer measurement delay relative to IMU measurements

src/modules/logger/logged_topics.cpp

@@ -187,6 +187,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES);