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

EKF: Set Kaman gains to zero for un-used states

Browse Source 
This is a defensive programming technique. In theory this should not be necessary if the corresponding covariance entries are zero.
This commit is contained in:
Paul Riseborough
2016-02-16 11:08:30 +11:00
parent 69b8982043
commit 1d40507af8
2 changed files with 84 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+62 -13
View File
@@ -284,10 +284,18 @@ void Ekf::fuseMag()
SK_MX[1] + P[20][18] * SK_MX[3]);
Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] *
SK_MX[1] + P[21][18] * SK_MX[3]);
Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
SK_MX[1] + P[22][18] * SK_MX[3]);
Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
SK_MX[1] + P[23][18] * SK_MX[3]);
// Don't update wind states unless we are doing wind estimation
if (_control_status.flags.wind) {
Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
SK_MX[1] + P[22][18] * SK_MX[3]);
Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
SK_MX[1] + P[23][18] * SK_MX[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
} else if (index == 1) {
// Calculate Y axis Kalman gains
@@ -340,10 +348,18 @@ void Ekf::fuseMag()
SK_MY[1] - P[20][16] * SK_MY[3]);
Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] *
SK_MY[1] - P[21][16] * SK_MY[3]);
Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
SK_MY[1] - P[22][16] * SK_MY[3]);
Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
SK_MY[1] - P[23][16] * SK_MY[3]);
// Don't update wind states unless we are doing wind estimation
if (_control_status.flags.wind) {
Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
SK_MY[1] - P[22][16] * SK_MY[3]);
Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
SK_MY[1] - P[23][16] * SK_MY[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
} else if (index == 2) {
// Calculate Z axis Kalman gains
@@ -396,10 +412,18 @@ void Ekf::fuseMag()
SK_MZ[2] - P[20][17] * SK_MZ[3]);
Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] *
SK_MZ[2] - P[21][17] * SK_MZ[3]);
Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
SK_MZ[2] - P[22][17] * SK_MZ[3]);
Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
SK_MZ[2] - P[23][17] * SK_MZ[3]);
// Don't update wind states unless we are doing wind estimation
if (_control_status.flags.wind) {
Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
SK_MZ[2] - P[22][17] * SK_MZ[3]);
Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
SK_MZ[2] - P[23][17] * SK_MZ[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
} else {
return;
@@ -550,7 +574,8 @@ void Ekf::fuseHeading()
// calculate the kalman gains taking dvantage of the reduce size of H_YAW
float Kfusion[_k_num_states] = {};
for (unsigned row = 0; row < _k_num_states; row++) {
// gains for states that are always used
for (unsigned row = 0; row <= 15; row++) {
for (unsigned column = 0; column < 3; column++) {
Kfusion[row] += P[row][column] * H_YAW[column];
}
@@ -558,6 +583,28 @@ void Ekf::fuseHeading()
Kfusion[row] *= innovation_var_inv;
}
// only calculate gains for magnetic field states if they are being used
if (_control_status.flags.mag_3D) {
for (unsigned row = 16; row <= 21; row++) {
for (unsigned column = 0; column < 3; column++) {
Kfusion[row] += P[row][column] * H_YAW[column];
}
Kfusion[row] *= innovation_var_inv;
}
}
// only calculate gains for wind states if they are being used
if (_control_status.flags.wind) {
for (unsigned row = 22; row <= 23; row++) {
for (unsigned column = 0; column < 3; column++) {
Kfusion[row] += P[row][column] * H_YAW[column];
}
Kfusion[row] *= innovation_var_inv;
}
}
// innovation test ratio
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
@@ -680,6 +727,8 @@ void Ekf::fuseDeclination()
Kfusion[13] = -t12 * (P[13][16] * t22 - P[13][17] * t23);
Kfusion[14] = -t12 * (P[14][16] * t22 - P[14][17] * t23);
Kfusion[15] = -t12 * (P[15][16] * t22 - P[15][17] * t23);
// We only do declination fusion when we are using all the field states, so no logic required here
Kfusion[16] = -t12 * (t6 - P[16][17] * t23);
Kfusion[17] = t12 * (t14 - P[17][16] * t22);
Kfusion[18] = -t12 * (P[18][16] * t22 - P[18][17] * t23);
EKF/vel_pos_fusion.cpp
+22 -2
View File
@@ -153,9 +153,29 @@ void Ekf::fuseVelPosHeight()
unsigned state_index = obs_index + 3; // 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 < 24; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
for (int row = 0; row <= 15; 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;
}
}
// by definition the angle error state is zero at the fusion time
_state.ang_error.setZero();