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

Enable vision velocity fusion

Browse Source
This commit is contained in:
kamilritz
2019-09-24 07:48:46 +10:00
committed by Paul Riseborough
parent 149233a9ab
commit 3e8139ff9f
5 changed files with 87 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+5 -5
View File
@@ -67,8 +67,8 @@ void Ekf::fuseVelPosHeight()
}
// Set observation noise variance and innovation consistency check gate size for the NE position observations
R[0] = _velObsVarNE(0);
R[1] = _velObsVarNE(1);
R[0] = _velObsVarNED(0);
R[1] = _velObsVarNED(1);
gate_size[1] = gate_size[0] = _hvelInnovGate;
}
@@ -76,12 +76,12 @@ void Ekf::fuseVelPosHeight()
if (_fuse_vert_vel) {
fuse_map[2] = true;
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
R[2] = _velObsVarNED(2);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc);
R[2] = R[2] * R[2];
// innovation gate size
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
gate_size[2] = _vvelInnovGate;
}
if (_fuse_pos) {
@@ -152,7 +152,7 @@ void Ekf::fuseVelPosHeight()
R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f);
gate_size[5] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
}
// update innovation class variable for logging purposes