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

gps_alt: extract measurement variance computation and saturation

Browse Source
This commit is contained in:
bresch 2020-12-10 10:16:14 +01:00 committed by Paul Riseborough
parent 1541e4b414
commit 02369cd415
3 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/control.cpp
View File

@ -1103,11 +1103,7 @@ void Ekf::controlHeightFusion()
Vector3f gps_hgt_obs_var;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
// observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
gps_hgt_obs_var(2) = getGpsAltVar();
// innovation gate size
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// fuse height information

3
EKF/ekf.h
View File

@ -851,6 +851,9 @@ private:
void updateBaroHgtOffset();
// return an estimation of the GPS altitude variance
float getGpsAltVar();
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar();

10
EKF/ekf_helper.cpp
View File

@ -1244,6 +1244,16 @@ void Ekf::updateBaroHgtOffset()
}
}
float Ekf::getGpsAltVar()
{
// observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
const float gps_alt_var = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
return gps_alt_var;
}
Vector3f Ekf::getVisionVelocityInEkfFrame() const
{
Vector3f vel;