dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Make PR comply with project convention for indenting

This commit is contained in:
Paul Riseborough 2016-02-08 15:12:38 +11:00
parent cdc42c1de0
commit 47ab5ebcdd
5 changed files with 148 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/control.cpp
View File

@ -111,8 +111,9 @@ void Ekf::controlFusionModes()
}
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination needs to be fused as an observation to prevent long term heading drift
if(_control_status.flags.mag_3D && _control_status.flags.gps) {
if (_control_status.flags.mag_3D && _control_status.flags.gps) {
_control_status.flags.mag_dec = false;
} else {
_control_status.flags.mag_dec = true;
}

2
EKF/ekf.cpp
View File

@ -146,9 +146,11 @@ 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) {
fuseDeclination();
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
fuseHeading();
}

95
EKF/mag_fusion.cpp
View File

@ -618,23 +618,24 @@ void Ekf::fuseDeclination()
if (fabsf(magN) < 0.001f) {
return;
}
float t2 = 1.0f/magN;
float t4 = magE*t2;
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
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) {
@ -650,47 +651,47 @@ void Ekf::fuseDeclination()
return;
}
float t14 = 1.0f/t13;
float t14 = 1.0f / t13;
float t18 = magE;
float t19 = magN;
float t21 = 1.0f/t19;
float t22 = t18*t21;
float t21 = 1.0f / t19;
float t22 = t18 * t21;
float t20 = tanf(t22);
float t23 = t20*t20;
float t24 = t23+1.0f;
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;
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);
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);