fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 06:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: inhibit accel bias dynamically when fake position/height fusion active

Browse Source 
- inhibit accel bias learning dynamically depending on the angle between an accelerometer and the vertical axis when fake position fusion is active
This commit is contained in:
Daniel Agar
2022-09-22 20:32:20 -04:00
committed by GitHub
parent 1806f0d2b4
commit 38d332a553
4 changed files with 748 additions and 735 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+14 -1
View File
@@ -149,7 +149,20 @@ void Ekf::predictCovariance()
for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) {
const unsigned index = stateIndex - 13;
const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index];
bool is_bias_observable = true;
if (_control_status.flags.vehicle_at_rest) {
is_bias_observable = true;
} else if (_control_status.flags.fake_hgt) {
is_bias_observable = false;
} else if (_control_status.flags.fake_pos) {
// when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later