[EKF2] Tilt estimate improvements #24247
Conversation
bresch
force-pushed
the
pr-ekf2_grav_fusion_threshold
branch
from
b1b6810 to
ef78725
Compare
This prevent rapid switching in presence of noise and the innovation filter is good at rejecting spikes
Gravity fusion uses the bias corrected accelerometer data to correct the tilt estimate. We should not continue to estimate the accel bias when this is active as it creates an unwanted feedback loop.
This can destabilize the tilt estimate when the mag field is disturbed
bresch
force-pushed
the
pr-ekf2_grav_fusion_threshold
branch
from
ef78725 to
0a37ca0
Compare
| const bool enable_fake_pos = !enable_valid_fake_pos | ||
| && (getTiltVariance() > sq(math::radians(3.f))) | ||
| && !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) | ||
| && !_control_status.flags.gravity_vector |
There was a problem hiding this comment.
The gravity_vector flag is handled a bit differently where it might be flipping on/off relatively rapidly with the accel threshold. Should we change that behavior so it's more consistent with other aid sources? Alternatively we could check here if gravity fusion hasn't been active for some period of time before finally giving up and starting fake_pos.
There was a problem hiding this comment.
Sorry, completely missed the plot in the PR description about the rapid switching.
Are we completely sure additional protection isn't needed so that we only start fake_pos as a last resort given it will reset pos/vel on start?
There was a problem hiding this comment.
The tilt variance condition (getTiltVariance() > sq(math::radians(3.f))) should make sure the fake position doesn't start immediately when the gravity fusion was healthy and briefly switches off (it should take several seconds to reach 3 degrees of tilt uncertainty).
| const bool accel_lpf_norm_good = (accel_lpf_norm_sq > sq(lower_accel_limit)) | ||
| && (accel_lpf_norm_sq < sq(upper_accel_limit)); |
There was a problem hiding this comment.
is the lpf filter not an issue when we still have vibrations? Would using accel_vibration_metric be of any additional benefit or do you think is good enough like this?
There was a problem hiding this comment.
Good question. Yes, it could still be problematic, but seemed to be ok on the logs I used in replay. Not sure if the vibration metric can help, but we could lower the cutoff frequency of the filter if we see that it's still sensitive. I'll continue to monitor that in the new flights.
3 participants
Solved Problem
Constant small accelerations during gravity fusion creates a small tilt error. As a consequence, the accel bias gets incorrectly estimated and the gravity fusion (using this bias estimate) takes more time to correct back the error.
Solution
Stop accel bias estimation while gravity fusion is active.
Additionally:
Test coverage
EKF replay, unit tests, SITL tests
Before: rapid flag switching
After: incorrect acceleration are rejected by he innovation filter and not the control flag
