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

EKF: fix pos/vel fusion innovation variance bug

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-08 17:51:47 +10:00
parent 2144a40a3c
commit 00973d6215
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -153,7 +153,7 @@ void Ekf::fuseVelPosHeight()
for (unsigned obs_index = 0; obs_index < 6; obs_index++) { for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (fuse_map[obs_index]) { if (fuse_map[obs_index]) {
// compute the innovation variance SK = HPH + R // compute the innovation variance SK = HPH + R
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index]; _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
// Compute the ratio of innovation to gate size // Compute the ratio of innovation to gate size
_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) *