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

EKF: Combine GPS velocity innovation gate parameters

Browse Source 
Separate vertical and horizontal parameters for GPS velocity innovation gates are not required
This commit is contained in:
Paul Riseborough 2016-01-30 22:46:30 +11:00 committed by bugobliterator
parent 466a104534
commit f1b82057c0
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
EKF/estimator_base.h
View File

@ -134,10 +134,9 @@ namespace estimator {
float gps_vel_noise = 0.05f;
float gps_pos_noise = 1.0f;
float baro_noise = 0.1f;
float baro_innov_gate = 5.0f; // barometer fusion innovation consistency gate size in standard deviations
float velD_innov_gate = 5.0f; // Vertical velocity fusion innovation consistency gate size in standard deviations
float posNE_innov_gate = 5.0f; // Horizontal position innovation consistency gate size in standard deviations
float velNE_innov_gate = 5.0f; // Horizontal velocity fusion innovation consistency gate size in standard deviations
float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations
float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
float mag_declination_deg = 0.0f; // magnetic declination in degrees

4
EKF/vel_pos_fusion.cpp
View File

@ -56,7 +56,7 @@ void Ekf::fuseVelPosHeight()
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
R[0] = _params.gps_vel_noise;
R[1] = _params.gps_vel_noise;
gate_size[0] = fmaxf(_params.velNE_innov_gate, 1.0f);
gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
gate_size[1] = gate_size[0];
}
@ -64,7 +64,7 @@ void Ekf::fuseVelPosHeight()
fuse_map[2] = true;
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
R[2] = _params.gps_vel_noise;
gate_size[2] = fmaxf(_params.velD_innov_gate, 1.0f);
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
}
if (_fuse_pos) {