diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index e3daff595e..1f9fd428e4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -279,21 +279,21 @@ bool NavEKF::healthy(void) const return true; } -// Returns true if height drift is not being constrained +// return true if filter is dead-reckoning height bool NavEKF::HeightDrifting(void) const { // Set to true if height measurements are failing the innovation consistency check return !hgtHealth; } -// Returns true if position drift is not being constrained +// return true if filter is dead-reckoning position bool NavEKF::PositionDrifting(void) const { // Set to true if position measurements are failing the innovation consistency check return !posHealth; } -// Resets position states to last GPS measurement or to zero if in static mode +// resets position states to last GPS measurement or to zero if in static mode void NavEKF::ResetPosition(void) { if (staticMode) { @@ -309,7 +309,7 @@ void NavEKF::ResetPosition(void) } } -// Resets velocity states to last GPS measurement or to zero if in static mode +// resets velocity states to last GPS measurement or to zero if in static mode void NavEKF::ResetVelocity(void) { if (staticMode) { @@ -337,18 +337,19 @@ void NavEKF::ResetVelocity(void) } } +// reset the vertical position state using the last height measurement void NavEKF::ResetHeight(void) { // read the altimeter readHgtData(); - // write to state vector + // write to the state vector states[9] = -hgtMea; // down position from blended accel data state.posD1 = -hgtMea; // down position from IMU1 accel data state.posD2 = -hgtMea; // down position from IMU2 accel data } -// This function is used to initialise the filter whilst moving, using the AHRS DCM solution -// It should NOT be used to re-initialise after a timeout as DCM will also be corrupted +// this function is used to initialise the filter whilst moving, using the AHRS DCM solution +// it should NOT be used to re-initialise after a timeout as DCM will also be corrupted void NavEKF::InitialiseFilterDynamic(void) { // this forces healthy() to be false so that when we ask for ahrs @@ -427,6 +428,8 @@ void NavEKF::InitialiseFilterDynamic(void) readIMUData(); } +// Initialise the states from accelerometer and magnetometer data (if present) +// This method can only be used when the vehicle is static void NavEKF::InitialiseFilterBootstrap(void) { // set re-used variables to zero @@ -515,6 +518,7 @@ void NavEKF::InitialiseFilterBootstrap(void) readIMUData(); } +// Update Filter States - this should be called whenever new IMU data is available void NavEKF::UpdateFilter() { // don't run filter updates if states have not been initialised @@ -708,11 +712,19 @@ void NavEKF::SelectMagFusion() // select fusion of true airspeed measurements void NavEKF::SelectTasFusion() { + // get true airspeed measurement readAirSpdData(); - // Determine if data is waiting to be fused + + // if the filter is initialised and we are using airspeed measurements and we are flying and + // we either have new data or are waiting to fuse old data, then perform fusion tasDataWaiting = (statesInitialised && useAirspeed() && !onGround && (tasDataWaiting || newDataTas)); + + // if we have waited too long, set a timeout flag which will force fusion to occur bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax); - // Fuse Airspeed Measurements - hold off if magnetometer fusion has been performed, unless maximum time interval exceeded + + // we don't fuse airspeed measurements if magnetometer fusion has been performed in the same frame, unless timed out or the fuseMeNow option is selected + // this helps to spreasthe load associated with fusion of different measurements across multiple frames + // setting fuseMeNow to true disables this load spreading feature if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow)) { FuseAirspeed(); @@ -736,35 +748,36 @@ void NavEKF::SelectBetaFusion() } } +// update the quaternion, velocity and position states using IMU measurements void NavEKF::UpdateStrapdownEquationsNED() { - Vector3f delVelNav; - Vector3f delVelNav1; - Vector3f delVelNav2; - float rotationMag; - float rotScaler; - Quaternion qUpdated; - Quaternion deltaQuat; - const Vector3f gravityNED(0, 0, GRAVITY_MSS); + Vector3f delVelNav; // delta velocity vector calculated using a blend of IMU1 and IMU2 data + Vector3f delVelNav1; // delta velocity vector calculated using IMU1 data + Vector3f delVelNav2; // delta velocity vector calculated using IMU2 data + float rotationMag; // magnitude of rotation vector from previous to current time step + float rotScaler; // scaling variable used to calculate delta quaternion from last to current time step + Quaternion qUpdated; // quaternion at current time step after application of delta quaternion + Quaternion deltaQuat; // quaternion from last to current time step + const Vector3f gravityNED(0, 0, GRAVITY_MSS); // NED gravity vector m/s^2 - // Remove sensor bias errors + // remove sensor bias errors correctedDelAng = dAngIMU - state.gyro_bias; correctedDelVel1 = dVelIMU1; correctedDelVel2 = dVelIMU2; correctedDelVel1.z -= state.accel_zbias1; correctedDelVel2.z -= state.accel_zbias2; - // Use weighted average of both IMU units for delta velocities + // use weighted average of both IMU units for delta velocities correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting); - // Save current measurements + // save current measurements prevDelAng = correctedDelAng; - // Apply corrections for earths rotation rate and coning errors + // apply corrections for earths rotation rate and coning errors // % * - and + operators have been overloaded correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f; - // Convert the rotation vector to its equivalent quaternion + // convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) { @@ -782,18 +795,18 @@ void NavEKF::UpdateStrapdownEquationsNED() deltaQuat[3] = correctedDelAng.z * rotScaler; } - // Update the quaternions by rotating from the previous attitude through + // update the quaternions by rotating from the previous attitude through // the delta angle rotation quaternion qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - // Normalise the quaternions and update the quaternion states + // normalise the quaternions and update the quaternion states qUpdated.normalize(); state.quat = qUpdated; - // Calculate the body to nav cosine matrix + // calculate the body to nav cosine matrix Quaternion q(states[0],states[1],states[2],states[3]); Matrix3f Tbn_temp; q.rotation_matrix(Tbn_temp); @@ -807,10 +820,10 @@ void NavEKF::UpdateStrapdownEquationsNED() delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMU; delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMU; - // Calculate the rate of change of velocity (used for launch detect and other functions) + // calculate the rate of change of velocity (used for launch detect and other functions) velDotNED = delVelNav / dtIMU ; - // Apply a first order lowpass filter + // apply a first order lowpass filter velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f; // calculate a magnitude of the filtered nav acceleration (required for GPS @@ -818,54 +831,54 @@ void NavEKF::UpdateStrapdownEquationsNED() accNavMag = velDotNEDfilt.length(); accNavMagHoriz = pythagorous2(velDotNEDfilt.x , velDotNEDfilt.y); - // If calculating position save previous velocity + // save velocity for use in trapezoidal intergration for position calcuation Vector3f lastVelocity = state.velocity; Vector3f lastVel1 = state.vel1; Vector3f lastVel2 = state.vel2; - // Sum delta velocities to get velocity + // sum delta velocities to get velocity state.velocity += delVelNav; state.vel1 += delVelNav1; state.vel2 += delVelNav2; - // If calculating postions, do a trapezoidal integration for position + // apply a trapezoidal integration to velocities to calculate position state.position += (state.velocity + lastVelocity) * (dtIMU*0.5f); state.posD1 += (state.vel1.z + lastVel1.z) * (dtIMU*0.5f); state.posD2 += (state.vel2.z + lastVel2.z) * (dtIMU*0.5f); - // Limit states to protect against divergence + // limit states to protect against divergence ConstrainStates(); } +// calculate the predicted state covariance matrix void NavEKF::CovariancePrediction() { perf_begin(_perf_CovariancePrediction); - // scalars - float windVelSigma; - float dAngBiasSigma; - float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; - float daxCov; - float dayCov; - float dazCov; - float dvxCov; - float dvyCov; - float dvzCov; - float dvx; - float dvy; - float dvz; - float dax; - float day; - float daz; - float q0; - float q1; - float q2; - float q3; - float dax_b; - float day_b; - float daz_b; - float dvz_b; + float windVelSigma; // wind velocity 1-sigma process noise - m/s + float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s + float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s + float magEarthSigma;// earth magnetic field 1-sigma process noise + float magBodySigma; // body magnetic field 1-sigma process noise + float daxCov; // X axis delta angle variance rad^2 + float dayCov; // Y axis delta angle variance rad^2 + float dazCov; // Z axis delta angle variance rad^2 + float dvxCov; // X axis delta velocity variance (m/s)^2 + float dvyCov; // Y axis delta velocity variance (m/s)^2 + float dvzCov; // Z axis delta velocity variance (m/s)^2 + float dvx; // X axis delta velocity (m/s) + float dvy; // Y axis delta velocity (m/s) + float dvz; // Z axis delta velocity (m/s) + float dax; // X axis delta angle (rad) + float day; // Y axis delta angle (rad) + float daz; // Z axis delta angle (rad) + float q0; // attitude quaternion + float q1; // attitude quaternion + float q2; // attitude quaternion + float q3; // attitude quaternion + float dax_b; // X axis delta angle measurement bias (rad) + float day_b; // Y axis delta angle measurement bias (rad) + float daz_b; // Z axis delta angle measurement bias (rad) + float dvz_b; // Z axis delta velocity measurement bias (rad) // calculate covariance prediction process noise // use filtered height rate to increase wind process noise when climbing or descending @@ -920,7 +933,7 @@ void NavEKF::CovariancePrediction() dvyCov = sq(dt*_accNoise); dvzCov = sq(dt*_accNoise); - // Predicted covariance calculation + // calculate the predicted covariance due to inertial sensor error propagation SF[0] = dvz - dvz_b; SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; @@ -1452,12 +1465,13 @@ void NavEKF::CovariancePrediction() nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; + // add the general state process noise variances for (uint8_t i=0; i<= 21; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } - // If the total position variance exceeds 1E6 (1000m), then stop covariance + // if the total position variance exceeds 1E6 (1000m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods // without GPS @@ -1473,17 +1487,21 @@ void NavEKF::CovariancePrediction() } } - // Copy covariances to output and fix numerical errors + // copy covariances to output and fix numerical errors CopyAndFixCovariances(); - // Constrain diagonals to prevent ill-conditioning + // constrain diagonals to prevent ill-conditioning ConstrainVariances(); perf_end(_perf_CovariancePrediction); } +// fuse selected position, velocity and height measurements, checking dat for consistency +// provide a static mode that allows maintenance of the attitude reference without GPS provided the vehicle is not accelerating +// check innovation consistency of velocity states calculated using IMU1 and IMU2 and calculate the optimal weighting of accel data void NavEKF::FuseVelPosNED() { + // start performance timer perf_begin(_perf_FuseVelPosNED); // health is set bad until test passed @@ -1512,7 +1530,7 @@ void NavEKF::FuseVelPosNED() Vector6 observation; float SK; - // Perform sequential fusion of GPS measurements. This assumes that the + // perform sequential fusion of GPS measurements. This assumes that the // errors in the different velocity and position components are // uncorrelated which is not true, however in the absence of covariance // data from the GPS receiver it is the only assumption we can make @@ -1522,8 +1540,8 @@ void NavEKF::FuseVelPosNED() { // if static mode is active use the current states to calculate the predicted - // measurement. We need to do this because there may be no stored states due - // to lack of measurements. + // measurement rather than use states from a previous time. We need to do this + // because there may be no stored states due to lack of real measurements. // in static mode, only position and height fusion is used if (staticMode) { for (uint8_t i=7; i<=9; i++) { @@ -1537,8 +1555,7 @@ void NavEKF::FuseVelPosNED() if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS; else gpsRetryTime = _gpsRetryTimeNoTAS; - // Form the observation vector - // zero observations if in static mode (used for pre-arm and bench testing) + // form the observation vector and zero observations if in static mode if (~staticMode) { for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; @@ -1547,14 +1564,14 @@ void NavEKF::FuseVelPosNED() for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f; } - // additional error in GPS velocity caused by manoeuvring + // calculate additional error in GPS velocity caused by manoeuvring NEvelErr = _gpsNEVelVarAccScale * accNavMag; DvelErr = _gpsDVelVarAccScale * accNavMag; - // additional error in GPS position caused by manoeuvring + // calculate additional error in GPS position caused by manoeuvring posErr = _gpsPosVarAccScale * accNavMag; - // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + // estimate the GPS Velocity, GPS horiz position and height measurement variances. R_OBS[0] = gpsVarScaler*(sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr)); R_OBS[1] = R_OBS[0]; R_OBS[2] = gpsVarScaler*(sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr)); @@ -1562,7 +1579,7 @@ void NavEKF::FuseVelPosNED() R_OBS[4] = R_OBS[3]; R_OBS[5] = hgtVarScaler*sq(constrain_float(_baroAltNoise, 0.1f, 10.0f)); - // If vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer + // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. bool badIMUdata = false; @@ -1570,7 +1587,7 @@ void NavEKF::FuseVelPosNED() // calculate innovations for height and vertical GPS vel measurements float hgtErr = statesAtVelTime[9] - observation[5]; float velDErr = statesAtVelTime[6] - observation[2]; - //check if they are the same sign and both more than 2-sigma out of bounds + // check if they are the same sign and both more than 2-sigma out of bounds if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 4.0f * (P[9][9] + R_OBS[5])) && (sq(velDErr) > 4.0f * (P[6][6] + R_OBS[2]))) { badIMUdata = true; } else { @@ -1610,10 +1627,10 @@ void NavEKF::FuseVelPosNED() K1 += R_hgt / (R_hgt + sq(velInnov1[i])); K2 += R_hgt / (R_hgt + sq(velInnov2[i])); } - // Calculate weighting used by fuseVelPosNED to do IMU accel data blending - // This is used to detect and compensate for aliasing errors with the accelerometers - // Provision for a first order lowpass filter to reduce noise on the weighting if required - IMU1_weighting = 1.0f * (K1 / (K1 + K2)) + 0.0f * IMU1_weighting; + // calculate weighting used by fuseVelPosNED to do IMU accel data blending + // this is used to detect and compensate for aliasing errors with the accelerometers + // provide for a first order lowpass filter to reduce noise on the weighting if required + IMU1_weighting = 1.0f * (K1 / (K1 + K2)) + 0.0f * IMU1_weighting; // filter currently inactive // apply an innovation consistency threshold test, but don't fail if bad IMU data velHealth = !((sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) > (sq(_gpsVelInnovGate) * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2])) && !badIMUdata); velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime; @@ -1693,8 +1710,7 @@ void NavEKF::FuseVelPosNED() hgtHealth = false; } } - // Set range for sequential fusion of velocity and position measurements depending - // on which data is available and its health + // set range for sequential fusion of velocity and position measurements depending on which data is available and its health if (fuseVelData && _fusionModeGPS == 0 && velHealth && !staticMode) { fuseData[0] = true; @@ -1725,14 +1741,13 @@ void NavEKF::FuseVelPosNED() indexLimit = 13; } - // Fuse measurements sequentially + // fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) { if (fuseData[obsIndex]) { stateIndex = 4 + obsIndex; - // Calculate the measurement innovation, using states from a - // different time coordinate if fusing height data + // calculate the measurement innovation, using states from a different time coordinate if fusing height data if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; @@ -1745,8 +1760,7 @@ void NavEKF::FuseVelPosNED() { innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; } - // Calculate the Kalman Gain - // Calculate innovation variances - also used for data logging + // calculate the Kalman gain and calculate innovation variances varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; SK = 1.0f/varInnovVelPos[obsIndex]; for (uint8_t i= 0; i<=indexLimit; i++) @@ -1795,8 +1809,8 @@ void NavEKF::FuseVelPosNED() Kfusion[13] = 0.0f; Kfusion[22] = 0.0f; } - // Calculate state corrections and re-normalise the quaternions for blended IMU data predicted states - // Don't update the Zacc bias state becasue it has already been updated + // calculate state corrections and re-normalise the quaternions for blended IMU data predicted states + // don't update the Zacc bias state because it has already been updated for (uint8_t i = 0; i<=indexLimit; i++) { if (i != 13) { @@ -1804,9 +1818,8 @@ void NavEKF::FuseVelPosNED() } } state.quat.normalize(); - // Update the covariance - take advantage of direct observation of a - // single state at index = stateIndex to reduce computations - // Optimised implementation of standard equation P = (I - K*H)*P; + // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations + // this is a numerically optimised implementation of standard equation P = (I - K*H)*P; for (uint8_t i= 0; i<=indexLimit; i++) { for (uint8_t j= 0; j<=indexLimit; j++) @@ -1825,17 +1838,22 @@ void NavEKF::FuseVelPosNED() } } - // force the covariance matrix to me symmetrical and limit the variances to prevent - // ill-condiioning. + // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); + // stop performance timer perf_end(_perf_FuseVelPosNED); } +// fuse magnetometer measurements and apply innovation consistency checks +// fuse each axis on consecutive time steps to spread computional load void NavEKF::FuseMagnetometer() { + // start performance timer perf_begin(_perf_FuseMagnetometer); + + // declarations ftype &q0 = mag_state.q0; ftype &q1 = mag_state.q1; ftype &q2 = mag_state.q2; @@ -1857,15 +1875,15 @@ void NavEKF::FuseMagnetometer() Vector6 SK_MZ; uint8_t indexLimit; // used to prevent access to wind and magnetic field states and variances when on ground - // Perform sequential fusion of Magnetometer measurements. - // This assumes that the errors in the different components are + // perform sequential fusion of magnetometer measurements. + // this assumes that the errors in the different components are // uncorrelated which is not true, however in the absence of covariance // data fit is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion if (fuseMagData || obsIndex == 1 || obsIndex == 2) { - // Prevent access last 9 states when on ground (acc bias, wind and magnetometer states). + // prevent access last 9 states when on ground (acc bias, wind and magnetometer states). if (!onGround) { indexLimit = 21; @@ -1874,10 +1892,10 @@ void NavEKF::FuseMagnetometer() { indexLimit = 12; } - // Calculate observation jacobians and Kalman gains + // calculate observation jacobians and Kalman gains if (fuseMagData) { - // Copy required states to local variable names + // copy required states to local variable names q0 = statesAtMagMeasTime[0]; q1 = statesAtMagMeasTime[1]; q2 = statesAtMagMeasTime[2]; @@ -1907,7 +1925,7 @@ void NavEKF::FuseMagnetometer() // scale magnetometer observation error with total angular rate R_MAG = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(_magVarRateScale*dAngIMU.length() / dtIMU); - // Calculate observation jacobians + // calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; @@ -1927,7 +1945,7 @@ void NavEKF::FuseMagnetometer() H_MAG[18] = 2*q1*q3 - 2*q0*q2; H_MAG[19] = 1; - // Calculate Kalman gain + // calculate Kalman gain SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; @@ -1947,7 +1965,7 @@ void NavEKF::FuseMagnetometer() Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); - // This term has been zeroed to improve stability of the Z accel bias + // this term has been zeroed to improve stability of the Z accel bias Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); @@ -1958,19 +1976,20 @@ void NavEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); - // Calculate the observation innovation variance + // calculate the observation innovation variance varInnovMag[0] = 1.0f/SK_MX[0]; - // reset the observation index to 0 (we start by fusing the X - // measurement) + // reset the observation index to 0 (we start by fusing the X measurement) obsIndex = 0; - // set flags to indicate to other processes that fusion has been perfomred and is required on the next time step + + // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step magFusePerformed = true; magFuseRequired = true; } else if (obsIndex == 1) // we are now fusing the Y measurement { - // Calculate observation jacobians + // calculate observation jacobians for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[2]; H_MAG[1] = SH_MAG[1]; @@ -1981,7 +2000,7 @@ void NavEKF::FuseMagnetometer() H_MAG[18] = 2*q0*q1 + 2*q2*q3; H_MAG[20] = 1; - // Calculate Kalman gain + // calculate Kalman gain SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; @@ -2000,7 +2019,7 @@ void NavEKF::FuseMagnetometer() Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); - // This term has been zeroed to improve stability of the Z accel bias + // this term has been zeroed to improve stability of the Z accel bias Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); @@ -2011,16 +2030,17 @@ void NavEKF::FuseMagnetometer() Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); - // Calculate the observation innovation variance + // calculate the observation innovation variance varInnovMag[1] = 1.0f/SK_MY[0]; - // set flags to indicate to other processes that fusion has been perfomred and is required on the next time step + // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step magFusePerformed = true; magFuseRequired = true; } else if (obsIndex == 2) // we are now fusing the Z measurement { - // Calculate observation jacobians + // calculate observation jacobians for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[1]; H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; @@ -2031,7 +2051,7 @@ void NavEKF::FuseMagnetometer() H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; H_MAG[21] = 1; - // Calculate Kalman gain + // calculate Kalman gain SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; @@ -2051,7 +2071,7 @@ void NavEKF::FuseMagnetometer() Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]); Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]); Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); - // This term has been zeroed to improve stability of the Z accel bias + // this term has been zeroed to improve stability of the Z accel bias Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); @@ -2062,16 +2082,17 @@ void NavEKF::FuseMagnetometer() Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); - // Calculate the observation innovation variance + // calculate the observation innovation variance varInnovMag[2] = 1.0f/SK_MZ[0]; - // set flags to indicate to other processes that fusion has been perfomred and is not required on the next time step + // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step magFusePerformed = true; magFuseRequired = false; } - // Calculate the measurement innovation + // calculate the measurement innovation innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex]; - // Apply and innovation consistency check + // apply and innovation consistency check if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < sq(_magInnovGate)) { // correct the state vector @@ -2136,22 +2157,27 @@ void NavEKF::FuseMagnetometer() } else { - // set flags to indicate to other processes that fusion has not been performed and is not required on the next time step + // set flags to indicate to other processes that fusion has not been performed and is not required on the next time step magFusePerformed = false; magFuseRequired = false; } - // force the covariance matrix to me symmetrical and limit the variances to prevent + // force the covariance matrix to be symmetrical and limit the variances to prevent // ill-condiioning. ForceSymmetry(); ConstrainVariances(); + // stop performance timer perf_end(_perf_FuseMagnetometer); } +// fuse true airspeed measurements void NavEKF::FuseAirspeed() { + // start performance timer perf_begin(_perf_FuseAirspeed); + + // declarations float vn; float ve; float vd; @@ -2164,19 +2190,19 @@ void NavEKF::FuseAirspeed() Vector22 H_TAS; float VtasPred; - // Copy required states to local variable names + // copy required states to local variable names vn = statesAtVtasMeasTime[4]; ve = statesAtVtasMeasTime[5]; vd = statesAtVtasMeasTime[6]; vwn = statesAtVtasMeasTime[14]; vwe = statesAtVtasMeasTime[15]; - // Calculate the predicted airspeed + // calculate the predicted airspeed VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd); - // Perform fusion of True Airspeed measurement + // perform fusion of True Airspeed measurement if (VtasPred > 1.0f) { - // Calculate observation jacobians + // calculate observation jacobians SH_TAS[0] = 1.0f/VtasPred; SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; @@ -2187,7 +2213,7 @@ void NavEKF::FuseAirspeed() H_TAS[14] = -SH_TAS[2]; H_TAS[15] = -SH_TAS[1]; - // Calculate Kalman gains + // calculate Kalman gains SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); @@ -2202,7 +2228,7 @@ void NavEKF::FuseAirspeed() Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); - // This term has been zeroed to improve stability of the Z accel bias + // this term has been zeroed to improve stability of the Z accel bias Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); @@ -2213,13 +2239,13 @@ void NavEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); - // Calculate measurement innovation variance + // calculate measurement innovation variance varInnovVtas = 1.0f/SK_TAS; - // Calculate measurement innovation + // calculate measurement innovation innovVtas = VtasPred - VtasMeas; - // Aplly an innovation consistency check + // apply a innovation consistency check if ((innovVtas*innovVtas*SK_TAS) < sq(_tasInnovGate)) { // correct the state vector @@ -2234,8 +2260,7 @@ void NavEKF::FuseAirspeed() states[i] = q[i]; } // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in H to reduce the - // number of operations + // take advantage of the empty columns in H to reduce the number of operations for (uint8_t i = 0; i<=21; i++) { for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; @@ -2275,17 +2300,21 @@ void NavEKF::FuseAirspeed() } } - // force the covariance matrix to me symmetrical and limit the variances to prevent - // ill-condiioning. + // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); + // stop performance timer perf_end(_perf_FuseAirspeed); } +// fuse sythetic sideslip measurement of zero void NavEKF::FuseSideslip() { + // start performance timer perf_begin(_perf_FuseSideslip); + + // declarations float q0; float q1; float q2; @@ -2295,14 +2324,14 @@ void NavEKF::FuseSideslip() float vd; float vwn; float vwe; - const float R_BETA = 0.03f; + const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg float SH_BETA[13]; float SK_BETA[8]; Vector3f vel_rel_wind; Vector22 H_BETA; float innovBeta; - // Copy required states to local variable names + // copy required states to local variable names q0 = states[0]; q1 = states[1]; q2 = states[2]; @@ -2313,15 +2342,15 @@ void NavEKF::FuseSideslip() vwn = states[14]; vwe = states[15]; - // Calculate predicted wind relative velocity in NED + // calculate predicted wind relative velocity in NED vel_rel_wind.x = vn - vwn; vel_rel_wind.y = ve - vwe; vel_rel_wind.z = vd; - // Rotate into body axes + // rotate into body axes vel_rel_wind = prevTnb * vel_rel_wind; - // Perform fusion of assumed sideslip constraint (sideslip = 0) + // perform fusion of assumed sideslip = 0 if (vel_rel_wind.x > 5.0f) { // Calculate observation jacobians @@ -2373,7 +2402,7 @@ void NavEKF::FuseSideslip() Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][14]*SK_BETA[1] - P[10][15]*SK_BETA[2]); Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][14]*SK_BETA[1] - P[11][15]*SK_BETA[2]); Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][14]*SK_BETA[1] - P[12][15]*SK_BETA[2]); - // This term has been zeroed to improve stability of the Z accel bias + // this term has been zeroed to improve stability of the Z accel bias Kfusion[13] = 0.0f;//SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][14]*SK_BETA[1] - P[13][15]*SK_BETA[2]); Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][14]*SK_BETA[1] - P[14][15]*SK_BETA[2]); Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][14]*SK_BETA[1] - P[15][15]*SK_BETA[2]); @@ -2384,7 +2413,7 @@ void NavEKF::FuseSideslip() Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][14]*SK_BETA[1] - P[20][15]*SK_BETA[2]); Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][14]*SK_BETA[1] - P[21][15]*SK_BETA[2]); - // Calculate predicted sideslip angle and innovation using small angle approximation and assuming zero sideslip + // calculate predicted sideslip angle and innovation using small angle approximation and assuming zero sideslip innovBeta = vel_rel_wind.y / vel_rel_wind.x; // correct the state vector @@ -2438,14 +2467,15 @@ void NavEKF::FuseSideslip() } } - // force the covariance matrix to me symmetrical and limit the variances to prevent - // ill-condiioning. + // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); + // stop the performance timer perf_end(_perf_FuseSideslip); } +// zero specified range of rows in the state covariance matrix void NavEKF::zeroRows(Matrix22 &covMat, uint8_t first, uint8_t last) { uint8_t row; @@ -2455,6 +2485,7 @@ void NavEKF::zeroRows(Matrix22 &covMat, uint8_t first, uint8_t last) } } +// zero specified range of columns in the state covariance matrix void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last) { uint8_t row; @@ -2464,7 +2495,7 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last) } } -// Store states in a history array along with time stamp +// store states in a history array along with time stamp void NavEKF::StoreStates() { if (storeIndex > 49) storeIndex = 0; @@ -2473,7 +2504,7 @@ void NavEKF::StoreStates() storeIndex = storeIndex + 1; } -// Reset the stored state history and store the current state +// reset the stored state history and store the current state void NavEKF::StoreStatesReset() { // clear stored state history @@ -2486,7 +2517,7 @@ void NavEKF::StoreStatesReset() storeIndex = storeIndex + 1; } -// Output the state vector stored at the time that best matches that specified by msec +// recall state vector stored at closest time to the one specified by msec void NavEKF::RecallStates(Vector31 &statesForFusion, uint32_t msec) { uint32_t timeDelta; @@ -2515,12 +2546,14 @@ void NavEKF::RecallStates(Vector31 &statesForFusion, uint32_t msec) } } +// calculate nav to body quaternions from body to nav rotation matrix void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const { // Calculate the body to nav cosine matrix quat.rotation_matrix(Tbn); } +// return the Euler roll, pitch and yaw angle in radians void NavEKF::getEulerAngles(Vector3f &euler) const { Quaternion q(states[0], states[1], states[2], states[3]); @@ -2528,6 +2561,7 @@ void NavEKF::getEulerAngles(Vector3f &euler) const euler = euler - _ahrs->get_trim(); } +// return NED velocity in m/s void NavEKF::getVelNED(Vector3f &vel) const { vel.x = states[4]; @@ -2535,6 +2569,8 @@ void NavEKF::getVelNED(Vector3f &vel) const vel.z = states[6]; } +// return the last calculated NED position relative to the reference point (m). +// return false if no position is available bool NavEKF::getPosNED(Vector3f &pos) const { pos.x = states[7]; @@ -2543,6 +2579,7 @@ bool NavEKF::getPosNED(Vector3f &pos) const return true; } +// return body axis gyro bias estimates in rad/sec void NavEKF::getGyroBias(Vector3f &gyroBias) const { gyroBias.x = states[10] / dtIMU; @@ -2550,6 +2587,7 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const gyroBias.z = states[12] / dtIMU; } +// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 void NavEKF::getAccelBias(Vector3f &accelBias) const { accelBias.x = IMU1_weighting; @@ -2557,6 +2595,7 @@ void NavEKF::getAccelBias(Vector3f &accelBias) const accelBias.z = states[13] / dtIMU; } +// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) void NavEKF::getWind(Vector3f &wind) const { wind.x = states[14]; @@ -2564,6 +2603,7 @@ void NavEKF::getWind(Vector3f &wind) const wind.z = 0.0f; // curently don't estimate this } +// return earth magnetic field estimates in measurement units / 1000 void NavEKF::getMagNED(Vector3f &magNED) const { magNED.x = states[16]*1000.0f; @@ -2571,6 +2611,7 @@ void NavEKF::getMagNED(Vector3f &magNED) const magNED.z = states[18]*1000.0f; } +// return body magnetic field estimates in measurement units / 1000 void NavEKF::getMagXYZ(Vector3f &magXYZ) const { magXYZ.x = states[19]*1000.0f; @@ -2578,6 +2619,7 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const magXYZ.z = states[21]*1000.0f; } +// return the last calculated latitude, longitude and height bool NavEKF::getLLH(struct Location &loc) const { loc.lat = _ahrs->get_home().lat; @@ -2587,6 +2629,7 @@ bool NavEKF::getLLH(struct Location &loc) const return true; } +// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed void NavEKF::OnGroundCheck() { const AP_Airspeed *airspeed = _ahrs->get_airspeed(); @@ -2618,10 +2661,9 @@ void NavEKF::OnGroundCheck() prevOnGround = onGround; } +// initialise the covariance matrix void NavEKF::CovarianceInit(float roll, float pitch, float yaw) { - //TODO better maths for initial quaternion covariances - // that uses roll, pitch and yaw // zero the matrix for (uint8_t i=1; i<=21; i++) { @@ -2630,42 +2672,41 @@ void NavEKF::CovarianceInit(float roll, float pitch, float yaw) P[i][j] = 0.0f; } } - // Quaternions + // quaternions - TODO better maths for initial quaternion covariances that uses roll, pitch and yaw P[0][0] = 1.0e-9f; P[1][1] = 0.25f*sq(radians(1.0f)); P[2][2] = 0.25f*sq(radians(1.0f)); P[3][3] = 0.25f*sq(radians(1.0f)); - // Velocities + // velocities P[4][4] = sq(0.7f); P[5][5] = P[4][4]; P[6][6] = sq(0.7f); - // Positions + // positions P[7][7] = sq(15.0f); P[8][8] = P[7][7]; P[9][9] = sq(5.0f); - // Delta angle biases + // delta angle biases P[10][10] = sq(radians(0.1f * dtIMU)); P[11][11] = P[10][10]; P[12][12] = P[10][10]; - //Z delta velocity Bbias + // Z delta velocity bias P[13][13] = sq(radians(0.5f * dtIMU)); - // Wind velocities + // wind velocities P[14][14] = sq(8.0f); P[15][15] = P[14][14]; - // NED magnetic field + // earth magnetic field P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; - // XYZ magnetic field + // body magnetic field P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; } +// force symmetry on the covariance matrix to prevent ill-conditioning void NavEKF::ForceSymmetry() { - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up for (uint8_t i=1; i<=21; i++) { for (uint8_t j=0; j<=i-1; j++) @@ -2677,9 +2718,10 @@ void NavEKF::ForceSymmetry() } } +// copy covariances across from covariance prediction calculation and fix numerical errors void NavEKF::CopyAndFixCovariances() { - // if we are in ground or static mode, we want all the off-diagonals for the wind + // if we are on-ground or in static mode, we want all the off-diagonals for the wind // and magnetic field states to remain zero and want to keep the old variances // for these states if (onGround || staticMode) { @@ -2725,7 +2767,7 @@ void NavEKF::CopyAndFixCovariances() } } } - // if flying with all sensors all covariance terms are active + // if we are flying with all sensors then all covariance terms are active else { // copy calculated variances we want to propagate for (uint8_t i=0; i<=21; i++) { @@ -2742,27 +2784,26 @@ void NavEKF::CopyAndFixCovariances() } } +// constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning void NavEKF::ConstrainVariances() { - // Constrain variances to be within set limits - for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); - for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); - for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); - for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMU)); - P[13][13] = constrain_float(P[13][13],0.0f,sq(10.0f * dtIMU)); - for (uint8_t i=14; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); - for (uint8_t i=16; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); + for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // quaternions + for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities + for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); // positions + for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMU)); // delta angle biases + P[13][13] = constrain_float(P[13][13],0.0f,sq(10.0f * dtIMU)); // delta velocity bias + for (uint8_t i=14; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // earth magnetic field + for (uint8_t i=16; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // body magnetic field } +// constrain states to prevent ill-conditioning void NavEKF::ConstrainStates() { - // Constrain states to be within set limits - // Quaternions + // quaternions are limited between +-1 for (uint8_t i=0; i<=3; i++) states[i] = constrain_float(states[i],-1.0f,1.0f); // velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS) for (uint8_t i=4; i<=6; i++) states[i] = constrain_float(states[i],-5.0e2f,5.0e2f); - // position limit 1000 km - //TODO apply circular limit + // position limit 1000 km - TODO apply circular limit for (uint8_t i=7; i<=8; i++) states[i] = constrain_float(states[i],-1.0e6f,1.0e6f); // height limit covers home alt on everest through to home alt at SL and ballon drop states[9] = constrain_float(states[9],-4.0e4f,1.0e4f); @@ -2771,24 +2812,27 @@ void NavEKF::ConstrainStates() // Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data) states[13] = constrain_float(states[13],-1.0f*dtIMU,1.0f*dtIMU); states[22] = constrain_float(states[22],-1.0f*dtIMU,1.0f*dtIMU); - // Wind Limit 100 m/s (should be based on some multiple of max airspeed * EAS2TAS) - //TODO apply circular limit + // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit for (uint8_t i=14; i<=15; i++) states[i] = constrain_float(states[i],-100.0f,100.0f); - // Earth Field limit 1000 mGauss + // earth magnetic field limit for (uint8_t i=16; i<=18; i++) states[i] = constrain_float(states[i],-1.0f,1.0f); - // Body Field limit 500 mGauss + // body magnetic field limit for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f); } +// update IMU delta angle and delta velocity measurements void NavEKF::readIMUData() { - Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) - Vector3f accel1; // acceleration vector in XYZ body axes measured by IMU1 (m/s^2) - Vector3f accel2; // acceleration vector in XYZ body axes measured by IMU2 (m/s^2) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f accel1; // acceleration vector in XYZ body axes measured by IMU1 (m/s^2) + Vector3f accel2; // acceleration vector in XYZ body axes measured by IMU2 (m/s^2) + + // get the time the IMU data was read + IMUmsec = hal.scheduler->millis(); + + // limit IMU delta time to prevent numerical problems elsewhere + dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f); - IMUmsec = hal.scheduler->millis(); - // Limit IMU delta time to prevent numerical problems elsewhere - dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f); // get accels and gyro data from dual sensors if healthy if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) { accel1 = _ahrs->get_ins().get_accel(0); @@ -2820,22 +2864,33 @@ void NavEKF::readIMUData() lastAccel2 = accel2; } +// check for new valid GPS data and update stored measurement if available void NavEKF::readGpsData() { + // check for new GPS data if ((_ahrs->get_gps()->last_message_time_ms() != lastFixTime_ms) && (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D)) { + // store fix time from previous read secondLastFixTime_ms = lastFixTime_ms; + + // get current fix time lastFixTime_ms = _ahrs->get_gps()->last_message_time_ms(); + + // set flag that lets other functions know that new GPS data has arrived newDataGps = true; + + // get state vectors that were stored at the time that is closest to when the the GPS measurement + // time after accounting for measurement delays RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500))); RecallStates(statesAtPosTime, (IMUmsec - constrain_int16(_msecPosDelay, 0, 500))); - velNED[0] = _ahrs->get_gps()->velocity_north(); // (rad) - velNED[1] = _ahrs->get_gps()->velocity_east(); // (m/s) - velNED[2] = _ahrs->get_gps()->velocity_down(); // (m/s) - //::printf("GPSVEL=(%.2f,%.2f,%.2f)\n", velNED[0], velNED[1], velNED[2]); - // Convert GPS measurements to Pos NE + // read the NED velocity from the GPS + velNED[0] = _ahrs->get_gps()->velocity_north(); + velNED[1] = _ahrs->get_gps()->velocity_east(); + velNED[2] = _ahrs->get_gps()->velocity_down(); + + // read latitutde and longitude from GPS and convert to NE position struct Location gpsloc; gpsloc.lat = _ahrs->get_gps()->latitude; gpsloc.lng = _ahrs->get_gps()->longitude; @@ -2845,40 +2900,57 @@ void NavEKF::readGpsData() } } +// check for new altitude measurement data and update stored measurement if available void NavEKF::readHgtData() { + // check to see if baro measurement has changed so we know if a new measurement has arrived if (_baro.get_last_update() != lastHgtMeasTime) { // time stamp used to check for new measurement lastHgtMeasTime = _baro.get_last_update(); + // time stamp used to check for timeout lastHgtTime_ms = hal.scheduler->millis(); + + // get measurement and set flag to let other functions know new data has arrived hgtMea = _baro.get_altitude(); newDataHgt = true; - // recall states at effective measurement time + + // get states that wer stored at the time closest to the measurement time, taking measurement delay into account RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay)); } else { newDataHgt = false; } } +// check for new magnetometer data and update store measurements if available void NavEKF::readMagData() { - // scale compass data to improve numerical conditioning if (use_compass() && _ahrs->get_compass()->last_update != lastMagUpdate) { + // store time of last measurement update lastMagUpdate = _ahrs->get_compass()->last_update; - // Body fixed magnetic bias is opposite sign to APM compass offsets + + // read compass data and assign to bias and uncorrected measurement + // body fixed magnetic bias is opposite sign to APM compass offsets + // we scale compass data to improve numerical conditioning magBias = -_ahrs->get_compass()->get_offsets() * 0.001f; magData = _ahrs->get_compass()->get_field() * 0.001f + magBias; - // Recall states from compass measurement time + + // get states stored at time closest to measurement time after allowance for measurement delay RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay)); + + // let other processes know that new compass data has arrived newDataMag = true; } else { newDataMag = false; } } +// check for new airspeed data and update stored measurements if available void NavEKF::readAirSpdData() { + // if airspeed reading is valid and is set by the user to be used and has been updated then + // we take a new reading, convert from EAS to TAS and set the flag letting other functions + // know a new measurement is available const AP_Airspeed *aspeed = _ahrs->get_airspeed(); if (aspeed && aspeed->use() && @@ -2892,6 +2964,7 @@ void NavEKF::readAirSpdData() } } +// calculate the NED earth spin vector in rad/sec void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const { float lat_rad = radians(latitude*1.0e-7f); @@ -2900,10 +2973,8 @@ void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const omega.z = -earthRate*sinf(lat_rad); } -/* -This function is used to do a forced alignment of the yaw angle to aligwith the horizontal velocity -vector from GPS. It is used to align the yaw angle after launch or takeoff without a magnetometer. -*/ +// this function is used to do a forced alignment of the yaw angle to aligwith the horizontal velocity +// vector from GPS. It is used to align the yaw angle after launch or takeoff without a magnetometer. void NavEKF::ForceYawAlignment() { if ((sq(velNED[0]) + sq(velNED[1])) > 16.0f) { @@ -2932,21 +3003,20 @@ void NavEKF::ForceYawAlignment() states[4] = velNED[0]; states[5] = velNED[1]; } - // Reinitialise the quaternion, velocity and position covariances + // reinitialise the quaternion, velocity and position covariances // zero the matrix entries zeroRows(P,0,9); zeroCols(P,0,9); - // Quaternions - // TODO - maths that sets them based on different roll, yaw and pitch uncertainties + // quaternions - TODO maths that sets them based on different roll, yaw and pitch uncertainties P[0][0] = 1.0e-9f; P[1][1] = 0.25f*sq(radians(1.0f)); P[2][2] = 0.25f*sq(radians(1.0f)); P[3][3] = 0.25f*sq(radians(1.0f)); - // Velocities - we could have a big error coming out of static mode due to GPS lag + // velocities - we could have a big error coming out of static mode due to GPS lag P[4][4] = 400.0f; P[5][5] = P[4][4]; P[6][6] = sq(0.7f); - // Positions - we could have a big error coming out of static mode due to GPS lag + // positions - we could have a big error coming out of static mode due to GPS lag P[7][7] = 400.0f; P[8][8] = P[7][7]; P[9][9] = sq(5.0f); @@ -2954,6 +3024,7 @@ void NavEKF::ForceYawAlignment() } } +// return the transformation matrix from XYZ (body) to NED axes void NavEKF::getRotationBodyToNED(Matrix3f &mat) const { Vector3f trim = _ahrs->get_trim(); @@ -2962,6 +3033,7 @@ void NavEKF::getRotationBodyToNED(Matrix3f &mat) const mat.rotateXYinv(trim); } +// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements void NavEKF::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov) const { velInnov.x = innovVelPos[0]; @@ -2976,6 +3048,7 @@ void NavEKF::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &m tasInnov = innovVtas; } +// return the innovation variances for the NED Pos, NED Vel, XYZ Mag and Vtas measurements void NavEKF::getVariances(Vector3f &velVar, Vector3f &posVar, Vector3f &magVar, float &tasVar) const { velVar.x = varInnovVelPos[0]; @@ -2990,6 +3063,7 @@ void NavEKF::getVariances(Vector3f &velVar, Vector3f &posVar, Vector3f &magVar, tasVar = varInnovVtas; } +// zero stored variables - this needs to be called before a full filter initialisation void NavEKF::ZeroVariables() { velTimeout = false; @@ -3031,6 +3105,7 @@ void NavEKF::ZeroVariables() memset(&posNE[0], 0, sizeof(posNE)); } +// return true if we should use the airspeed sensor bool NavEKF::useAirspeed(void) const { if (_ahrs->get_airspeed() == NULL) { @@ -3039,17 +3114,15 @@ bool NavEKF::useAirspeed(void) const return _ahrs->get_airspeed()->use(); } -/* - see if the vehicle code has demanded static mode - */ +// return true if the vehicle code has requested use of static mode +// in static mode, position and height are constrained to zero, allowing an attitude +// reference to be initialised and maintained when on the ground and without GPS lock bool NavEKF::static_mode_demanded(void) const { return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal(); } -/* - see if we should use the compass - */ +// return true if we should use the compass bool NavEKF::use_compass(void) const { return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(); diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 4411343b33..8a6bde3921 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -75,8 +75,8 @@ public: // Constructor NavEKF(const AP_AHRS *ahrs, AP_Baro &baro); - // Initialise the filter states from the AHRS and magnetometer data (if present) - // This method can be used when the vehicle is moving + // This function is used to initialise the filter whilst moving, using the AHRS DCM solution + // It should NOT be used to re-initialise after a timeout as DCM will also be corrupted void InitialiseFilterDynamic(void); // Initialise the states from accelerometer and magnetometer data (if present) @@ -86,7 +86,7 @@ public: // Update Filter States - this should be called whenever new IMU data is available void UpdateFilter(void); - // return true if the filter is healthy + // Check basic filter health metrics and return a consolidated health status bool healthy(void) const; // return true if filter is dead-reckoning height @@ -95,33 +95,26 @@ public: // return true if filter is dead-reckoning position bool PositionDrifting(void) const; - // fill in latitude, longitude and height of the reference point - void getRefLLH(struct Location &loc) const; - - // set latitude, longitude and height of the reference point - void setRefLLH(int32_t lat, int32_t lng, int32_t alt_cm); - - // return the last calculated NED position relative to the - // reference point (m). Return false if no position is available + // return the last calculated NED position relative to the reference point (m). + // return false if no position is available bool getPosNED(Vector3f &pos) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; - // return bodyaxis gyro bias estimates in deg/hr + // return body axis gyro bias estimates in rad/sec void getGyroBias(Vector3f &gyroBias) const; - // return body axis accelerometer bias estimates in m/s^2 + // return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 void getAccelBias(Vector3f &accelBias) const; - // return the NED wind speed estimates in m/s - // positive is air moving in the direction of the corresponding axis + // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) void getWind(Vector3f &wind) const; - // return earth magnetic field estimates in measurement units + // return earth magnetic field estimates in measurement units / 1000 void getMagNED(Vector3f &magNED) const; - // return body magnetic field estimates in measurement units + // return body magnetic field estimates in measurement units / 1000 void getMagXYZ(Vector3f &magXYZ) const; // return the last calculated latitude, longitude and height @@ -130,13 +123,10 @@ public: // return the Euler roll, pitch and yaw angle in radians void getEulerAngles(Vector3f &eulers) const; - // get the transformation matrix from NED to XYD (body) axes - void getRotationNEDToBody(Matrix3f &mat) const; - - // get the transformation matrix from XYZ (body) to NED axes + // return the transformation matrix from XYZ (body) to NED axes void getRotationBodyToNED(Matrix3f &mat) const; - // get the quaternions defining the rotation from NED to XYZ (body) axes + // return the quaternions defining the rotation from NED to XYZ (body) axes void getQuaternion(Quaternion &quat) const; // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements @@ -163,7 +153,7 @@ private: // copy covariances across from covariance prediction calculation and fix numerical errors void CopyAndFixCovariances(); - // constrain variances (diagonal terms) on the state covariance matrix + // constrain variances (diagonal terms) in the state covariance matrix void ConstrainVariances(); // constrain states @@ -178,7 +168,7 @@ private: // fuse true airspeed measurements void FuseAirspeed(); - // fuse sideslip measurements + // fuse sythetic sideslip measurement of zero void FuseSideslip(); // zero specified range of rows in the state covariance matrix @@ -187,9 +177,6 @@ private: // zero specified range of columns in the state covariance matrix void zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last); - // normalise the quaternion states - void quatNorm(Quaternion &quatOut, const Quaternion &quatIn) const; - // store states along with system time stamp in msces void StoreStates(void); @@ -202,13 +189,10 @@ private: // calculate nav to body quaternions from body to nav rotation matrix void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const; - // calculate the earth spin vector in NED axes + // calculate the NED earth spin vector in rad/sec void calcEarthRateNED(Vector3f &omega, int32_t latitude) const; - // calculate a NED velocity vector from GPS speed, course and down velocity - void calcvelNED(Vector3f &velNED, float gpsCourse, float gpsGndSpd, float gpsVelD) const; - - // calculate from height, airspeed and ground speed whether the flight vehicle is on the ground or flying + // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed void OnGroundCheck(); // initialise the covariance matrix @@ -232,9 +216,6 @@ private: // determine when to perform fusion of GPS position and velocity measurements void SelectVelPosFusion(); - // determine when to perform fusion of height measurements - void SelectHgtFusion(); - // determine when to perform fusion of true airspeed measurements void SelectTasFusion(); @@ -262,7 +243,9 @@ private: // return true if we should use the airspeed sensor bool useAirspeed(void) const; - // check if static mode has been demanded by vehicle code + // return true if the vehicle code has requested use of static mode + // in static mode, position and height are constrained to zero, allowing an attitude + // reference to be initialised and maintained when on the ground and without GPS lock bool static_mode_demanded(void) const; private: