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

EKF: Reduce EKF-GSF vulnerability to large yaw gyro bias errors (#831)

Browse Source 
* EKF: Reduce EKF-GSF vulnerability to large yaw gyro bias errors

* Update EKF/ekf_helper.cpp

* EKF: Fix comment typo

Co-authored-by: Roman Bapst <bapstroman@gmail.com>

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>
This commit is contained in:
Paul Riseborough
2020-06-02 17:42:54 +10:00
committed by GitHub
co-authored by Roman Bapst Mathieu Bresciani
parent d0ddda8917
commit 59183f70ba
3 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+8 -2
View File
@@ -14,8 +14,9 @@ EKFGSF_yaw::EKFGSF_yaw()
}
void EKFGSF_yaw::update(const imuSample& imu_sample,
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed) // true airspeed used for centripetal accel compensation - set to 0 when not required.
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required.
const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec)
{
// copy to class variables
_delta_ang = imu_sample.delta_ang;
@@ -60,6 +61,11 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
if (!_ekf_gsf_vel_fuse_started) {
initialiseEKFGSF();
ahrsAlignYaw();
// Initialise to gyro bias estimate from main filter because there could be a large
// uncorrected rate gyro bias error about the gravity vector
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
}
_ekf_gsf_vel_fuse_started = true;
} else {
bool bad_update = false;