EKF2: Zero gyro update #21691

bresch · 2023-06-06T15:23:30Z

Solved Problem

Fusing a zero heading innovation when at rest to estimate the gyro bias has a side effect of making the estimator really confident about its current heading. This is undesirable as we should let it correct itself if there are NE observations (e.g.: GNSS).

Solution

Estimate the gyro bias through a direct bias observation when at rest.

Changelog Entry

For release notes:

Feature/Bugfix Improve heading convergence when no mag is used
New parameter: -
Documentation: -

Alternatives

Test coverage

new unit tests checking for the gyro bias convergence (note that we can't set large IMU biases in the unit tests currently because of the "at rest" detection, we might want to update that logic in the future)

dagar · 2023-06-06T17:19:41Z

src/modules/ekf2/EKF/zero_gyro_update.cpp

+				&& _control_status_prev.flags.vehicle_at_rest;
+
+		if (continuing_conditions_passing) {
+			// TODO: downsample to avoid aliasing


Accumulate a few samples while still?

dagar · 2023-06-06T17:25:27Z

src/modules/ekf2/EKF/zero_gyro_update.cpp

+void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
+{
+	// Fuse zero gyro at a limited rate
+	const bool zero_gyro_update_data_ready = isTimedOut(_time_last_zero_gyro_fuse, (uint64_t)2e5);


I'm just curious if anything particular thought goes into the specific rate limiting timing?

Wouldn't it be fair to simply let this fuse every IMU sample while still? Overkill and expensive, but avoid aliasing and downsampling considerations?

Yes, I don't feel that fusing this at a full rate is really reasonable: the fusion process is quite expensive. So far I didn't do the downsampling because I'm actually not that scared by aliasing in this specific case where it is only active when "at rest". Because we're "at rest" we know that there is almost no noise on the signal (nothing more than the noise of the sensor itself that is smaller than the bias and random enough to not have aliasing issues).

I can try to run it at full speed and see if there is a significant increase in CPU load.

Fusing at full speed increases the CPU load by 2% on fmu-v5, this is not acceptable.

Now the question is: Is aliasing a real concern in this specific case?

2% per EKF2 instance or across multiple?

Now the question is: Is aliasing a real concern in this specific case?

My guess would be mostly not, but I bet if any exceptions slip through it's going to be a fun surprise for the end user.

I'm trying to think of even a contrived case like a nearby cooling fan that's in bad shape, but somehow not enough to disrupt vehicle at reset detection?

Maybe we could maintain and share down averaged IMU with drag fusion?

Maybe we could maintain and share down averaged IMU with drag fusion?

Nevermind, I see you already did it.

2% per EKF2 instance or across multiple?

Per instance.

Nevermind, I see you already did it.

Yes, I figured out that it would take more time and risk to find out if it's worth doing it than just doing it.

dagar · 2023-06-06T17:29:05Z

src/modules/ekf2/EKF/zero_gyro_update.cpp

+		K(row) = P(row, state_index) / innov_var;
+	}
+
+	measurementUpdate(K, innov_var, innov);


Just for my own understanding, why no innovation check? We'd be screwed anyway with that bad gyro?

Yes, an innovation filter means more parameters and we anyway want to always force fuse the data here because we know that if we're at rest, the gyro value is the bias, no matter if the rest of the filter agrees with that or not. We could argue that the innovation filter will be useful to reject outliers, but I don't think it'll be a real problem here.

dagar · 2023-06-06T17:30:31Z

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

-		// 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);
+		float obs_var = 0.25f;


I know it was already like this, but any little comment we could add to justify this particular magic number?

dagar · 2023-06-07T18:26:19Z

src/modules/ekf2/EKF/zero_gyro_update.cpp

+void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
+{
+	// Downsample gyro data to run the fusion at a lower rate
+	_zgup_delta_ang += imu_delayed.delta_ang;


Shouldn't this be structured so that we reset the accumulation when not still?

You could share the reset at the end and do it if zero_gyro_update_data_ready || _control_status.flags.vehicle_at_rest.

Alternatively we could shuffle the logic around a bit more so it's only doing the accumulation work and checks if still, and only does the reset on the edge (at_rest -> !at_rest).

dagar · 2023-06-07T18:27:05Z

EKF2_IMU_CTRL?

dagar · 2023-06-07T20:41:47Z

src/modules/ekf2/EKF/zero_gyro_update.cpp

+				&& _control_status_prev.flags.vehicle_at_rest;
+
+		if (continuing_conditions_passing) {
+			Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * _dt_ekf_avg;


I'm wondering if we should start using the specific dt everywhere appropriate? Maybe something to discuss later after accel/gyro bias states.

Suggested change

Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * _dt_ekf_avg;

Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * imu_delayed.delta_ang_dt;

The filter estimates the delta angle based on _dt_ekf_avg, this is why we need to scale it with that value here. But yes, I generally agree that using angular rate values will reduce complexity.

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.

bresch added Admin: Enhancement (improvement) 💡 EKF2 labels Jun 6, 2023

bresch requested a review from dagar June 6, 2023 15:23

bresch self-assigned this Jun 6, 2023

bresch changed the title ~~Zero gyro update~~ EKF2: Zero gyro update Jun 6, 2023

dagar reviewed Jun 6, 2023

View reviewed changes

dagar reviewed Jun 7, 2023

View reviewed changes

bresch force-pushed the pr-ekf2-zero-gyro-update branch from a57a7af to 625b933 Compare June 8, 2023 13:33

bresch marked this pull request as ready for review June 9, 2023 07:52

bresch added 4 commits June 9, 2023 14:47

ekf: add zero gyro fusion on ground

5d7c5e5

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.

ekf2: update change indicator

14603ef

ekf2: add gyro bias estimation unit tests

817d466

ekf2: zero innov heading up - explain obs_var choice

ec6b3df

bresch force-pushed the pr-ekf2-zero-gyro-update branch from 651c822 to ec6b3df Compare June 9, 2023 12:54

dagar approved these changes Jun 10, 2023

View reviewed changes

dagar merged commit 24de623 into main Jun 10, 2023
83 of 86 checks passed

dagar deleted the pr-ekf2-zero-gyro-update branch June 10, 2023 01:13

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

EKF2: Zero gyro update #21691

EKF2: Zero gyro update #21691

bresch commented Jun 6, 2023 •

edited

dagar Jun 6, 2023

dagar Jun 6, 2023

bresch Jun 7, 2023

bresch Jun 7, 2023

dagar Jun 7, 2023

dagar Jun 7, 2023

bresch Jun 8, 2023

dagar Jun 6, 2023

bresch Jun 7, 2023

dagar Jun 6, 2023

bresch Jun 7, 2023

bresch Jun 9, 2023

dagar Jun 7, 2023

bresch Jun 9, 2023

dagar commented Jun 7, 2023

dagar Jun 7, 2023

bresch Jun 8, 2023

	Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * _dt_ekf_avg;
	Vector3f delta_ang_scaled = _zgup_delta_ang / _zgup_delta_ang_dt * imu_delayed.delta_ang_dt;

EKF2: Zero gyro update #21691

EKF2: Zero gyro update #21691

Conversation

bresch commented Jun 6, 2023 • edited

Solved Problem

Solution

Changelog Entry

Alternatives

Test coverage

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dagar commented Jun 7, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bresch commented Jun 6, 2023 •

edited