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

EKF: tidy up mag declination fusion

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent 0d31aad33a
commit f01f4ae4b1
1 changed files with 9 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+9 -12
View File
@@ -598,14 +598,13 @@ void Ekf::fuseDeclination()
float R_DECL = sq(0.5f); float R_DECL = sq(0.5f);
// Calculate intermediate variables // Calculate intermediate variables
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
if (magN < 0.001f) {
return;
}
float t2 = magE*magE; float t2 = magE*magE;
float t3 = magN*magN; float t3 = magN*magN;
float t4 = t2+t3; float t4 = t2+t3;
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
if (t4 < 1e-4f) {
return;
}
float t5 = P[16][16]*t2; float t5 = P[16][16]*t2;
float t6 = P[17][17]*t3; float t6 = P[17][17]*t3;
float t7 = t2*t2; float t7 = t2*t2;
@@ -622,10 +621,8 @@ void Ekf::fuseDeclination()
} else { } else {
return; return;
} }
float t16 = magE; float t18 = magE*magE;
float t17 = magN; float t19 = magN*magN;
float t18 = t16*t16;
float t19 = t17*t17;
float t20 = t18+t19; float t20 = t18+t19;
float t21; float t21;
if (fabsf(t20) > 1e-6f) { if (fabsf(t20) > 1e-6f) {
@@ -637,8 +634,8 @@ void Ekf::fuseDeclination()
// Calculate the observation Jacobian // Calculate the observation Jacobian
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
float H_DECL[24] = {}; float H_DECL[24] = {};
H_DECL[16] = -t16*t21; H_DECL[16] = -magE*t21;
H_DECL[17] = t17*t21; H_DECL[17] = magN*t21;
// Calculate the Kalman gains // Calculate the Kalman gains
float Kfusion[_k_num_states] = {}; float Kfusion[_k_num_states] = {};
@@ -668,7 +665,7 @@ void Ekf::fuseDeclination()
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
// calculate innovation and constrain // calculate innovation and constrain
float innovation = atanf(magE / magN) - _mag_declination; float innovation = atan2f(magE , magN) - _mag_declination;
innovation = math::constrain(innovation, -0.5f, 0.5f); innovation = math::constrain(innovation, -0.5f, 0.5f);
// perform the state correction // perform the state correction