From cd418c946c11bbf54f05586cfd5bbf375bf0b61d Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 25 Aug 2014 17:58:42 +1000 Subject: [PATCH] AP_NavEKF : preliminary implementation for optical flow and range finder fusion Range finder measurements are not input to EKF at this time, however the method for fusing them is implemented. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 728 +++++++++++++++++++++++++++++- libraries/AP_NavEKF/AP_NavEKF.h | 117 ++++- 2 files changed, 828 insertions(+), 17 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 3de803e4f1..bcc68d975f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -301,6 +301,46 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("GLITCH_RAD", 24, NavEKF, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT), + // @Param: GND_GRADIENT + // @DisplayName: Terrain Gradient % RMS + // @Description: This parameter sets the RMS terrain gradient percentage assumed by the terrain height estimation. Terrain height can be estimated using optical flow and/or range finder sensor data if fitted. Smaller values cause the terrain height estimate to be slower to respond to changes in measurement. Larger values casue the terrain height estimate to be faster to respond, but also more noisy. Generally this value can be reduced if operating over very flat terrain and increased if operating over uneven terrain. + // @Range: 1 - 50 + // @Increment: 1 + // @User: advanced + AP_GROUPINFO("GND_GRADIENT", 25, NavEKF, _gndGradientSigma, 2), + + // @Param: FLOW_NOISE + // @DisplayName: Optical flow measurement noise (rad/s) + // @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements. + // @Range: 0.05 - 1.0 + // @Increment: 0.05 + // @User: advanced + AP_GROUPINFO("FLOW_NOISE", 26, NavEKF, _flowNoise, 0.3f), + + // @Param: FLOW_GATE + // @DisplayName: Optical Flow measurement gate size + // @Description: This parameter sets the number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 1 - 100 + // @Increment: 1 + // @User: advanced + AP_GROUPINFO("FLOW_GATE", 27, NavEKF, _flowInnovGate, 3), + + // @Param: FLOW_DELAY + // @DisplayName: Optical Flow measurement delay (msec) + // @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. + // @Range: 0 - 500 + // @Increment: 10 + // @User: advanced + AP_GROUPINFO("FLOW_DELAY", 28, NavEKF, _msecFLowDelay, 20), + + // @Param: RNG_GATE + // @DisplayName: Range finder measurement gate size + // @Description: This parameter sets the number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 1 - 100 + // @Increment: 1 + // @User: advanced + AP_GROUPINFO("RNG_GATE", 29, NavEKF, _rngInnovGate, 5), + // @Param: FALLBACK // @DisplayName: Fallback strictness // @Description: This parameter controls the conditions necessary to trigger a fallback to DCM and INAV. A value of 1 will cause fallbacks to occur on loss of GPS and other conditions. A value of 0 will trust the EKF more. @@ -320,7 +360,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates TASmsecMax(200), // maximum allowed interval between airspeed measurement updates - fuseMeNow(false), // forces airspeed and sythetic sideslip fusion to occur on the IMU frame that data arrives staticMode(true), // staticMode forces position and velocity fusion with zero values prevStaticMode(true), // staticMode from previous filter update yawAligned(false), // set true when heading or yaw angle has been aligned @@ -350,20 +389,27 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec) _hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec) _hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec) - _magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) + _magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) _magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate _gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground _msecGpsAvg = 200; // average number of msec between GPS measurements _msecHgtAvg = 100; // average number of msec between height measurements _msecMagAvg = 100; // average number of msec between magnetometer measurements _msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements + _msecBetaMax = 200; // maximum number of msec between synthetic sideslip measurements dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval. + // Misc initial conditions hgtRate = 0.0f; mag_state.q0 = 1; mag_state.DCM.identity(); IMU1_weighting = 0.5f; memset(&faultStatus, 0, sizeof(faultStatus)); + // variables added for optical flow fusion + DCM33FlowMin = 0.71f; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. + fScaleFactorPnoise = 1e-10f; // Process noise added to focal length scale factor state variance at each time step + flowTimeDeltaAvg_ms = 100; // average interval between optical flow measurements (msec) + flowIntervalMax_ms = 200; // maximum allowable time between flow fusion events } // Check basic filter health metrics and return a consolidated health status @@ -480,6 +526,7 @@ void NavEKF::ResetHeight(void) for (uint8_t i=0; i<=49; i++){ storedStates[i].position.z = -hgtMea; } + flowStates[1] = states[9] + 0.5f; } // this function is used to initialise the filter whilst moving, using the AHRS DCM solution @@ -495,7 +542,14 @@ void NavEKF::InitialiseFilterDynamic(void) // get initial time deltat between IMU measurements (sec) dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); + dtIMUinv = 1.0f / dtIMU; + // If we have a high rate update step of >100 Hz, then there may not enough time to all the fusion steps at once, so load levelling is used + if (dtIMU < 0.009f) { + inhibitLoadLeveling = false; + } else { + inhibitLoadLeveling = true; + } // set number of updates over which gps and baro measurements are applied to the velocity and position states gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); @@ -517,6 +571,9 @@ void NavEKF::InitialiseFilterDynamic(void) ResetHeight(); state.body_magfield.zero(); + // set stored states to current state + StoreStatesReset(); + // set to true now that states have be initialised statesInitialised = true; @@ -539,6 +596,14 @@ void NavEKF::InitialiseFilterBootstrap(void) // get initial time deltat between IMU measurements (sec) dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); + dtIMUinv = 1.0f / dtIMU; + + // If we have a high rate update step of >100 Hz, then there may not enough time to all the fusion steps at once, so load levelling is used + if (dtIMU < 0.009f) { + inhibitLoadLeveling = false; + } else { + inhibitLoadLeveling = true; + } // set number of updates over which gps and baro measurements are applied to the velocity and position states gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); @@ -590,6 +655,9 @@ void NavEKF::InitialiseFilterBootstrap(void) state.wind_vel.zero(); state.body_magfield.zero(); + // set stored states to current state + StoreStatesReset(); + // set to true now we have intialised the states statesInitialised = true; @@ -614,6 +682,9 @@ void NavEKF::UpdateFilter() // start the timer used for load measurement perf_begin(_perf_UpdateFilter); + //get starting time for update step + imuSampleTime_ms = hal.scheduler->millis(); + // read IMU data and convert to delta angles and velocities readIMUData(); @@ -687,6 +758,7 @@ void NavEKF::UpdateFilter() // Update states using GPS, altimeter, compass, airspeed and synthetic sideslip observations SelectVelPosFusion(); SelectMagFusion(); + SelectFlowFusion(); SelectTasFusion(); SelectBetaFusion(); @@ -802,8 +874,10 @@ void NavEKF::SelectMagFusion() fuseMagData = false; } - // call the function that performs fusion of magnetometer data - FuseMagnetometer(); + // delay if covariance prediction is being performed on this prediction cycle unless load levelling is inhibited + if (!covPredStep || inhibitLoadLeveling) { + FuseMagnetometer(); + } // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output if (magUpdateCount < magUpdateCountMax) { @@ -812,6 +886,49 @@ void NavEKF::SelectMagFusion() states[i] += magIncrStateDelta[i]; } } +} + +// select fusion of optical flow measurements +void NavEKF::SelectFlowFusion() +{ + // Apply tilt check + bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin); + // if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading + bool timeout = ((imuSampleTime_ms - prevFlowFusionTime_ms) >= flowIntervalMax_ms); + // check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature. + bool delayFusion = ((covPredStep || magFusePerformed) && !(timeout || inhibitLoadLeveling)); + // if the filter is initialised, we have data to fuse and the vehicle is not excessively tilted, then perform optical flow fusion + if (useOptFlow() && newDataFlow && tiltOK && !delayFusion) + { + // Set the flow noise used by the fusion processes + R_LOS = sq(max(_flowNoise, 0.05f)); + // Fuse the optical flow X axis data into the main filter + flow_state.obsIndex = 0; + FuseOptFlow(); + // increment the index to fuse the Y axis data on the next prediction cycle + flow_state.obsIndex = 1; + // reset flag to indicate that no new flow data is available for fusion + newDataFlow = false; + // indicate that flow fusion has been performed. This is used for load spreading. + flowFusePerformed = true; + // update the time stamp + prevFlowFusionTime_ms = imuSampleTime_ms; + } else if (flow_state.obsIndex == 1 && !delayFusion){ + // Fuse the optical flow Y axis data into the main filter + FuseOptFlow(); + // increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle + flow_state.obsIndex = 2; + // indicate that flow fusion has been performed. This is used for load spreading. + flowFusePerformed = true; + } else if (flow_state.obsIndex == 2) { + // Estimate the focal length scale factor (runs a two state EKF) + OpticalFlowEKF(); + // increment the index so that no further flow fusion is performed using this measurement + flow_state.obsIndex = 3; + // clear the flag indicating that flow fusion has been performed. The 2-state fusion is relatively computationally + // cheap and can be perfomred on the same prediction cycle with subsequent fusion steps + flowFusePerformed = false; + } } @@ -823,14 +940,12 @@ void NavEKF::SelectTasFusion() // if the filter is initialised, wind states are not inhibited and we have data to fuse, then queue TAS fusion tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas)); - // if we have waited too long, set a timeout flag which will force fusion to occur bool timeout = ((imuSampleTime_ms - TASmsecPrev) >= TASmsecMax); - // 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)) + if (tasDataWaiting && (!(covPredStep || magFusePerformed || flowFusePerformed) || timeout || inhibitLoadLeveling)) { FuseAirspeed(); TASmsecPrev = imuSampleTime_ms; @@ -844,7 +959,7 @@ void NavEKF::SelectTasFusion() void NavEKF::SelectBetaFusion() { // set to true if fusion is locked out due to magnetometer fusion on the same time step (done for load levelling) - bool f_lockedOut = (magFusePerformed && !fuseMeNow); + bool f_lockedOut = (magFusePerformed && !inhibitLoadLeveling); // set true when the fusion time interval has triggered bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= _msecBetaAvg); // set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position @@ -955,6 +1070,9 @@ void NavEKF::UpdateStrapdownEquationsNED() state.posD1 += (state.vel1.z + lastVel1.z) * (dtIMU*0.5f); state.posD2 += (state.vel2.z + lastVel2.z) * (dtIMU*0.5f); + // capture current angular rate to augmented state vector for use by optical flow fusion + state.omega = correctedDelAng * dtIMUinv; + // limit states to protect against divergence ConstrainStates(); } @@ -2375,6 +2493,486 @@ void NavEKF::FuseMagnetometer() perf_end(_perf_FuseMagnetometer); } +/* +Estimation of optical flow sensor focal length scale factor and terrain offset using a two state EKF +This fiter requires optical flow rates that are not motion compensated +*/ +void NavEKF::OpticalFlowEKF() +{ + // start performance timer + perf_begin(_perf_OpticalFlowEKF); + + // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption + // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning + if (!inhibitGndState) { + float distanceTravelledSq; + if (fuseRngData) { + distanceTravelledSq = sq(statesAtRngTime.position[0] - prevPosN) + sq(statesAtRngTime.position[1] - prevPosE); + prevPosN = statesAtRngTime.position[0]; + prevPosE = statesAtRngTime.position[1]; + } else if (fuseOptFlowData) { + distanceTravelledSq = sq(statesAtFlowTime.position[0] - prevPosN) + sq(statesAtFlowTime.position[1] - prevPosE); + prevPosN = statesAtFlowTime.position[0]; + prevPosE = statesAtFlowTime.position[1]; + } else { + return; + } + distanceTravelledSq = min(distanceTravelledSq, 100.0f); + Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))); + } + + // fuse range finder data + if (fuseRngData) { + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors + + // Copy required states to local variable names + q0 = statesAtRngTime.quat[0]; + q1 = statesAtRngTime.quat[1]; + q2 = statesAtRngTime.quat[2]; + q3 = statesAtRngTime.quat[3]; + + // calculate Kalman gains + float SK_RNG[3]; + SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0])); + SK_RNG[2] = 1/SK_RNG[0]; + float K_RNG[2]; + if (!fScaleInhibit) { + K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2]; + } else { + K_RNG[0] = 0.0f; + } + if (!inhibitGndState) { + K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2]; + } else { + K_RNG[1] = 0.0f; + } + + // Calculate the innovation variance for data logging + varInnovRng = 1.0f/SK_RNG[1]; + + // constrain terrain height to be below the vehicle + flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.5f); + + // estimate range to centre of image + range = (flowStates[1] - statesAtRngTime.position[2]) * SK_RNG[2]; + + // Calculate the measurement innovation + innovRng = range - rngMea; + + // calculate the innovation consistency test ratio + auxRngTestRatio = sq(innovRng) / (sq(_rngInnovGate) * varInnovRng); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((sq(innovRng)*SK_RNG[1]) < 25.0f) + { + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_RNG[i] * innovRng; + } + // constrain the states + flowStates[0] = constrain_float(flowStates[0], 0.1f, 10.0f); + flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.5f); + + // correct the covariance matrix + float nextPopt[2][2]; + nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2]; + nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2]; + nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); + nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = max(nextPopt[0][0],0.0f); + Popt[1][1] = max(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + } + } + + + if (fuseOptFlowData) { + Vector3f vel; // velocity of sensor relative to ground in NED axes + Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes + float losPred[2]; // predicted optical flow angular rate measurements + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + + // propagate scale factor state noise + if (!fScaleInhibit) { + Popt[0][0] += 1e-9f; + } else { + Popt[0][0] = 0.0f; + } + + // Copy required states to local variable names + q0 = statesAtFlowTime.quat[0]; + q1 = statesAtFlowTime.quat[1]; + q2 = statesAtFlowTime.quat[2]; + q3 = statesAtFlowTime.quat[3]; + vel.x = statesAtFlowTime.velocity[0]; + vel.y = statesAtFlowTime.velocity[1]; + vel.z = statesAtFlowTime.velocity[2]; + + // constrain terrain height to be below the vehicle + flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.5f); + + // estimate range to centre of image + range = (flowStates[1] - statesAtFlowTime.position[2]) / Tnb_flow.c.z; + + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow*vel; + + // divide velocity by range, subtract body rates and apply scale factor to + // get predicted sensed angular optical rates relative to X and Y sensor axes + losPred[0] = flowStates[0]*( relVelSensor.y/range) - omegaAcrossFlowTime.x; + losPred[1] = flowStates[0]*(-relVelSensor.x/range) - omegaAcrossFlowTime.y; + + // calculate innovations + auxFlowObsInnov[0] = losPred[0] - flowRadXY[0]; + auxFlowObsInnov[1] = losPred[1] - flowRadXY[1]; + + // calculate Kalman gains + float SH_OPT[6]; + SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3); + SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3); + SH_OPT[3] = statesAtFlowTime.position[2] - flowStates[1]; + SH_OPT[4] = 1/sq(SH_OPT[3]); + SH_OPT[5] = 1/SH_OPT[3]; + float SK_OPT[3]; + SK_OPT[0] = 1.0f/(R_LOS + SH_OPT[0]*SH_OPT[1]*SH_OPT[5]*(Popt[0][0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[5] + Popt[1][0]*flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4]) + flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4]*(Popt[0][1]*SH_OPT[0]*SH_OPT[1]*SH_OPT[5] + Popt[1][1]*flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4])); + SK_OPT[1] = 1.0f/(R_LOS + SH_OPT[0]*SH_OPT[2]*SH_OPT[5]*(Popt[0][0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[5] + Popt[1][0]*flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4]) + flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4]*(Popt[0][1]*SH_OPT[0]*SH_OPT[2]*SH_OPT[5] + Popt[1][1]*flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4])); + SK_OPT[2] = SH_OPT[0]; + float K_OPT[2][2]; + if (!fScaleInhibit) { + K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[0][1] = SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[0][0] = 0.0f; + K_OPT[0][1] = 0.0f; + } + if (!inhibitGndState) { + K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[1][1] = SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[1][0] = 0.0f; + K_OPT[1][1] = 0.0f; + } + + + // calculate innovation variances + auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1]; + auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0]; + + // Check the innovation for consistency and don't fuse if > threshold + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + + // calculate the innovation consistency test ratio + auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(_flowInnovGate) * auxFlowObsInnovVar[obsIndex]); + + if (auxFlowTestRatio[obsIndex] < 1.0f) { + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; + } + // constrain the states + flowStates[0] = constrain_float(flowStates[0], 0.1f, 10.0f); + flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.5f); + + // correct the covariance matrix + float nextPopt[2][2]; + if (obsIndex == 0) { + nextPopt[0][0] = - Popt[0][0]*(SH_OPT[0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[1][0]*flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + nextPopt[0][1] = - Popt[0][1]*(SH_OPT[0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[1][1]*flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + nextPopt[1][0] = - Popt[1][0]*(flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[0][0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + nextPopt[1][1] = - Popt[1][1]*(flowStates[0]*SH_OPT[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[0][1]*SH_OPT[0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + } else if (obsIndex == 1) { + nextPopt[0][0] = - Popt[0][0]*(SH_OPT[0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[1][0]*flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + nextPopt[0][1] = - Popt[0][1]*(SH_OPT[0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[1][1]*flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + nextPopt[1][0] = - Popt[1][0]*(flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[0][0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + nextPopt[1][1] = - Popt[1][1]*(flowStates[0]*SH_OPT[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]) - 1) - Popt[0][1]*SH_OPT[0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = max(nextPopt[0][0],0.0f); + Popt[1][1] = max(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + } + } + } + + // stop the performance timer + perf_end(_perf_OpticalFlowEKF); +} + +void NavEKF::FuseOptFlow() +{ + // start performance timer + perf_begin(_perf_FuseOptFlow); + + float H_LOS[22]; + float tempVar[9]; + Vector3f velNED_local; + Vector3f relVelSensor; + + uint8_t &obsIndex = flow_state.obsIndex; + ftype &q0 = flow_state.q0; + ftype &q1 = flow_state.q1; + ftype &q2 = flow_state.q2; + ftype &q3 = flow_state.q3; + ftype *SH_LOS = &flow_state.SH_LOS[0]; + ftype *SK_LOS = &flow_state.SK_LOS[0]; + ftype &vn = flow_state.vn; + ftype &ve = flow_state.ve; + ftype &vd = flow_state.vd; + ftype &pd = flow_state.pd; + ftype *losPred = &flow_state.losPred[0]; + + // Copy required states to local variable names + q0 = statesAtFlowTime.quat[0]; + q1 = statesAtFlowTime.quat[1]; + q2 = statesAtFlowTime.quat[2]; + q3 = statesAtFlowTime.quat[3]; + vn = statesAtFlowTime.velocity[0]; + ve = statesAtFlowTime.velocity[1]; + vd = statesAtFlowTime.velocity[2]; + pd = statesAtFlowTime.position[2]; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; + + // constrain terrain to be below vehicle + flowStates[1] = max(flowStates[1], pd + 0.5f); + float heightAboveGndEst = flowStates[1] - pd; + // Calculate observation jacobians and Kalman gains + if (obsIndex == 0) { + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),0.5f,1000.0f); + + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow*velNED_local; + + // divide velocity by range to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + + // Calculate common expressions for observation jacobians + SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2); + SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2); + SH_LOS[3] = 1/(pd - flowStates[1]); + SH_LOS[4] = sq(SH_LOS[3]); + + // Calculate common expressions for Kalman gains + float temp = (R_LOS + (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)))); + if (fabsf(temp) < 1e-9f) return; + SK_LOS[0] = 1.0f/temp; + temp = (R_LOS + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)))); + if (fabsf(temp) < 1e-9f) return; + SK_LOS[1] = 1.0f/temp; + SK_LOS[2] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn); + SK_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + SK_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn); + SK_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + SK_LOS[6] = sq(q0) - sq(q1) + sq(q2) - sq(q3); + SK_LOS[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SK_LOS[8] = SH_LOS[3]; + + // Calculate common intermediate terms + tempVar[0] = SH_LOS[0]*SK_LOS[6]*SK_LOS[8]; + tempVar[1] = SH_LOS[0]*SH_LOS[2]*SH_LOS[4]; + tempVar[2] = 2.0f*SH_LOS[2]*SK_LOS[8]; + tempVar[3] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q1 + 2.0f*q2*q3); + tempVar[4] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q3 - 2.0f*q1*q2); + tempVar[5] = (SK_LOS[5] - q2*tempVar[2]); + tempVar[6] = (SK_LOS[2] - q3*tempVar[2]); + tempVar[7] = (SK_LOS[3] - q1*tempVar[2]); + tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); + + // calculate observation jacobians for X LOS rate + for (uint8_t i = 0; i < 23; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; + H_LOS[1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + H_LOS[2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]; + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); + H_LOS[6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3); + H_LOS[9] = tempVar[1]; + + // calculate Kalman gains for X LOS rate + Kfusion[0] = -(P[0][0]*tempVar[8] + P[0][1]*tempVar[7] - P[0][3]*tempVar[6] + P[0][2]*tempVar[5] - P[0][4]*tempVar[4] + P[0][6]*tempVar[3] - P[0][9]*tempVar[1] + P[0][5]*tempVar[0])/(R_LOS + tempVar[8]*(P[0][0]*tempVar[8] + P[1][0]*tempVar[7] + P[2][0]*tempVar[5] - P[3][0]*tempVar[6] - P[4][0]*tempVar[4] + P[6][0]*tempVar[3] - P[9][0]*tempVar[1] + P[5][0]*tempVar[0]) + tempVar[7]*(P[0][1]*tempVar[8] + P[1][1]*tempVar[7] + P[2][1]*tempVar[5] - P[3][1]*tempVar[6] - P[4][1]*tempVar[4] + P[6][1]*tempVar[3] - P[9][1]*tempVar[1] + P[5][1]*tempVar[0]) + tempVar[5]*(P[0][2]*tempVar[8] + P[1][2]*tempVar[7] + P[2][2]*tempVar[5] - P[3][2]*tempVar[6] - P[4][2]*tempVar[4] + P[6][2]*tempVar[3] - P[9][2]*tempVar[1] + P[5][2]*tempVar[0]) - tempVar[6]*(P[0][3]*tempVar[8] + P[1][3]*tempVar[7] + P[2][3]*tempVar[5] - P[3][3]*tempVar[6] - P[4][3]*tempVar[4] + P[6][3]*tempVar[3] - P[9][3]*tempVar[1] + P[5][3]*tempVar[0]) - tempVar[4]*(P[0][4]*tempVar[8] + P[1][4]*tempVar[7] + P[2][4]*tempVar[5] - P[3][4]*tempVar[6] - P[4][4]*tempVar[4] + P[6][4]*tempVar[3] - P[9][4]*tempVar[1] + P[5][4]*tempVar[0]) + tempVar[3]*(P[0][6]*tempVar[8] + P[1][6]*tempVar[7] + P[2][6]*tempVar[5] - P[3][6]*tempVar[6] - P[4][6]*tempVar[4] + P[6][6]*tempVar[3] - P[9][6]*tempVar[1] + P[5][6]*tempVar[0]) - tempVar[1]*(P[0][9]*tempVar[8] + P[1][9]*tempVar[7] + P[2][9]*tempVar[5] - P[3][9]*tempVar[6] - P[4][9]*tempVar[4] + P[6][9]*tempVar[3] - P[9][9]*tempVar[1] + P[5][9]*tempVar[0]) + tempVar[0]*(P[0][5]*tempVar[8] + P[1][5]*tempVar[7] + P[2][5]*tempVar[5] - P[3][5]*tempVar[6] - P[4][5]*tempVar[4] + P[6][5]*tempVar[3] - P[9][5]*tempVar[1] + P[5][5]*tempVar[0])); + Kfusion[1] = -SK_LOS[1]*(P[1][0]*tempVar[8] + P[1][1]*tempVar[7] - P[1][3]*tempVar[6] + P[1][2]*tempVar[5] - P[1][4]*tempVar[4] + P[1][6]*tempVar[3] - P[1][9]*tempVar[1] + P[1][5]*tempVar[0]); + Kfusion[2] = -SK_LOS[1]*(P[2][0]*tempVar[8] + P[2][1]*tempVar[7] - P[2][3]*tempVar[6] + P[2][2]*tempVar[5] - P[2][4]*tempVar[4] + P[2][6]*tempVar[3] - P[2][9]*tempVar[1] + P[2][5]*tempVar[0]); + Kfusion[3] = -SK_LOS[1]*(P[3][0]*tempVar[8] + P[3][1]*tempVar[7] - P[3][3]*tempVar[6] + P[3][2]*tempVar[5] - P[3][4]*tempVar[4] + P[3][6]*tempVar[3] - P[3][9]*tempVar[1] + P[3][5]*tempVar[0]); + Kfusion[4] = -SK_LOS[1]*(P[4][0]*tempVar[8] + P[4][1]*tempVar[7] - P[4][3]*tempVar[6] + P[4][2]*tempVar[5] - P[4][4]*tempVar[4] + P[4][6]*tempVar[3] - P[4][9]*tempVar[1] + P[4][5]*tempVar[0]); + Kfusion[5] = -SK_LOS[1]*(P[5][0]*tempVar[8] + P[5][1]*tempVar[7] - P[5][3]*tempVar[6] + P[5][2]*tempVar[5] - P[5][4]*tempVar[4] + P[5][6]*tempVar[3] - P[5][9]*tempVar[1] + P[5][5]*tempVar[0]); + Kfusion[6] = -SK_LOS[1]*(P[6][0]*tempVar[8] + P[6][1]*tempVar[7] - P[6][3]*tempVar[6] + P[6][2]*tempVar[5] - P[6][4]*tempVar[4] + P[6][6]*tempVar[3] - P[6][9]*tempVar[1] + P[6][5]*tempVar[0]); + Kfusion[7] = -SK_LOS[1]*(P[7][0]*tempVar[8] + P[7][1]*tempVar[7] - P[7][3]*tempVar[6] + P[7][2]*tempVar[5] - P[7][4]*tempVar[4] + P[7][6]*tempVar[3] - P[7][9]*tempVar[1] + P[7][5]*tempVar[0]); + Kfusion[8] = -SK_LOS[1]*(P[8][0]*tempVar[8] + P[8][1]*tempVar[7] - P[8][3]*tempVar[6] + P[8][2]*tempVar[5] - P[8][4]*tempVar[4] + P[8][6]*tempVar[3] - P[8][9]*tempVar[1] + P[8][5]*tempVar[0]); + Kfusion[9] = -SK_LOS[1]*(P[9][0]*tempVar[8] + P[9][1]*tempVar[7] - P[9][3]*tempVar[6] + P[9][2]*tempVar[5] - P[9][4]*tempVar[4] + P[9][6]*tempVar[3] - P[9][9]*tempVar[1] + P[9][5]*tempVar[0]); + Kfusion[10] = -SK_LOS[1]*(P[10][0]*tempVar[8] + P[10][1]*tempVar[7] - P[10][3]*tempVar[6] + P[10][2]*tempVar[5] - P[10][4]*tempVar[4] + P[10][6]*tempVar[3] - P[10][9]*tempVar[1] + P[10][5]*tempVar[0]); + Kfusion[11] = -SK_LOS[1]*(P[11][0]*tempVar[8] + P[11][1]*tempVar[7] - P[11][3]*tempVar[6] + P[11][2]*tempVar[5] - P[11][4]*tempVar[4] + P[11][6]*tempVar[3] - P[11][9]*tempVar[1] + P[11][5]*tempVar[0]); + Kfusion[12] = -SK_LOS[1]*(P[12][0]*tempVar[8] + P[12][1]*tempVar[7] - P[12][3]*tempVar[6] + P[12][2]*tempVar[5] - P[12][4]*tempVar[4] + P[12][6]*tempVar[3] - P[12][9]*tempVar[1] + P[12][5]*tempVar[0]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + Kfusion[13] = 0.0f;//-SK_LOS[1]*(P[13][0]*tempVar[8] + P[13][1]*tempVar[7] - P[13][3]*tempVar[6] + P[13][2]*tempVar[5] - P[13][4]*tempVar[4] + P[13][6]*tempVar[3] - P[13][9]*tempVar[1] + P[13][5]*tempVar[0]); + if (inhibitWindStates) { + Kfusion[14] = -SK_LOS[1]*(P[14][0]*tempVar[8] + P[14][1]*tempVar[7] - P[14][3]*tempVar[6] + P[14][2]*tempVar[5] - P[14][4]*tempVar[4] + P[14][6]*tempVar[3] - P[14][9]*tempVar[1] + P[14][5]*tempVar[0]); + Kfusion[15] = -SK_LOS[1]*(P[15][0]*tempVar[8] + P[15][1]*tempVar[7] - P[15][3]*tempVar[6] + P[15][2]*tempVar[5] - P[15][4]*tempVar[4] + P[15][6]*tempVar[3] - P[15][9]*tempVar[1] + P[15][5]*tempVar[0]); + } else { + Kfusion[14] = 0.0f; + Kfusion[15] = 0.0f; + } + if (inhibitMagStates) { + Kfusion[16] = -SK_LOS[1]*(P[16][0]*tempVar[8] + P[16][1]*tempVar[7] - P[16][3]*tempVar[6] + P[16][2]*tempVar[5] - P[16][4]*tempVar[4] + P[16][6]*tempVar[3] - P[16][9]*tempVar[1] + P[16][5]*tempVar[0]); + Kfusion[17] = -SK_LOS[1]*(P[17][0]*tempVar[8] + P[17][1]*tempVar[7] - P[17][3]*tempVar[6] + P[17][2]*tempVar[5] - P[17][4]*tempVar[4] + P[17][6]*tempVar[3] - P[17][9]*tempVar[1] + P[17][5]*tempVar[0]); + Kfusion[18] = -SK_LOS[1]*(P[18][0]*tempVar[8] + P[18][1]*tempVar[7] - P[18][3]*tempVar[6] + P[18][2]*tempVar[5] - P[18][4]*tempVar[4] + P[18][6]*tempVar[3] - P[18][9]*tempVar[1] + P[18][5]*tempVar[0]); + Kfusion[19] = -SK_LOS[1]*(P[19][0]*tempVar[8] + P[19][1]*tempVar[7] - P[19][3]*tempVar[6] + P[19][2]*tempVar[5] - P[19][4]*tempVar[4] + P[19][6]*tempVar[3] - P[19][9]*tempVar[1] + P[19][5]*tempVar[0]); + Kfusion[20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); + Kfusion[21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + + // calculate innovation variance and innovation for X axis observation + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - flowRadXYcomp[0]; + + } else if (obsIndex == 1) { + + // calculate intermediate common variables + tempVar[0] = 2.0f*SH_LOS[1]*SK_LOS[8]; + tempVar[1] = (SK_LOS[2] + q0*tempVar[0]); + tempVar[2] = (SK_LOS[5] - q1*tempVar[0]); + tempVar[3] = (SK_LOS[3] + q2*tempVar[0]); + tempVar[4] = (SK_LOS[4] + q3*tempVar[0]); + tempVar[5] = SH_LOS[0]*SK_LOS[8]*(2*q0*q3 + 2*q1*q2); + tempVar[6] = SH_LOS[0]*SK_LOS[8]*(2*q0*q2 - 2*q1*q3); + tempVar[7] = SH_LOS[0]*SH_LOS[1]*SH_LOS[4]; + tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; + + // Calculate observation jacobians for Y LOS rate + for (uint8_t i = 0; i < 23; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; + H_LOS[1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; + H_LOS[2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]; + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)); + H_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2); + H_LOS[6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3); + H_LOS[9] = -tempVar[7]; + + // Calculate Kalman gains for Y LOS rate + Kfusion[0] = SK_LOS[0]*(P[0][0]*tempVar[1] + P[0][1]*tempVar[2] - P[0][2]*tempVar[3] + P[0][3]*tempVar[4] + P[0][5]*tempVar[5] - P[0][6]*tempVar[6] - P[0][9]*tempVar[7] + P[0][4]*tempVar[8]); + Kfusion[1] = SK_LOS[0]*(P[1][0]*tempVar[1] + P[1][1]*tempVar[2] - P[1][2]*tempVar[3] + P[1][3]*tempVar[4] + P[1][5]*tempVar[5] - P[1][6]*tempVar[6] - P[1][9]*tempVar[7] + P[1][4]*tempVar[8]); + Kfusion[2] = SK_LOS[0]*(P[2][0]*tempVar[1] + P[2][1]*tempVar[2] - P[2][2]*tempVar[3] + P[2][3]*tempVar[4] + P[2][5]*tempVar[5] - P[2][6]*tempVar[6] - P[2][9]*tempVar[7] + P[2][4]*tempVar[8]); + Kfusion[3] = SK_LOS[0]*(P[3][0]*tempVar[1] + P[3][1]*tempVar[2] - P[3][2]*tempVar[3] + P[3][3]*tempVar[4] + P[3][5]*tempVar[5] - P[3][6]*tempVar[6] - P[3][9]*tempVar[7] + P[3][4]*tempVar[8]); + Kfusion[4] = SK_LOS[0]*(P[4][0]*tempVar[1] + P[4][1]*tempVar[2] - P[4][2]*tempVar[3] + P[4][3]*tempVar[4] + P[4][5]*tempVar[5] - P[4][6]*tempVar[6] - P[4][9]*tempVar[7] + P[4][4]*tempVar[8]); + Kfusion[5] = SK_LOS[0]*(P[5][0]*tempVar[1] + P[5][1]*tempVar[2] - P[5][2]*tempVar[3] + P[5][3]*tempVar[4] + P[5][5]*tempVar[5] - P[5][6]*tempVar[6] - P[5][9]*tempVar[7] + P[5][4]*tempVar[8]); + Kfusion[6] = SK_LOS[0]*(P[6][0]*tempVar[1] + P[6][1]*tempVar[2] - P[6][2]*tempVar[3] + P[6][3]*tempVar[4] + P[6][5]*tempVar[5] - P[6][6]*tempVar[6] - P[6][9]*tempVar[7] + P[6][4]*tempVar[8]); + Kfusion[7] = SK_LOS[0]*(P[7][0]*tempVar[1] + P[7][1]*tempVar[2] - P[7][2]*tempVar[3] + P[7][3]*tempVar[4] + P[7][5]*tempVar[5] - P[7][6]*tempVar[6] - P[7][9]*tempVar[7] + P[7][4]*tempVar[8]); + Kfusion[8] = SK_LOS[0]*(P[8][0]*tempVar[1] + P[8][1]*tempVar[2] - P[8][2]*tempVar[3] + P[8][3]*tempVar[4] + P[8][5]*tempVar[5] - P[8][6]*tempVar[6] - P[8][9]*tempVar[7] + P[8][4]*tempVar[8]); + Kfusion[9] = SK_LOS[0]*(P[9][0]*tempVar[1] + P[9][1]*tempVar[2] - P[9][2]*tempVar[3] + P[9][3]*tempVar[4] + P[9][5]*tempVar[5] - P[9][6]*tempVar[6] - P[9][9]*tempVar[7] + P[9][4]*tempVar[8]); + Kfusion[10] = SK_LOS[0]*(P[10][0]*tempVar[1] + P[10][1]*tempVar[2] - P[10][2]*tempVar[3] + P[10][3]*tempVar[4] + P[10][5]*tempVar[5] - P[10][6]*tempVar[6] - P[10][9]*tempVar[7] + P[10][4]*tempVar[8]); + Kfusion[11] = SK_LOS[0]*(P[11][0]*tempVar[1] + P[11][1]*tempVar[2] - P[11][2]*tempVar[3] + P[11][3]*tempVar[4] + P[11][5]*tempVar[5] - P[11][6]*tempVar[6] - P[11][9]*tempVar[7] + P[11][4]*tempVar[8]); + Kfusion[12] = SK_LOS[0]*(P[12][0]*tempVar[1] + P[12][1]*tempVar[2] - P[12][2]*tempVar[3] + P[12][3]*tempVar[4] + P[12][5]*tempVar[5] - P[12][6]*tempVar[6] - P[12][9]*tempVar[7] + P[12][4]*tempVar[8]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + Kfusion[13] = 0.0f;//SK_LOS[0]*(P[13][0]*tempVar[1] + P[13][1]*tempVar[2] - P[13][2]*tempVar[3] + P[13][3]*tempVar[4] + P[13][5]*tempVar[5] - P[13][6]*tempVar[6] - P[13][9]*tempVar[7] + P[13][4]*tempVar[8]); + if (inhibitWindStates) { + Kfusion[14] = SK_LOS[0]*(P[14][0]*tempVar[1] + P[14][1]*tempVar[2] - P[14][2]*tempVar[3] + P[14][3]*tempVar[4] + P[14][5]*tempVar[5] - P[14][6]*tempVar[6] - P[14][9]*tempVar[7] + P[14][4]*tempVar[8]); + Kfusion[15] = SK_LOS[0]*(P[15][0]*tempVar[1] + P[15][1]*tempVar[2] - P[15][2]*tempVar[3] + P[15][3]*tempVar[4] + P[15][5]*tempVar[5] - P[15][6]*tempVar[6] - P[15][9]*tempVar[7] + P[15][4]*tempVar[8]); + } else { + Kfusion[14] = 0.0f; + Kfusion[15] = 0.0f; + } + if (inhibitMagStates) { + Kfusion[16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]); + Kfusion[17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]); + Kfusion[18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]); + Kfusion[19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]); + Kfusion[20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); + Kfusion[21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + + // calculate variance and innovation for Y observation + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - flowRadXYcomp[1]; + + } + + // calculate the innovation consistency test ratio + flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(_flowInnovGate) * varInnovOptFlow[obsIndex]); + + // Check the innovation for consistency and don't fuse if out of bounds + if (flowTestRatio[obsIndex] < 1.0f) { + // correct the state vector + for (uint8_t j = 0; j <= 21; j++) + { + states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + state.quat.normalize(); + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i <= 21; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + } + for (uint8_t i = 0; i <= 21; i++) + { + for (uint8_t j = 0; j <= 21; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + } + } + for (uint8_t i = 0; i <= 21; i++) + { + for (uint8_t j = 0; j <= 21; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + + // stop the performance timer + perf_end(_perf_FuseOptFlow); +} + // fuse true airspeed measurements void NavEKF::FuseAirspeed() { @@ -2793,6 +3391,29 @@ void NavEKF::RecallStates(state_elements &statesForFusion, uint32_t msec) } } +// recall omega (angular rate vector) average across the time interval from msecStart to msecEnd +void NavEKF::RecallOmega(Vector3f &omegaAvg, uint32_t msecStart, uint32_t msecEnd) +{ + // calculate average angular rate vector over the time interval from msecStart to msecEnd + // if no values are inside the time window, return the current angular rate + omegaAvg.zero(); + uint8_t numAvg = 0; + for (uint8_t i=0; i<=49; i++) + { + if (msecStart <= statetimeStamp[i] && msecEnd >= statetimeStamp[i]) + { + omegaAvg += storedStates[i].omega; + numAvg += 1; + } + } + if (numAvg >= 1) + { + omegaAvg = omegaAvg / float(numAvg); + } else { + omegaAvg = correctedDelAng * (1.0f * dtIMUinv); + } +} + // calculate nav to body quaternions from body to nav rotation matrix void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const { @@ -2861,8 +3482,8 @@ void NavEKF::getAccelZBias(float &zbias1, float &zbias2) const { zbias1 = 0; zbias2 = 0; } else { - zbias1 = state.accel_zbias1 / dtIMU; - zbias2 = state.accel_zbias2 / dtIMU; + zbias1 = state.accel_zbias1 * dtIMUinv; + zbias2 = state.accel_zbias2 * dtIMUinv; } } @@ -2898,6 +3519,18 @@ bool NavEKF::getLLH(struct Location &loc) const return true; } +// return data for debugging optical flow fusion +void NavEKF::getFlowDebug(float &scaleFactor, float &obsX, float &obsY, float &innovX, float &innovY, float &gndPos, uint8_t &quality) const +{ + scaleFactor = flowStates[0]; + obsX = flowRadXY[0]; + obsY = flowRadXY[1]; + innovX = innovOptFlow[0]; + innovY = innovOptFlow[1]; + gndPos = flowStates[1]; + quality = flowQuality; +} + // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed void NavEKF::SetFlightAndFusionModes() { @@ -2988,6 +3621,18 @@ void NavEKF::SetFlightAndFusionModes() bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2)); // inhibit the magnetic field calibration if not requested or denied inhibitMagStates = (!magCalRequested || magCalDenied); + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable + if ((!highGndSpdStage2 || (imuSampleTime_ms - lastFixTime_ms) > 1000) && !useRngFinder()) { + inhibitGndState = true; + } else { + inhibitGndState = false; + } + // Don't update focal length offset state if there is no range finder + if (!useRngFinder()) { + fScaleInhibit = true; + } else { + fScaleInhibit = false; + } } // initialise the covariance matrix @@ -3113,7 +3758,7 @@ void NavEKF::readIMUData() // 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 + // get accel 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); accel2 = _ahrs->get_ins().get_accel(1); @@ -3255,6 +3900,39 @@ void NavEKF::readAirSpdData() } } +// write the raw optical flow measurements +// this needs to be called externally. +void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas) +{ + // The raw measurements need to be optical flow rates in radians/second, however they are currently output by the sensor as pixels over an internal sampling period + // A modified PX4flow firmware has been used which effectivly sets the range used to calculate the velocity internally within the px4flow sensor to unity + // so that the velocity outputs are in radians/sec + // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a negative sensor rotation about that axis, + // hence the need to swap axes and signs. + flowMeaTime_ms = msecFlowMeas; + if (rawFlowQuality > 150){ + flowQuality = rawFlowQuality; + // recall vehicle states at mid sample time for flow observations allowing for delays + RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2); + // recall angular rates averaged across flow observation period allowing for average 5 msec intersample delay + RecallOmega(omegaAcrossFlowTime, flowMeaTime_ms - flowTimeDeltaAvg_ms - _msecFLowDelay, flowMeaTime_ms - _msecFLowDelay); + // calculate rotation matrices at mid sample time for flow observations + Quaternion q(statesAtFlowTime.quat[0],statesAtFlowTime.quat[1],statesAtFlowTime.quat[2],statesAtFlowTime.quat[3]); + q.rotation_matrix(Tbn_flow); + Tnb_flow = Tbn_flow.transposed(); + // write uncorrected flow rate measurements that will be used by the focal length scale factor estimator + flowRadXY[0] = + rawFlowRates.y; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) + flowRadXY[1] = - rawFlowRates.x; // raw (non motion compensated) optical flow angular rate about the Y axis (rad/sec) + // write flow rate measurements corrected for focal length scale factor errors and body rates + flowRadXYcomp[0] = flowStates[0]*flowRadXY[0] + omegaAcrossFlowTime.x; + flowRadXYcomp[1] = flowStates[0]*flowRadXY[1] + omegaAcrossFlowTime.y; + // set flag that will trigger observations + newDataFlow = true; + } else { + newDataFlow = false; + } +} + // calculate the NED earth spin vector in rad/sec void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const { @@ -3448,6 +4126,7 @@ void NavEKF::ZeroVariables() lastMagUpdate = imuSampleTime_ms; lastHgtMeasTime = imuSampleTime_ms; lastHgtTime_ms = imuSampleTime_ms; + lastAirspeedUpdate = imuSampleTime_ms; velFailTime = imuSampleTime_ms; posFailTime = imuSampleTime_ms; hgtFailTime = imuSampleTime_ms; @@ -3492,6 +4171,19 @@ void NavEKF::ZeroVariables() memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); gpsPosGlitchOffsetNE.zero(); + // optical flow variables + newDataFlow = false; + flowFusePerformed = false; + fuseOptFlowData = false; + flowMeaTime_ms = imuSampleTime_ms; + memset(&Popt[0][0], 0, sizeof(Popt)); + flowStates[0] = 1.0f; + flowStates[1] = 0.0f; + prevPosN = gpsPosNE.x; + prevPosE = gpsPosNE.y; + fuseRngData = false; + inhibitGndState = true; + prevFlowFusionTime_ms = imuSampleTime_ms; // time the last flow measurement was fused } // return true if we should use the airspeed sensor @@ -3503,6 +4195,20 @@ bool NavEKF::useAirspeed(void) const return _ahrs->get_airspeed()->use(); } +// return true if we should use the range finder sensor +bool NavEKF::useRngFinder(void) const +{ + // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor + return false; +} + +// return true if we should use the optical flow sensor +bool NavEKF::useOptFlow(void) const +{ + // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor + return true; +} + // 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 diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index d3ab834fcf..cc8bdded45 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -53,9 +53,11 @@ public: typedef VectorN Vector14; typedef VectorN Vector15; typedef VectorN Vector22; + typedef VectorN Vector31; + typedef VectorN Vector34; typedef VectorN,3> Matrix3; typedef VectorN,22> Matrix22; - typedef VectorN,22> Matrix22_50; + typedef VectorN,22> Matrix34_50; #else typedef ftype Vector2[2]; typedef ftype Vector3[3]; @@ -67,9 +69,10 @@ public: typedef ftype Vector15[15]; typedef ftype Vector22[22]; typedef ftype Vector31[31]; + typedef ftype Vector34[34]; typedef ftype Matrix3[3][3]; typedef ftype Matrix22[22][22]; - typedef ftype Matrix31_50[31][50]; + typedef ftype Matrix34_50[34][50]; #endif // Constructor @@ -151,6 +154,16 @@ public: // reporting via ahrs.use_compass() bool use_compass(void) const; + // write the raw optical flow measurements + // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality + // rawFlowRates is the flow rate in radians per second. A positive ground relative velocity along the X axis produces positive raw X value, and similarly for the Y axis + // rawSonarRange is the range in metres measured by the px4flow sensor + // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas); + + // return data for debugging optical flow fusion + void getFlowDebug(float &scaleFactor, float &obsX, float &obsY, float &innovX, float &innovY, float &gndPos, uint8_t &quality) const; + /* return the filter fault status as a bitmasked integer 0 = unassigned @@ -171,10 +184,10 @@ private: const AP_AHRS *_ahrs; AP_Baro &_baro; - // the states are available in two forms, either as a Vector27, or + // the states are available in two forms, either as a Vector34, or // broken down as individual elements. Both are equivalent (same // memory) - Vector31 states; + Vector34 states; struct state_elements { Quaternion quat; // 0..3 Vector3f velocity; // 4..6 @@ -189,6 +202,7 @@ private: float posD1; // 26 Vector3f vel2; // 27 .. 29 float posD2; // 30 + Vector3f omega; // 31 .. 33 } &state; // update the quaternion, velocity and position states using IMU measurements @@ -312,6 +326,31 @@ private: // this allows large GPS position jumps to be accomodated gradually void decayGpsOffset(void); + // Check for filter divergence + void checkDivergence(void); + + // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 + void calcIMU_Weighting(float K1, float K2); + + // return true if we should use the optical flow sensor + bool useOptFlow(void) const; + + // return true if we should use the range finder sensor + bool useRngFinder(void) const; + + // determine when to perform fusion of optical flow measurements + void SelectFlowFusion(); + + // recall omega (angular rate vector) average from time specified by msec to current time + // this is useful for motion compensation of optical flow measurements + void RecallOmega(Vector3f &omegaAvg, uint32_t msecStart, uint32_t msecEnd); + + // Estimate optical flow focal length scale factor and terrain offset using a 2-state EKF + void OpticalFlowEKF(); + + // fuse optical flow measurements into the main filter + void FuseOptFlow(); + // EKF Mavlink Tuneable Parameters AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s @@ -338,6 +377,11 @@ private: AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration AP_Int16 _gpsGlitchAccelMax; // Maximum allowed discrepancy between inertial and GPS Horizontal acceleration before GPS data is ignored : cm/s^2 AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m + AP_Int8 _gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation. + AP_Float _flowNoise; // optical flow rate measurement noise + AP_Int8 _flowInnovGate; // Number of standard deviations applied to optical flow innovation consistency check + AP_Int8 _msecFLowDelay; // effective average delay of optical flow measurements rel to IMU (msec) + AP_Int8 _rngInnovGate; // Number of standard deviations applied to range finder innovation consistency check AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively. // Tuning parameters @@ -357,7 +401,8 @@ private: uint16_t _msecGpsAvg; // average number of msec between GPS measurements uint16_t _msecHgtAvg; // average number of msec between height measurements uint16_t _msecMagAvg; // average number of msec between magnetometer measurements - uint16_t _msecBetaAvg; // maximum number of msec between synthetic sideslip measurements + uint16_t _msecBetaAvg; // Average number of msec between synthetic sideslip measurements + uint16_t _msecBetaMax; // maximum number of msec between synthetic sideslip measurements float dtVelPos; // average of msec between position and velocity corrections // Variables @@ -432,7 +477,9 @@ private: uint32_t TASmsecPrev; // time stamp of last TAS fusion step uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip fusion step const uint32_t TASmsecMax; // maximum allowed interval between TAS fusion steps - const bool fuseMeNow; // boolean to force fusion whenever data arrives + uint32_t MAGmsecPrev; // time stamp of last compass fusion step + uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step + bool inhibitLoadLeveling; // boolean that turns off delay of fusion to level processor loading bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing bool prevStaticMode; // value of static mode from last update uint32_t lastMagUpdate; // last time compass was updated @@ -495,6 +542,62 @@ private: uint8_t magUpdateCountMax; // limit on the number of minor state corrections using Magnetometer data float magUpdateCountMaxInv; // floating point inverse of magFilterCountMax + // variables added for optical flow fusion + float dtIMUinv; // inverse of IMU time step + bool newDataFlow; // true when new optical flow data has arrived + bool flowFusePerformed; // true when optical flow fusion has been perfomred in that time step + state_elements statesAtFlowTime;// States at the middle of the optical flow sample period + bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused + float auxFlowObsInnov[2]; // optical flow observation innovations from 2-state focal length scale factor and terrain offset estimator + float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from 2-state focal length scale factor and terrain offset estimator + float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec) + float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec) + uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) + uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality. + float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. + float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step + Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period + Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period + Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period + float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2 + float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) + uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec) + float Popt[2][2]; // state covariance matrix + float flowStates[2]; // flow states [scale factor, terrain position] + float prevPosN; // north position at last measurement + float prevPosE; // east position at last measurement + state_elements statesAtRngTime; // States at the range finder measurement time + bool fuseRngData; // true when fusion of range data is demanded + float varInnovRng; // range finder observation innovation variance (m^2) + float innovRng; // range finder observation innovation (m) + float rngMea; // range finder measurement (m) + bool inhibitGndState; // true when the terrain position state is to remain constant + uint32_t prevFlowFusionTime_ms; // time the last flow measurement was fused + uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events + bool fScaleInhibit; // true when the focal length scale factor is to remain constant + float flowTestRatio[2]; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail + float auxFlowTestRatio[2]; // sum of squares of optical flow innovations divided by fail threshold used by aux filter + float R_LOS; // variance of optical flow rate measurements (rad/sec)^2 + float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail + + // states held by optical flow fusion across time steps + // optical flow X,Y motion compensated rate measurements are fused across two time steps + // to level computational load as this can be an expensive operation + struct { + uint8_t obsIndex; + ftype SH_LOS[5]; + ftype SK_LOS[9]; + ftype q0; + ftype q1; + ftype q2; + ftype q3; + ftype vn; + ftype ve; + ftype vd; + ftype pd; + ftype losPred[2]; + } flow_state; + struct { bool bad_xmag:1; bool bad_ymag:1; @@ -533,6 +636,8 @@ private: perf_counter_t _perf_FuseMagnetometer; perf_counter_t _perf_FuseAirspeed; perf_counter_t _perf_FuseSideslip; + perf_counter_t _perf_OpticalFlowEKF; + perf_counter_t _perf_FuseOptFlow; #endif // should we assume zero sideslip?