EKF2: Zero gyro update (#21691)

When at rest, directly fuse the gyro data as an observation of its bias.
This allows to strongly observe the gyro biases without having to fuse a
constant heading that makes the ekf too confident about its heading.

src/modules/ekf2/EKF/CMakeLists.txt

@@ -54,6 +54,7 @@ list(APPEND EKF_SRCS
 	output_predictor.cpp
 	vel_pos_fusion.cpp
 	zero_innovation_heading_update.cpp
+	zero_gyro_update.cpp
 	zero_velocity_update.cpp
 )
 

src/modules/ekf2/EKF/control.cpp

@@ -139,6 +139,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlZeroInnovationHeadingUpdate();
 
 	controlZeroVelocityUpdate();
+	controlZeroGyroUpdate(imu_delayed);
 
 	// Fake position measurement for constraining drift when no other velocity or position measurements
 	controlFakePosFusion();

src/modules/ekf2/EKF/ekf.h

@@ -567,6 +567,10 @@ private:
 
 	Dcmf _R_to_earth{};	///< transformation matrix from body frame to earth frame from last EKF prediction
 
+	// zero gyro update
+	Vector3f _zgup_delta_ang{};
+	float _zgup_delta_ang_dt{0.f};
+
 	// used by magnetometer fusion mode selection
 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
@@ -575,7 +579,6 @@ private:
 	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
 	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
 	uint64_t _mag_use_not_inhibit_us{0};	///< last system time in usec before magnetometer use was inhibited
-	float _last_static_yaw{NAN};		///< last yaw angle recorded when on ground motion checks were passing (rad)
 
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
@@ -1046,6 +1049,8 @@ private:
 	void stopFakeHgtFusion();
 
 	void controlZeroVelocityUpdate();
+	void controlZeroGyroUpdate(const imuSample &imu_delayed);
+	void fuseDeltaAngBias(float innov, float innov_var, int obs_index);
 
 	void controlZeroInnovationHeadingUpdate();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1155,8 +1155,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	_state_reset_status.reset_count.quat++;
 
 	_time_last_heading_fuse = _time_delayed_us;
-
-	_last_static_yaw = NAN;
 }
 
 // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

src/modules/ekf2/EKF/mag_control.cpp

@@ -150,8 +150,6 @@ void Ekf::controlMagFusion()
 
 					_aid_src_mag_heading.time_last_fuse = _time_delayed_us;
 					_time_last_heading_fuse = _time_delayed_us;
-
-					_last_static_yaw = NAN;
 				}
 			}
 		}

src/modules/ekf2/EKF/zero_gyro_update.cpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file zero_gyro_update.cpp
+ * Control function for ekf zero gyro update
+ */
+
+#include "ekf.h"
+
+void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
+{
+	if (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias))) {
+		return;
+	}
+
+	// When at rest, fuse the gyro data as a direct observation of the gyro bias
+	if (_control_status.flags.vehicle_at_rest) {
+		// Downsample gyro data to run the fusion at a lower rate
+		_zgup_delta_ang += imu_delayed.delta_ang;
+		_zgup_delta_ang_dt += imu_delayed.delta_ang_dt;
+
+		static constexpr float zgup_dt = 0.2f;
+		const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt;
+
+		if (zero_gyro_update_data_ready) {
+			Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * _dt_ekf_avg;
+			Vector3f innovation = _state.delta_ang_bias - delta_ang_scaled;
+
+			const float d_ang_sig = _dt_ekf_avg * math::constrain(_params.gyro_noise, 0.0f, 1.0f);
+			//const float obs_var = sq(d_ang_sig) * (_dt_ekf_avg / _zgup_delta_ang_dt); // This is correct but too small for single precision
+			const float obs_var = sq(d_ang_sig);
+
+			Vector3f innov_var{
+				P(10, 10) + obs_var,
+				P(11, 11) + obs_var,
+				P(12, 12) + obs_var};
+
+			for (int i = 0; i < 3; i++) {
+				fuseDeltaAngBias(innovation(i), innov_var(i), i);
+			}
+
+			// Reset the integrators
+			_zgup_delta_ang.setZero();
+			_zgup_delta_ang_dt = 0.f;
+		}
+
+	} else if (_control_status_prev.flags.vehicle_at_rest) {
+		// Reset the integrators
+		_zgup_delta_ang.setZero();
+		_zgup_delta_ang_dt = 0.f;
+	}
+}
+
+void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index)
+{
+	Vector24f K;  // Kalman gain vector for any single observation - sequential fusion is used.
+	const unsigned state_index = obs_index + 10;
+
+	// calculate kalman gain K = PHS, where S = 1/innovation variance
+	for (int row = 0; row < _k_num_states; row++) {
+		K(row) = P(row, state_index) / innov_var;
+	}
+
+	measurementUpdate(K, innov_var, innov);
+}

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -43,52 +43,23 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 	const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D
 				|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
 
-	if (!_control_status.flags.tilt_align) {
-		// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
-		float innovation = 0.f;
-		float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
-		estimator_aid_source1d_s unused;
-		fuseYaw(innovation, obs_var, unused);
-		_time_last_heading_fuse = 0;
-		_last_static_yaw = NAN;
+	// fuse zero innovation at a limited rate if the yaw variance is too large
+	if (_control_status.flags.tilt_align
+	    && !yaw_aiding
+	    && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
 
-	} else if (_control_status.flags.vehicle_at_rest) {
-		// When at rest or no source of yaw aiding is active yaw fusion is run selectively to enable yaw gyro
-		// bias learning when stationary on ground and to prevent uncontrolled yaw variance growth
-		const float euler_yaw = getEulerYaw(_R_to_earth);
+		// Use an observation variance larger than usual but small enough
+		// to constrain the yaw variance just below the threshold
+		float obs_var = 0.25f;
+		estimator_aid_source1d_s aid_src_status;
+		Vector24f H_YAW;
 
-		if (PX4_ISFINITE(_last_static_yaw)) {
-			// fuse last static yaw at a limited rate (every 200 milliseconds)
-			if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
-				float innovation = wrap_pi(euler_yaw - _last_static_yaw);
-				float obs_var = 0.01f;
-				estimator_aid_source1d_s unused;
-				fuseYaw(innovation, obs_var, unused);
-			}
+		computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
 
-		} else {
-			// record static yaw when transitioning to at rest
-			_last_static_yaw = euler_yaw;
+		if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
+			// The yaw variance is too large, fuse fake measurement
+			float innovation = 0.f;
+			fuseYaw(innovation, obs_var, aid_src_status, H_YAW);
 		}
-
-	} else {
-		// vehicle moving and tilt alignment completed
-
-		// fuse zero innovation at a limited rate if the yaw variance is too large
-		if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
-			float obs_var = 0.25f;
-			estimator_aid_source1d_s aid_src_status;
-			Vector24f H_YAW;
-
-			computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
-
-			if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
-				// The yaw variance is too large, fuse fake measurement
-				float innovation = 0.f;
-				fuseYaw(innovation, obs_var, aid_src_status, H_YAW);
-			}
-		}
-
-		_last_static_yaw = NAN;
 	}
 }