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

EKF: tidy up Kalman gain calculations

Browse Source 
Inhibiting of states is controlled via zeroing rows and columns in the covariance prediction so conditional logic in gain calculations is unnecessary.
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent 311d046a26
commit 82da832816
4 changed files with 31 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+1 -25
View File
@@ -199,34 +199,10 @@ void Ekf::fuseVelPosHeight()
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row <= 15; row++) {
for (int row = 0; row < _k_num_states; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
// only update magnetic field states if we are fusing 3-axis observations
if (_control_status.flags.mag_3D) {
for (int row = 16; row <= 21; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
} else {
for (int row = 16; row <= 21; row++) {
Kfusion[row] = 0.0f;
}
}
// only update wind states if we are doing wind estimation
if (_control_status.flags.wind) {
for (int row = 22; row <= 23; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
} else {
for (int row = 22; row <= 23; row++) {
Kfusion[row] = 0.0f;
}
}
// update the states
fuse(Kfusion, _vel_pos_innov[obs_index]);