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

EKF: Add support auxiliary velocity observation

Browse Source 
This enables the EKF to use an additional NE velocity measurement. This can be used to improve position hold stability when landing using a beacon system for positioning by fusing the beacon velocity estimates.
This commit is contained in:
Paul Riseborough
2017-11-21 11:18:09 +11:00
parent bd8f30752a
commit 8e30c2666d
6 changed files with 90 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+6 -11
View File
@@ -54,18 +54,13 @@ void Ekf::fuseVelPosHeight()
// calculate innovations, innovations gate sizes and observation variances
if (_fuse_hor_vel) {
// enable fusion for NE velocity axes
fuse_map[0] = fuse_map[1] = true;
// horizontal velocity innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
R[0] = fmaxf(R[0], _gps_sample_delayed.sacc);
R[0] = R[0] * R[0];
R[1] = R[0];
// innovation gate sizes
gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
gate_size[1] = gate_size[0];
// Set observation noise variance and innovation consistency check gate size for the NE position observations
R[0] = _velObsVarNE(0);
R[1] = _velObsVarNE(1);
gate_size[1] = gate_size[0] = _hvelInnovGate;
}
if (_fuse_vert_vel) {