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

gps sacc: apply same minimum for EKF2 and yaw estimator

Browse Source
This commit is contained in:
bresch 2021-03-04 11:35:12 +01:00 committed by Mathieu Bresciani
parent e3d1ade660
commit 6f2dec726a
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/EKFGSF_yaw.cpp
View File

@ -293,7 +293,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
{
// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f));
const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f));
// calculate velocity observation innovations
_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);

4
EKF/control.cpp
View File

@ -702,7 +702,9 @@ void Ekf::controlGpsFusion()
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
}
_last_vel_obs_var.setAll(sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)));
_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
_last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc));
_last_vel_obs_var(2) *= sq(1.5f);
// calculate innovations