From 7da40a45a4356d488c681784f6ed9985873d1de0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 10:04:37 +1100 Subject: [PATCH 01/10] EKF: Add control mode for wind state estimation --- EKF/common.h | 1 + EKF/control.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 2e9b9f77ef..9cf70fef8c 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -220,6 +220,7 @@ union filter_control_status_u { uint8_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused uint8_t in_air : 1; // 7 - true when the vehicle is airborne uint8_t armed : 1; // 8 - true when the vehicle motors are armed + uint8_t wind : 1; // 9 - true when wind velocity is being estimated } flags; uint16_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index bb33b11fcf..a5596fbd77 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -145,6 +145,12 @@ void Ekf::controlFusionModes() _control_status.flags.mag_dec = false; } + // Placeholder for control of wind velocity states estimation + // TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement + if (false) { + _control_status.flags.wind = false; + } + } void Ekf::calculateVehicleStatus() From d8afca5e7a5a9e3f615dd7d4e85be25bad894d6c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 10:00:12 +1100 Subject: [PATCH 02/10] EKF derivations: Correct error in direct yaw and declination angle fusion The atan function is now being used correctly instead of the tan function. This fixes problems with large heading errors or declination values. The simple heading fusion has been decoupled from the magnetic field measurements. This enables external yaw measurements to be used in the future. --- .../Magnetic Declination Fusion.txt | 96 +++++++++---------- .../Simple Magnetometer Fusion.txt | 43 +++------ .../GenerateNavFilterEquations.m | 27 +++--- 3 files changed, 74 insertions(+), 92 deletions(-) diff --git a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt index 8e1d146575..c1455008a5 100644 --- a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt @@ -9,62 +9,56 @@ declinationAngle is the estimated declination as that location This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements (e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors can cause the magnetic declination of the earth field estimates to slowly rotate. + +Divide by zero protection and protection against a badly conditioned covariance matrix must be included. */ // Calculate intermediate variable -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; -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; +float t2 = magE*magE; +float t3 = magN*magN; +float t4 = t2+t3; +float t5 = 1.0f/t4; +float t22 = magE*t5; +float t23 = magN*t5; +float t6 = P[16][16]*t22; +float t13 = P[17][16]*t23; +float t7 = t6-t13; +float t8 = t22*t7; +float t9 = P[16][17]*t22; +float t14 = P[17][17]*t23; +float t10 = t9-t14; +float t15 = t23*t10; +float t11 = R_DECL+t8-t15; // innovation variance +float t12 = 1.0f/t11; // 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 -H_MAG[16] = -t18*1.0f/(t19*t19)*t24; -H_MAG[17] = t21*t24; +H_DECL[16] = -magE*t5; +H_DECL[17] = magN*t5; // Calculate the Kalman gains -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] = -t12*(P[0][16]*t22-P[0][17]*t23); +Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23); +Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23); +Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23); +Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23); +Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23); +Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23); +Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23); +Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23); +Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23); +Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23); +Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23); +Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23); +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); +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); +Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23); +Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23); +Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23); +Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23); +Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23); + diff --git a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt index 606c51d51b..ea4a5ba1d3 100644 --- a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt @@ -5,6 +5,8 @@ innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationA magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame. declinationAngle is the estimated declination as that location + +Protection against /0 and covariance matrix errors will need to be added. */ // intermediate variables @@ -12,36 +14,19 @@ float t2 = q0*q0; float t3 = q1*q1; float t4 = q2*q2; float t5 = q3*q3; -float t6 = q0*q2*2.0f; -float t7 = q1*q3*2.0f; -float t8 = t6+t7; -float t9 = q0*q3*2.0f; -float t13 = q1*q2*2.0f; -float t10 = t9-t13; -float t11 = t2+t3-t4-t5; -float t12 = magX*t11; -float t14 = magZ*t8; -float t19 = magY*t10; -float t15 = t12+t14-t19; -float t16 = t2-t3+t4-t5; -float t17 = q0*q1*2.0f; -float t24 = q2*q3*2.0f; -float t18 = t17-t24; -float t20 = 1.0f/t15; -float t21 = magY*t16; -float t22 = t9+t13; -float t23 = magX*t22; -float t28 = magZ*t18; -float t25 = t21+t23-t28; -float t29 = t20*t25; -float t26 = tan(t29); -float t27 = 1.0f/(t15*t15); -float t30 = t26*t26; -float t31 = t30+1.0f; +float t6 = t2+t3-t4-t5; +float t7 = q0*q3*2.0f; +float t8 = q1*q2*2.0f; +float t9 = t7+t8; +float t10 = 1.0f/(t6*t6); +float t11 = t9*t9; +float t12 = t10*t11; +float t13 = t12+1.0f; +float t14 = 1.0f/t13; +float t15 = 1.0f/t6; // Calculate the observation jacobian for the innovation derivative wrt the attitude error states only // Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update float H_MAG[3]; -H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10)); -H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11)); -H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11)); +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)); \ No newline at end of file diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index 7af03eecca..1a200935e8 100644 --- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -68,7 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2 syms ptd real % location of terrain in D axis syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame syms decl real; % earth magnetic field declination from true north -syms R_MAGS real; % variance for magnetic deviation measurement +syms R_YAW real; % variance for magnetic deviation measurement syms R_DECL real; % variance of supplied declination syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes syms rho real % air density (kg/m^3) @@ -269,19 +269,22 @@ ccode(H_LOS,'file','H_LOS.txt'); ccode(K_LOSX,'file','K_LOSX.txt'); ccode(K_LOSY,'file','K_LOSY.txt'); -%% derive equations for fusion of magnetic heading measurement +%% derive equations for fusion of direct yaw measurement -% rotate magnetic field into earth axes -magMeasNED = Tbn*[magX;magY;magZ]; -% the predicted measurement is the angle wrt magnetic north of the horizontal -% component of the measured field -angMeas = tan(magMeasNED(2)/magMeasNED(1)) - decl; -H_MAGS = jacobian(angMeas,errRotVec); % measurement Jacobian +% rotate X body axis into earth axes +yawVec = Tbn*[1;0;0]; +% Calculate the yaw angle of the projection +angMeas = atan(yawVec(2)/yawVec(1)); +H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian %H_MAGS = H_MAGS(1:3); -H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -H_MAGS = simplify(H_MAGS); +H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_YAW = simplify(H_YAW); %[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS'); -ccode(H_MAGS,'file','calcH_MAGS.c'); +ccode(H_YAW,'file','calcH_YAW.c'); +% Calculate Kalman gain vector +K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW); +%K_MAGS = simplify(K_MAGS); +ccode(K_YAW,'file','calcK_YAW.c'); %% derive equations for fusion of synthetic deviation measurement % used to keep correct heading when operating without absolute position or @@ -290,7 +293,7 @@ ccode(H_MAGS,'file','calcH_MAGS.c'); magMeasNED = [magN;magE;magD]; % the predicted measurement is the angle wrt magnetic north of the horizontal % component of the measured field -angMeas = tan(magMeasNED(2)/magMeasNED(1)); +angMeas = atan(magMeasNED(2)/magMeasNED(1)); H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_MAGD = simplify(H_MAGD); From 402206a305a40f44117f8b6141451cf043479485 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 10:03:50 +1100 Subject: [PATCH 03/10] EKF: Fix critical bug in fusion of yaw and declination observations This bug was in the derivation and resulted from use of a tan instead of an atan operator. The derivations have been reworked and the updated auto-code has been imported as part of this patch. --- EKF/mag_fusion.cpp | 208 +++++++++++++++++++++++---------------------- 1 file changed, 106 insertions(+), 102 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index e1f7857063..52c2b1433e 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -468,73 +468,71 @@ void Ekf::fuseHeading() float q2 = _state.quat_nominal(2); float q3 = _state.quat_nominal(3); - float magX = _mag_sample_delayed.mag(0); - float magY = _mag_sample_delayed.mag(1); - float magZ = _mag_sample_delayed.mag(2); - - float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f); - R_mag = R_mag * R_mag; + float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f); + R_YAW = R_YAW * R_YAW; + // calculate intermediate variables for observation jacobian float t2 = q0 * q0; float t3 = q1 * q1; float t4 = q2 * q2; float t5 = q3 * q3; - float t6 = q0 * q2 * 2.0f; - float t7 = q1 * q3 * 2.0f; - float t8 = t6 + t7; - float t9 = q0 * q3 * 2.0f; - float t13 = q1 * q2 * 2.0f; - float t10 = t9 - t13; - float t11 = t2 + t3 - t4 - t5; - float t12 = magX * t11; - float t14 = magZ * t8; - float t19 = magY * t10; - float t15 = t12 + t14 - t19; - float t16 = t2 - t3 + t4 - t5; - float t17 = q0 * q1 * 2.0f; - float t24 = q2 * q3 * 2.0f; - float t18 = t17 - t24; - float t20 = 1.0f / t15; - float t21 = magY * t16; - float t22 = t9 + t13; - float t23 = magX * t22; - float t28 = magZ * t18; - float t25 = t21 + t23 - t28; - float t29 = t20 * t25; - float t26 = tan(t29); - float t27 = 1.0f / (t15 * t15); - float t30 = t26 * t26; - float t31 = t30 + 1.0f; + float t6 = t2 + t3 - t4 - t5; + float t7 = q0 * q3 * 2.0f; + float t8 = q1 * q2 * 2.0f; + float t9 = t7 + t8; + float t10; - float H_HDG[3] = {}; - H_HDG[0] = -t31 * (t20 * (magZ * t16 + magY * t18) + t25 * t27 * (magY * t8 + magZ * t10)); - H_HDG[1] = t31 * (t20 * (magX * t18 + magZ * t22) + t25 * t27 * (magX * t8 - magZ * t11)); - H_HDG[2] = t31 * (t20 * (magX * t16 - magY * t22) + t25 * t27 * (magX * t10 + magY * t11)); + if (fabsf(t6) > 1e-6f) { + t10 = 1.0f / (t6 * t6); + + } else { + return; + } + + float t11 = t9 * t9; + float t12 = t10 * t11; + float t13 = t12 + 1.0f; + float t14; + + if (fabsf(t13) > 1e-6f) { + t14 = 1.0f / t13; + + } else { + return; + } + + 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)); // calculate innovation + // rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement + // TODO - enable use of an off-board heading measurement matrix::Dcm R_to_earth(_state.quat_nominal); matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination; - innovation = math::constrain(innovation, -0.5f, 0.5f); + // wrap the innovation to the interval between +-pi + innovation = matrix::wrap_pi(innovation); _heading_innov = innovation; - float innovation_var = R_mag; - _heading_innov_var = innovation_var; - // 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_HDG[column]; + PH[row] += P[row][column] * H_YAW[column]; } - innovation_var += H_HDG[row] * PH[row]; + innovation_var += H_YAW[row] * PH[row]; } - if (innovation_var >= R_mag) { + if (innovation_var >= R_YAW) { // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.bad_mag_hdg = false; @@ -547,14 +545,14 @@ void Ekf::fuseHeading() return; } - float innovation_var_inv = 1 / innovation_var; + float innovation_var_inv = 1.0f / innovation_var; - // calculate kalman gain + // 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++) { for (unsigned column = 0; column < 3; column++) { - Kfusion[row] += P[row][column] * H_HDG[column]; + Kfusion[row] += P[row][column] * H_YAW[column]; } Kfusion[row] *= innovation_var_inv; @@ -572,12 +570,18 @@ void Ekf::fuseHeading() // by interference or a large initial gyro bias if (_control_status.flags.in_air) { return; + + } 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)); + innovation = math::constrain(innovation, -gate_limit, gate_limit); } } else { _mag_healthy = true; } + // zero the attitude error states and use the kalman gain vector and innovation to update the states _state.ang_error.setZero(); fuse(Kfusion, innovation); @@ -587,11 +591,12 @@ void Ekf::fuseHeading() _state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal.normalize(); + // update the covariance matrix taking advantage of the reduced size of H_YAW float HP[_k_num_states] = {}; for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned row = 0; row < 3; row++) { - HP[column] += H_HDG[row] * P[row][column]; + HP[column] += H_YAW[row] * P[row][column]; } } @@ -615,30 +620,28 @@ void Ekf::fuseDeclination() // Calculate intermediate variables // if the horizontal magnetic field is too small, this calculation will be badly conditioned - if (fabsf(magN) < 0.001f) { + if (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 + float t2 = magE * magE; + float t3 = magN * magN; + float t4 = t2 + t3; + float t5 = 1.0f / t4; + float t22 = magE * t5; + float t23 = magN * t5; + float t6 = P[16][16] * t22; + float t13 = P[17][16] * t23; + float t7 = t6 - t13; + float t8 = t22 * t7; + float t9 = P[16][17] * t22; + float t14 = P[17][17] * t23; + float t10 = t9 - t14; + float t15 = t23 * t10; + float t11 = R_DECL + t8 - t15; // innovation variance // check the innovation variance calculation for a badly conditioned covariance matrix - if (t13 >= R_DECL) { + if (t11 >= R_DECL) { // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.bad_mag_decl = false; @@ -651,50 +654,51 @@ void Ekf::fuseDeclination() 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; + float t12 = 1.0f / t11; // 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] = -magE * t5; + H_DECL[17] = magN * t5; // 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] = -t12 * (P[0][16] * t22 - P[0][17] * t23); + Kfusion[1] = -t12 * (P[1][16] * t22 - P[1][17] * t23); + Kfusion[2] = -t12 * (P[2][16] * t22 - P[2][17] * t23); + Kfusion[3] = -t12 * (P[3][16] * t22 - P[3][17] * t23); + Kfusion[4] = -t12 * (P[4][16] * t22 - P[4][17] * t23); + Kfusion[5] = -t12 * (P[5][16] * t22 - P[5][17] * t23); + Kfusion[6] = -t12 * (P[6][16] * t22 - P[6][17] * t23); + Kfusion[7] = -t12 * (P[7][16] * t22 - P[7][17] * t23); + Kfusion[8] = -t12 * (P[8][16] * t22 - P[8][17] * t23); + Kfusion[9] = -t12 * (P[9][16] * t22 - P[9][17] * t23); + Kfusion[10] = -t12 * (P[10][16] * t22 - P[10][17] * t23); + Kfusion[11] = -t12 * (P[11][16] * t22 - P[11][17] * t23); + Kfusion[12] = -t12 * (P[12][16] * t22 - P[12][17] * t23); + 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); + 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); + Kfusion[19] = -t12 * (P[19][16] * t22 - P[19][17] * t23); + Kfusion[20] = -t12 * (P[20][16] * t22 - P[20][17] * t23); + Kfusion[21] = -t12 * (P[21][16] * t22 - P[21][17] * t23); + + // Don't update wind states unless we are doing wind estimation + if (_control_status.flags.wind) { + Kfusion[22] = -t12 * (P[22][16] * t22 - P[22][17] * t23); + Kfusion[23] = -t12 * (P[23][16] * t22 - P[23][17] * t23); + + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } // calculate innovation and constrain - float innovation = atanf(t4) - _mag_declination; + float innovation = atanf(magE / magN) - _mag_declination; innovation = math::constrain(innovation, -0.5f, 0.5f); // zero attitude error states and perform the state correction From d9bf4e9870636b352bc08f994fbfde10dc24d536 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 10:40:05 +1100 Subject: [PATCH 04/10] EKF: Enable control mode transitions to be detected Save the previous value of the filter control modes --- EKF/control.cpp | 2 ++ EKF/ekf.h | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index a5596fbd77..d2ceafb9a6 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -151,6 +151,8 @@ void Ekf::controlFusionModes() _control_status.flags.wind = false; } + // Store the status to enable change detection + _control_status_prev.value = _control_status.value; } void Ekf::calculateVehicleStatus() diff --git a/EKF/ekf.h b/EKF/ekf.h index de693135e9..a7a426463a 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -85,8 +85,12 @@ public: bool collect_gps(uint64_t time_usec, struct gps_message *gps); bool collect_imu(imuSample &imu); + // this is the current status of the filter control modes filter_control_status_u _control_status = {}; + // this is the previous status of the filter control modes - used to detect mode transitions + filter_control_status_u _control_status_prev = {}; + // get the ekf WGS-84 origin position and height and the system time it was last set void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt); From 69b8982043258bfa32d344b25c34d4a20d19da21 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 10:42:18 +1100 Subject: [PATCH 05/10] EKF: Initialise variances to zero for un-used states Setting these variances to zero makes it less likely that these states will be modified by fusion processing. --- EKF/covariance.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 105a692602..45b63a00d0 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -81,19 +81,22 @@ void Ekf::initialiseCovariance() // accel z bias P[15][15] = 0.0001f; + // variances for optional states + // these state variances are set to zero until the states are required, then they must be initialised + // earth magnetic field - P[16][16] = 0.0001f; - P[17][17] = 0.0001f; - P[18][18] = 0.0001f; + P[16][16] = 0.0f; + P[17][17] = 0.0f; + P[18][18] = 0.0f; // body magnetic field - P[19][19] = 0.0001f; - P[20][20] = 0.0001f; - P[21][21] = 0.0001f; + P[19][19] = 0.0f; + P[20][20] = 0.0f; + P[21][21] = 0.0f; // wind - P[22][22] = 0.01f; - P[23][23] = 0.01f; + P[22][22] = 0.0f; + P[23][23] = 0.0f; } From 1d40507af84068d2402ba9b70240f7a4bfec7806 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 11:01:51 +1100 Subject: [PATCH 06/10] EKF: Set Kaman gains to zero for un-used states This is a defensive programming technique. In theory this should not be necessary if the corresponding covariance entries are zero. --- EKF/mag_fusion.cpp | 75 ++++++++++++++++++++++++++++++++++-------- EKF/vel_pos_fusion.cpp | 24 ++++++++++++-- 2 files changed, 84 insertions(+), 15 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 52c2b1433e..9434266921 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -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); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index dcc695262f..6ec7300a89 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -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(); From 294aca8609855cd0c03865fbb26d9aacad7202dd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 11:05:46 +1100 Subject: [PATCH 07/10] EKF: Improve variance limiting for stationary states Turn off the process noise if they grow too large and use the hard variance limit as a backup. This is numerically more stable than relying on a hard variance limit which does not deal with the covariances. --- EKF/covariance.cpp | 34 +++++++++++++++++++++++++++++----- 1 file changed, 29 insertions(+), 5 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 45b63a00d0..3a8b528302 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -134,9 +134,33 @@ void Ekf::predictCovariance() float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f); float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f); float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f); - float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); - float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); - float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f); + float mag_I_sig, mag_B_sig; + + // Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) { + mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); + + } else { + mag_I_sig = 0.0f; + } + + // Don't continue to grow the earth field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) { + mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); + + } else { + mag_B_sig = 0.0f; + } + + float wind_vel_sig; + + // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned + if (_control_status.flags.wind && (P[22][22] + P[22][22]) < 1000.0f) { + wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f); + + } else { + wind_vel_sig = 0.0f; + } for (unsigned i = 0; i < 9; i++) { process_noise[i] = 0.0f; @@ -709,8 +733,8 @@ void Ekf::limitCov() P_lim[3] = 0.001f; // gyro bias max var P_lim[4] = 0.01f; // gyro scale max var P_lim[5] = 0.1f; // delta velocity z bias max var - P_lim[6] = 0.01f; // earth mag field max var - P_lim[7] = 0.01f; // body mag field max var + P_lim[6] = 0.1f; // earth mag field max var + P_lim[7] = 0.1f; // body mag field max var P_lim[8] = 1000.0f; // wind max var for (int i = 0; i < 3; i++) { From ffe1d30864be3e94029261e14c2ed04708ef91ef Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 11:06:54 +1100 Subject: [PATCH 08/10] EKF: Set covariances to zero for un-used states --- EKF/covariance.cpp | 358 ++++++++++++++++++++++++--------------------- 1 file changed, 194 insertions(+), 164 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 3a8b528302..135b325e48 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -520,170 +520,200 @@ void Ekf::predictCovariance() nextP[13][15] = P[13][15]; nextP[14][15] = P[14][15]; nextP[15][15] = P[15][15]; - nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18]; - nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17]; - nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16]; - nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21]; - nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11]; - nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0]; - nextP[6][16] = P[6][16] + P[3][16] * dt; - nextP[7][16] = P[7][16] + P[4][16] * dt; - nextP[8][16] = P[8][16] + P[5][16] * dt; - nextP[9][16] = P[9][16]; - nextP[10][16] = P[10][16]; - nextP[11][16] = P[11][16]; - nextP[12][16] = P[12][16]; - nextP[13][16] = P[13][16]; - nextP[14][16] = P[14][16]; - nextP[15][16] = P[15][16]; - nextP[16][16] = P[16][16]; - nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18]; - nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17]; - nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16]; - nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21]; - nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11]; - nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0]; - nextP[6][17] = P[6][17] + P[3][17] * dt; - nextP[7][17] = P[7][17] + P[4][17] * dt; - nextP[8][17] = P[8][17] + P[5][17] * dt; - nextP[9][17] = P[9][17]; - nextP[10][17] = P[10][17]; - nextP[11][17] = P[11][17]; - nextP[12][17] = P[12][17]; - nextP[13][17] = P[13][17]; - nextP[14][17] = P[14][17]; - nextP[15][17] = P[15][17]; - nextP[16][17] = P[16][17]; - nextP[17][17] = P[17][17]; - nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18]; - nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17]; - nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16]; - nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21]; - nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11]; - nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0]; - nextP[6][18] = P[6][18] + P[3][18] * dt; - nextP[7][18] = P[7][18] + P[4][18] * dt; - nextP[8][18] = P[8][18] + P[5][18] * dt; - nextP[9][18] = P[9][18]; - nextP[10][18] = P[10][18]; - nextP[11][18] = P[11][18]; - nextP[12][18] = P[12][18]; - nextP[13][18] = P[13][18]; - nextP[14][18] = P[14][18]; - nextP[15][18] = P[15][18]; - nextP[16][18] = P[16][18]; - nextP[17][18] = P[17][18]; - nextP[18][18] = P[18][18]; - nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18]; - nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17]; - nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16]; - nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21]; - nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11]; - nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0]; - nextP[6][19] = P[6][19] + P[3][19] * dt; - nextP[7][19] = P[7][19] + P[4][19] * dt; - nextP[8][19] = P[8][19] + P[5][19] * dt; - nextP[9][19] = P[9][19]; - nextP[10][19] = P[10][19]; - nextP[11][19] = P[11][19]; - nextP[12][19] = P[12][19]; - nextP[13][19] = P[13][19]; - nextP[14][19] = P[14][19]; - nextP[15][19] = P[15][19]; - nextP[16][19] = P[16][19]; - nextP[17][19] = P[17][19]; - nextP[18][19] = P[18][19]; - nextP[19][19] = P[19][19]; - nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18]; - nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17]; - nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16]; - nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21]; - nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11]; - nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0]; - nextP[6][20] = P[6][20] + P[3][20] * dt; - nextP[7][20] = P[7][20] + P[4][20] * dt; - nextP[8][20] = P[8][20] + P[5][20] * dt; - nextP[9][20] = P[9][20]; - nextP[10][20] = P[10][20]; - nextP[11][20] = P[11][20]; - nextP[12][20] = P[12][20]; - nextP[13][20] = P[13][20]; - nextP[14][20] = P[14][20]; - nextP[15][20] = P[15][20]; - nextP[16][20] = P[16][20]; - nextP[17][20] = P[17][20]; - nextP[18][20] = P[18][20]; - nextP[19][20] = P[19][20]; - nextP[20][20] = P[20][20]; - nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18]; - nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17]; - nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16]; - nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21]; - nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11]; - nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0]; - nextP[6][21] = P[6][21] + P[3][21] * dt; - nextP[7][21] = P[7][21] + P[4][21] * dt; - nextP[8][21] = P[8][21] + P[5][21] * dt; - nextP[9][21] = P[9][21]; - nextP[10][21] = P[10][21]; - nextP[11][21] = P[11][21]; - nextP[12][21] = P[12][21]; - nextP[13][21] = P[13][21]; - nextP[14][21] = P[14][21]; - nextP[15][21] = P[15][21]; - nextP[16][21] = P[16][21]; - nextP[17][21] = P[17][21]; - nextP[18][21] = P[18][21]; - nextP[19][21] = P[19][21]; - nextP[20][21] = P[20][21]; - nextP[21][21] = P[21][21]; - nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18]; - nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17]; - nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16]; - nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21]; - nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11]; - nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0]; - nextP[6][22] = P[6][22] + P[3][22] * dt; - nextP[7][22] = P[7][22] + P[4][22] * dt; - nextP[8][22] = P[8][22] + P[5][22] * dt; - nextP[9][22] = P[9][22]; - nextP[10][22] = P[10][22]; - nextP[11][22] = P[11][22]; - nextP[12][22] = P[12][22]; - nextP[13][22] = P[13][22]; - nextP[14][22] = P[14][22]; - nextP[15][22] = P[15][22]; - nextP[16][22] = P[16][22]; - nextP[17][22] = P[17][22]; - nextP[18][22] = P[18][22]; - nextP[19][22] = P[19][22]; - nextP[20][22] = P[20][22]; - nextP[21][22] = P[21][22]; - nextP[22][22] = P[22][22]; - nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18]; - nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17]; - nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16]; - nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21]; - nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11]; - nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0]; - nextP[6][23] = P[6][23] + P[3][23] * dt; - nextP[7][23] = P[7][23] + P[4][23] * dt; - nextP[8][23] = P[8][23] + P[5][23] * dt; - nextP[9][23] = P[9][23]; - nextP[10][23] = P[10][23]; - nextP[11][23] = P[11][23]; - nextP[12][23] = P[12][23]; - nextP[13][23] = P[13][23]; - nextP[14][23] = P[14][23]; - nextP[15][23] = P[15][23]; - nextP[16][23] = P[16][23]; - nextP[17][23] = P[17][23]; - nextP[18][23] = P[18][23]; - nextP[19][23] = P[19][23]; - nextP[20][23] = P[20][23]; - nextP[21][23] = P[21][23]; - nextP[22][23] = P[22][23]; - nextP[23][23] = P[23][23]; + + // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion + if (_control_status.flags.mag_3D) { + // Check if we have just transitioned into 3-axis fusion and set the state variances + if (!_control_status_prev.flags.mag_3D) { + for (uint8_t index = 16; index <= 21; index++) { + P[index][index] = sq(fmaxf(_params.mag_noise, 0.001f)); + } + } + + nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18]; + nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17]; + nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16]; + nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21]; + nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11]; + nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0]; + nextP[6][16] = P[6][16] + P[3][16] * dt; + nextP[7][16] = P[7][16] + P[4][16] * dt; + nextP[8][16] = P[8][16] + P[5][16] * dt; + nextP[9][16] = P[9][16]; + nextP[10][16] = P[10][16]; + nextP[11][16] = P[11][16]; + nextP[12][16] = P[12][16]; + nextP[13][16] = P[13][16]; + nextP[14][16] = P[14][16]; + nextP[15][16] = P[15][16]; + nextP[16][16] = P[16][16]; + + nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18]; + nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17]; + nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16]; + nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21]; + nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11]; + nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0]; + nextP[6][17] = P[6][17] + P[3][17] * dt; + nextP[7][17] = P[7][17] + P[4][17] * dt; + nextP[8][17] = P[8][17] + P[5][17] * dt; + nextP[9][17] = P[9][17]; + nextP[10][17] = P[10][17]; + nextP[11][17] = P[11][17]; + nextP[12][17] = P[12][17]; + nextP[13][17] = P[13][17]; + nextP[14][17] = P[14][17]; + nextP[15][17] = P[15][17]; + nextP[16][17] = P[16][17]; + nextP[17][17] = P[17][17]; + + nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18]; + nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17]; + nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16]; + nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21]; + nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11]; + nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0]; + nextP[6][18] = P[6][18] + P[3][18] * dt; + nextP[7][18] = P[7][18] + P[4][18] * dt; + nextP[8][18] = P[8][18] + P[5][18] * dt; + nextP[9][18] = P[9][18]; + nextP[10][18] = P[10][18]; + nextP[11][18] = P[11][18]; + nextP[12][18] = P[12][18]; + nextP[13][18] = P[13][18]; + nextP[14][18] = P[14][18]; + nextP[15][18] = P[15][18]; + nextP[16][18] = P[16][18]; + nextP[17][18] = P[17][18]; + nextP[18][18] = P[18][18]; + + nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18]; + nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17]; + nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16]; + nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21]; + nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11]; + nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0]; + nextP[6][19] = P[6][19] + P[3][19] * dt; + nextP[7][19] = P[7][19] + P[4][19] * dt; + nextP[8][19] = P[8][19] + P[5][19] * dt; + nextP[9][19] = P[9][19]; + nextP[10][19] = P[10][19]; + nextP[11][19] = P[11][19]; + nextP[12][19] = P[12][19]; + nextP[13][19] = P[13][19]; + nextP[14][19] = P[14][19]; + nextP[15][19] = P[15][19]; + nextP[16][19] = P[16][19]; + nextP[17][19] = P[17][19]; + nextP[18][19] = P[18][19]; + nextP[19][19] = P[19][19]; + + nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18]; + nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17]; + nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16]; + nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21]; + nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11]; + nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0]; + nextP[6][20] = P[6][20] + P[3][20] * dt; + nextP[7][20] = P[7][20] + P[4][20] * dt; + nextP[8][20] = P[8][20] + P[5][20] * dt; + nextP[9][20] = P[9][20]; + nextP[10][20] = P[10][20]; + nextP[11][20] = P[11][20]; + nextP[12][20] = P[12][20]; + nextP[13][20] = P[13][20]; + nextP[14][20] = P[14][20]; + nextP[15][20] = P[15][20]; + nextP[16][20] = P[16][20]; + nextP[17][20] = P[17][20]; + nextP[18][20] = P[18][20]; + nextP[19][20] = P[19][20]; + nextP[20][20] = P[20][20]; + + nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18]; + nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17]; + nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16]; + nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21]; + nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11]; + nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0]; + nextP[6][21] = P[6][21] + P[3][21] * dt; + nextP[7][21] = P[7][21] + P[4][21] * dt; + nextP[8][21] = P[8][21] + P[5][21] * dt; + nextP[9][21] = P[9][21]; + nextP[10][21] = P[10][21]; + nextP[11][21] = P[11][21]; + nextP[12][21] = P[12][21]; + nextP[13][21] = P[13][21]; + nextP[14][21] = P[14][21]; + nextP[15][21] = P[15][21]; + nextP[16][21] = P[16][21]; + nextP[17][21] = P[17][21]; + nextP[18][21] = P[18][21]; + nextP[19][21] = P[19][21]; + nextP[20][21] = P[20][21]; + nextP[21][21] = P[21][21]; + } + + // Don't do covariance prediction on wind states unless we are using them + if (_control_status.flags.wind) { + // Check if we have jsut transitioned to using wind states and set the variances accordingly + if (!_control_status_prev.flags.mag_3D) { + for (uint8_t index = 22; index <= 23; index++) { + // TODO initialise wind states using ground speed and airspeed and set initial variance using sum of ground speed and airspeed variances + P[index][index] = sq(5.0f); + } + } + + nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18]; + nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17]; + nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16]; + nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21]; + nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11]; + nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0]; + nextP[6][22] = P[6][22] + P[3][22] * dt; + nextP[7][22] = P[7][22] + P[4][22] * dt; + nextP[8][22] = P[8][22] + P[5][22] * dt; + nextP[9][22] = P[9][22]; + nextP[10][22] = P[10][22]; + nextP[11][22] = P[11][22]; + nextP[12][22] = P[12][22]; + nextP[13][22] = P[13][22]; + nextP[14][22] = P[14][22]; + nextP[15][22] = P[15][22]; + nextP[16][22] = P[16][22]; + nextP[17][22] = P[17][22]; + nextP[18][22] = P[18][22]; + nextP[19][22] = P[19][22]; + nextP[20][22] = P[20][22]; + nextP[21][22] = P[21][22]; + nextP[22][22] = P[22][22]; + + nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18]; + nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17]; + nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16]; + nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21]; + nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11]; + nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0]; + nextP[6][23] = P[6][23] + P[3][23] * dt; + nextP[7][23] = P[7][23] + P[4][23] * dt; + nextP[8][23] = P[8][23] + P[5][23] * dt; + nextP[9][23] = P[9][23]; + nextP[10][23] = P[10][23]; + nextP[11][23] = P[11][23]; + nextP[12][23] = P[12][23]; + nextP[13][23] = P[13][23]; + nextP[14][23] = P[14][23]; + nextP[15][23] = P[15][23]; + nextP[16][23] = P[16][23]; + nextP[17][23] = P[17][23]; + nextP[18][23] = P[18][23]; + nextP[19][23] = P[19][23]; + nextP[20][23] = P[20][23]; + nextP[21][23] = P[21][23]; + nextP[22][23] = P[22][23]; + nextP[23][23] = P[23][23]; + + } // add process noise for (unsigned i = 0; i < _k_num_states; i++) { From abf94768537a8cf4238ddd1c56bf593dacf3c9e0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 11:07:20 +1100 Subject: [PATCH 09/10] EKF: miscellaneous formatting and typo updates --- EKF/covariance.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 135b325e48..bf360eef10 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -721,7 +721,7 @@ void Ekf::predictCovariance() } // stop position covariance growth if our total position variance reaches 100m - // this can happen if we loose gps for some time + // this can happen if we lose gps for some time if ((P[6][6] + P[7][7]) > 1e4f) { for (uint8_t i = 6; i < 8; i++) { for (uint8_t j = 0; j < _k_num_states; j++) { @@ -738,10 +738,12 @@ void Ekf::predictCovariance() } } + // copy variances (diagonals) for (unsigned i = 0; i < _k_num_states; i++) { P[i][i] = nextP[i][i]; } + // force symmetry for (unsigned row = 1; row < _k_num_states; row++) { for (unsigned column = 0; column < row; column++) { P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]); @@ -799,20 +801,14 @@ void Ekf::limitCov() math::constrain(P[15][15], 0.0f, P_lim[5]); for (int i = 16; i < 19; i++) { - math::constrain(P[i][i], 0.0f, P_lim[6]); } for (int i = 19; i < 22; i++) { - - math::constrain(P[i][i], 0.0f, P_lim[7]); } for (int i = 22; i < 24; i++) { - math::constrain(P[i][i], 0.0f, P_lim[8]); } } - - From a679cdf1fb829075f06b7073a8e57da0f9686ab0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 17 Feb 2016 17:21:10 +1100 Subject: [PATCH 10/10] EKF: Change parameter default to not fuse declination when aiding Ground and flight testing has shown declination drift to not be a problem with current tuning and realistic vehicle motion. --- EKF/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 9cf70fef8c..49ff1318ed 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -141,7 +141,7 @@ struct parameters { float mag_declination_deg = 0.0f; // magnetic declination in degrees float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations - int mag_declination_source = 7; // bitmask used to control the handling of declination data + int mag_declination_source = 3; // bitmask used to control the handling of declination data int mag_fusion_type = 0; // integer used to specify the type of magnetometer fusion used // these parameters control the strictness of GPS quality checks used to determine uf the GPS is