fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 23:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Initialise height state variance to a value compatible with the measurement

Browse Source 
A large height state variance and small measurement variance can destabilise the filter in the first few seconds after alignment
This commit is contained in:
Paul Riseborough
2016-05-03 18:47:29 +10:00
parent 29a361d3a3
commit bc34b14779
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+9 -1
View File
@@ -69,7 +69,15 @@ void Ekf::initialiseCovariance()
// position
P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P[8][8] = P[7][7];
P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
if (_control_status.flags.rng_hgt) {
P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
} else if (_control_status.flags.gps_hgt) {
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
} else {
P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
}
// gyro bias
P[10][10] = sq(0.1f * dt);