zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Fix bug in velocity and position innovation consistency checks

master
Paul Riseborough 9 years ago
parent
f9e3db0504
commit
569886a4fc
1 changed files with 1 additions and 1 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      EKF/vel_pos_fusion.cpp

2
EKF/vel_pos_fusion.cpp
Unescape View File

@ -122,7 +122,7 @@ void Ekf::fuseVelPosHeight()
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state unsigned state_index = obs_index + 3; // 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_innov[obs_index]); _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov_var[obs_index]);
} }
} }

Write Preview
Loading…
Cancel
Save