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

EKF: Improve efficiency of yaw angle fusion

Browse Source 
Use direct calculation of Kaman gains with optimised algebra
This commit is contained in:
Paul Riseborough
2016-02-19 16:25:01 +11:00
parent 7f121e81e4
commit 7d6226eb45
1 changed files with 82 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+82 -69
View File
@@ -495,7 +495,7 @@ void Ekf::fuseHeading()
float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
R_YAW = R_YAW * R_YAW;
// calculate intermediate variables for observation jacobian
// calculate intermediate variables for observation jacobians
float t2 = q0 * q0;
float t3 = q1 * q1;
float t4 = q2 * q2;
@@ -509,7 +509,7 @@ void Ekf::fuseHeading()
if (fabsf(t6) > 1e-6f) {
t10 = 1.0f / (t6 * t6);
} else {
} else {
return;
}
@@ -527,10 +527,85 @@ void Ekf::fuseHeading()
float t15 = 1.0f / t6;
// calculate observation jacobian
float H_YAW[3] = {};
H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f));
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8)); // calculate observation jacobian
// calculate Kalman gains
float t16 = q0 * q1 * 2.0f;
float t29 = q2 * q3 * 2.0f;
float t17 = t16 - t29;
float t18 = t15 * t17;
float t19 = q0 * q2 * 2.0f;
float t20 = q1 * q3 * 2.0f;
float t21 = t19 + t20;
float t22 = t9 * t10 * t21;
float t23 = t18 + t22;
float t40 = t14 * t23;
float t24 = t2 - t3 + t4 - t5;
float t25 = t15 * t24;
float t26 = t7 - t8;
float t27 = t9 * t10 * t26;
float t28 = t25 + t27;
float t41 = t14 * t28;
float t30 = P[1][1] * t40;
float t31 = P[1][2] * t40;
float t32 = P[2][2] * t41;
float t33 = t31 + t32;
float t34 = t41 * t33;
float t35 = P[2][1] * t41;
float t36 = t30 + t35;
float t37 = t40 * t36;
float t38 = R_YAW + t34 + t37; // Innovation variance
float t39;
if (t38 >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_hdg = false;
t39 = 1.0f / t38;
_heading_innov_var = t38;
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.bad_mag_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
float Kfusion[24] = {};
Kfusion[0] = t39 * (P[0][1] * t40 + P[0][2] * t41);
Kfusion[1] = t39 * (t30 + P[1][2] * t41);
Kfusion[2] = t39 * (t32 + P[2][1] * t40);
Kfusion[3] = t39 * (P[3][1] * t40 + P[3][2] * t41);
Kfusion[4] = t39 * (P[4][1] * t40 + P[4][2] * t41);
Kfusion[5] = t39 * (P[5][1] * t40 + P[5][2] * t41);
Kfusion[6] = t39 * (P[6][1] * t40 + P[6][2] * t41);
Kfusion[7] = t39 * (P[7][1] * t40 + P[7][2] * t41);
Kfusion[8] = t39 * (P[8][1] * t40 + P[8][2] * t41);
Kfusion[9] = t39 * (P[9][1] * t40 + P[9][2] * t41);
Kfusion[10] = t39 * (P[10][1] * t40 + P[10][2] * t41);
Kfusion[11] = t39 * (P[11][1] * t40 + P[11][2] * t41);
Kfusion[12] = t39 * (P[12][1] * t40 + P[12][2] * t41);
Kfusion[13] = t39 * (P[13][1] * t40 + P[13][2] * t41);
Kfusion[14] = t39 * (P[14][1] * t40 + P[14][2] * t41);
Kfusion[15] = t39 * (P[15][1] * t40 + P[15][2] * t41);
/* we won't be using these states because we are doing heading fusion
Kfusion[16] = t39*(P[16][1]*t40+P[16][2]*t41);
Kfusion[17] = t39*(P[17][1]*t40+P[17][2]*t41);
Kfusion[18] = t39*(P[18][1]*t40+P[18][2]*t41);
Kfusion[19] = t39*(P[19][1]*t40+P[19][2]*t41);
Kfusion[20] = t39*(P[20][1]*t40+P[20][2]*t41);
Kfusion[21] = t39*(P[21][1]*t40+P[21][2]*t41);
*/
// don't adjust these states if we are not using them
if (_control_status.flags.wind) {
Kfusion[22] = t39 * (P[22][1] * t40 + P[22][2] * t41);
Kfusion[23] = t39 * (P[23][1] * t40 + P[23][2] * t41);
}
// rotate body field magnetic field measurement into earth frame and compare horizontal vector with published declination to get yaw measurement
// TODO - enable use of an off-board yaw measurement
@@ -546,70 +621,8 @@ void Ekf::fuseHeading()
innovation = matrix::wrap_pi(innovation);
_heading_innov = innovation;
// calculate innovation variance
float innovation_var = R_YAW;
_heading_innov_var = innovation_var;
float PH[3] = {};
for (unsigned row = 0; row < 3; row++) {
for (unsigned column = 0; column < 3; column++) {
PH[row] += P[row][column] * H_YAW[column];
}
innovation_var += H_YAW[row] * PH[row];
}
if (innovation_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_hdg = false;
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.bad_mag_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
float innovation_var_inv = 1.0f / innovation_var;
// calculate the kalman gains taking dvantage of the reduce size of H_YAW
float Kfusion[_k_num_states] = {};
// 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];
}
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);
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
@@ -623,7 +636,7 @@ void Ekf::fuseHeading()
} else {
// constrain the innovation to the maximum set by the gate
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var));
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var));
innovation = math::constrain(innovation, -gate_limit, gate_limit);
}
@@ -645,7 +658,7 @@ void Ekf::fuseHeading()
float HP[_k_num_states] = {};
for (unsigned column = 0; column < _k_num_states; column++) {
for (unsigned row = 0; row < 3; row++) {
for (unsigned row = 1; row <= 2; row++) {
HP[column] += H_YAW[row] * P[row][column];
}
}