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

EKF: Add fusion method to constrain declination when unobservable

Browse Source 
When fusing 3-axis magnetometer data without absolute external aiding leg GPS), the declination is ultimately unobservable and the declination of the  field states and the vehicle heading will slowly drift over time. To prevent this we need to fuse in a declination to constraint the NE earth field estimates.
This commit is contained in:
Paul Riseborough
2016-02-08 14:18:43 +11:00
parent 9347afe250
commit dbb8e12948
3 changed files with 143 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+3 -5
View File
@@ -146,11 +146,9 @@ bool Ekf::update()
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.angle_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
// TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding
// otherwise heading will slowly drift
}
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
fuseHeading();
}
EKF/ekf.h
+2
View File
@@ -162,6 +162,8 @@ private:
void fuseHeading();
void fuseDeclination();
void fuseAirspeed();
void fuseRange();
EKF/mag_fusion.cpp
+138 -7
View File
@@ -536,15 +536,11 @@ void Ekf::fuseHeading()
if (innovation_var >= R_mag) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_x = false;
_fault_status.bad_mag_y = false;
_fault_status.bad_mag_z = false;
_fault_status.bad_mag_hdg = false;
} else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.bad_mag_x = true;
_fault_status.bad_mag_y = true;
_fault_status.bad_mag_z = true;
// 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();
@@ -608,3 +604,138 @@ void Ekf::fuseHeading()
makeSymmetrical();
limitCov();
}
void Ekf::fuseDeclination()
{
// assign intermediate state variables
float magN = _state.mag_I(0);
float magE = _state.mag_I(1);
float R_DECL = sq(0.5f);
// Calculate intermediate variables
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
if (fabsf(magN) < 0.001f) {
return;
}
float t2 = 1.0f/magN;
float t4 = magE*t2;
float t3 = tanf(t4);
float t5 = t3*t3;
float t6 = t5+1.0f;
float t25 = t2*t6;
float t7 = 1.0f/(magN*magN);
float t26 = magE*t6*t7;
float t8 = P[17][17]*t25;
float t15 = P[16][17]*t26;
float t9 = t8-t15;
float t10 = t25*t9;
float t11 = P[17][16]*t25;
float t16 = P[16][16]*t26;
float t12 = t11-t16;
float t17 = t26*t12;
float t13 = R_DECL+t10-t17; // innovation variance
// check the innovation variance calculation for a badly conditioned covariance matrix
if (t13 >= R_DECL) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_decl = false;
} else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.bad_mag_decl = true;
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
float t14 = 1.0f/t13;
float t18 = magE;
float t19 = magN;
float t21 = 1.0f/t19;
float t22 = t18*t21;
float t20 = tanf(t22);
float t23 = t20*t20;
float t24 = t23+1.0f;
// 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
float H_DECL[24] = {};
H_DECL[16] = -t18*1.0f/(t19*t19)*t24;
H_DECL[17] = t21*t24;
// Calculate the Kalman gains
float Kfusion[_k_num_states] = {};
Kfusion[0] = t14*(P[0][17]*t25-P[0][16]*t26);
Kfusion[1] = t14*(P[1][17]*t25-P[1][16]*t26);
Kfusion[2] = t14*(P[2][17]*t25-P[2][16]*t26);
Kfusion[3] = t14*(P[3][17]*t25-P[3][16]*t26);
Kfusion[4] = t14*(P[4][17]*t25-P[4][16]*t26);
Kfusion[5] = t14*(P[5][17]*t25-P[5][16]*t26);
Kfusion[6] = t14*(P[6][17]*t25-P[6][16]*t26);
Kfusion[7] = t14*(P[7][17]*t25-P[7][16]*t26);
Kfusion[8] = t14*(P[8][17]*t25-P[8][16]*t26);
Kfusion[9] = t14*(P[9][17]*t25-P[9][16]*t26);
Kfusion[10] = t14*(P[10][17]*t25-P[10][16]*t26);
Kfusion[11] = t14*(P[11][17]*t25-P[11][16]*t26);
Kfusion[12] = t14*(P[12][17]*t25-P[12][16]*t26);
Kfusion[13] = t14*(P[13][17]*t25-P[13][16]*t26);
Kfusion[14] = t14*(P[14][17]*t25-P[14][16]*t26);
Kfusion[15] = t14*(P[15][17]*t25-P[15][16]*t26);
Kfusion[16] = -t14*(t16-P[16][17]*t25);
Kfusion[17] = t14*(t8-P[17][16]*t26);
Kfusion[18] = t14*(P[18][17]*t25-P[18][16]*t26);
Kfusion[19] = t14*(P[19][17]*t25-P[19][16]*t26);
Kfusion[20] = t14*(P[20][17]*t25-P[20][16]*t26);
Kfusion[21] = t14*(P[21][17]*t25-P[21][16]*t26);
Kfusion[22] = t14*(P[22][17]*t25-P[22][16]*t26);
Kfusion[23] = t14*(P[23][17]*t25-P[23][16]*t26);
// calculate innovation and constrain
float innovation = atanf(t4) - math::radians(_params.mag_declination_deg);
innovation = math::constrain(innovation, -0.5f, 0.5f);
// zero attitude error states and perform the state correction
_state.ang_error.setZero();
fuse(Kfusion, innovation);
// use the attitude error estimate to correct the quaternion
Quaternion dq;
dq.from_axis_angle(_state.ang_error);
_state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal.normalize();
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
// take advantage of the empty columns in KH to reduce the number of operations
float KH[_k_num_states][_k_num_states] = {};
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 16; column < 17; column++) {
KH[row][column] = Kfusion[row] * H_DECL[column];
}
}
float KHP[_k_num_states][_k_num_states] = {};
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][0] * P[0][column];
tmp += KH[row][16] * P[16][column];
tmp += KH[row][17] * P[17][column];
KHP[row][column] = tmp;
}
}
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P[row][column] -= KHP[row][column];
}
}
// force the covariance matrix to be symmetrical and don't allow the variances to be negative.
makeSymmetrical();
limitCov();
}