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

EKF: Update tuning parameters

Browse Source 
Set conservative defaults as a baseline for tuning
Add a missing parameter for magnetometer observation noise.
Correct error in definition of magnetic heading observations noise (previous parameter defined the variance directly, not the noise).
Scale vertical GPS noise to allow for average EPV to EPH ratio.
Update documentation and display names for consistency.
This commit is contained in:
Paul Riseborough
2016-02-03 17:05:45 +11:00
parent b21a49b3f9
commit 2b1e8fe910
5 changed files with 39 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+6 -6
View File
@@ -54,8 +54,8 @@ void Ekf::fuseVelPosHeight()
fuse_map[0] = fuse_map[1] = true;
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
R[0] = _params.gps_vel_noise;
R[1] = _params.gps_vel_noise;
R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
R[1] = R[0];
gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
gate_size[1] = gate_size[0];
}
@@ -63,7 +63,7 @@ void Ekf::fuseVelPosHeight()
if (_fuse_vert_vel) {
fuse_map[2] = true;
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
R[2] = _params.gps_vel_noise;
R[2] = 1.5f * fmaxf(_params.gps_vel_noise, 0.01f);
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
}
@@ -71,8 +71,8 @@ void Ekf::fuseVelPosHeight()
fuse_map[3] = fuse_map[4] = true;
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
R[3] = _params.gps_pos_noise;
R[4] = _params.gps_pos_noise;
R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
R[4] = R[3];
gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
gate_size[4] = gate_size[3];
}
@@ -80,7 +80,7 @@ void Ekf::fuseVelPosHeight()
if (_fuse_height) {
fuse_map[5] = true;
_vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis
R[5] = _params.baro_noise;
R[5] = fmaxf(_params.baro_noise, 0.01f);
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
}