diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 124c7d20ef..dc67fd8396 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1108,6 +1108,40 @@ void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl } } +/* + * Write body frame linear and angular displacement measurements from a visual odometry sensor + * + * quality is a normalised confidence value from 0 to 100 + * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m) + * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad) + * delTime is the time interval for the measurement of delPos and delAng (sec) + * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec) + * posOffset is the XYZ body frame position of the camera focal point (m) +*/ +void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset) +{ + if (core) { + for (uint8_t i=0; i= num_cores) { + instance = primary; + } + + if (core) { + ret = core[instance].getBodyFrameOdomDebug(velInnov, velInnovVar); + } + + return ret; +} + // return data for debugging range beacon fusion bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow, Vector3f &posNED) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index a93b9d55b5..3ad2304430 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -196,6 +196,28 @@ public: // posOffset is the XYZ flow sensor position in the body frame in m void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset); + /* + * Write body frame linear and angular displacement measurements from a visual odometry sensor + * + * quality is a normalised confidence value from 0 to 100 + * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m) + * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad) + * delTime is the time interval for the measurement of delPos and delAng (sec) + * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec) + * posOffset is the XYZ body frame position of the camera focal point (m) + */ + void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset); + + /* + * Return data for debugging body frame odometry fusion: + * + * velInnov are the XYZ body frame velocity innovations (m/s) + * velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2 + * + * Return the system time stamp of the last update (msec) + */ + uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar); + // return data for debugging optical flow fusion for the specified instance // An out of range instance (eg -1) returns data for the the primary instance void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 87ee1185b3..941bc49dd6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -191,7 +191,7 @@ void NavEKF3_core::setAidingMode() // GPS aiding is the preferred option unless excluded by the user if(readyToUseGPS() || readyToUseRangeBeacon()) { PV_AidingMode = AID_ABSOLUTE; - } else if (readyToUseOptFlow()) { + } else if (readyToUseOptFlow() || readyToUseBodyOdm()) { PV_AidingMode = AID_RELATIVE; } } else if (PV_AidingMode == AID_RELATIVE) { @@ -199,11 +199,15 @@ void NavEKF3_core::setAidingMode() bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); // Check if the fusion has timed out (flow measurements have been rejected for too long) bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); + // Check if the body odometry flow sensor has timed out + bool bodyOdmSensorTimeout = ((imuSampleTime_ms - bodyOdmMeasTime_ms) > 5000); + // Check if the fusion has timed out (body odometry measurements have been rejected for too long) + bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000); // Enable switch to absolute position mode if GPS or range beacon data is available // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding if(readyToUseGPS() || readyToUseRangeBeacon()) { PV_AidingMode = AID_ABSOLUTE; - } else if (flowSensorTimeout || flowFusionTimeout) { + } else if ((flowSensorTimeout || flowFusionTimeout) && (bodyOdmSensorTimeout || bodyOdmFusionTimeout)) { PV_AidingMode = AID_NONE; } } else if (PV_AidingMode == AID_ABSOLUTE) { @@ -213,6 +217,9 @@ void NavEKF3_core::setAidingMode() // Check if optical flow data is being used bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms); + // Check if body odometry data is being used + bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms); + // Check if airspeed data is being used bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms); @@ -224,10 +231,10 @@ void NavEKF3_core::setAidingMode() bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source - bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; + bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed; // check if velocity drift has been constrained by a measurement source - bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed; + bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed; // check if position drift has been constrained by a measurement source bool posAiding = gpsPosUsed || rngBcnUsed; @@ -278,7 +285,7 @@ void NavEKF3_core::setAidingMode() // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. if (PV_AidingMode == AID_NONE) { // We have ceased aiding - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u has stopped aiding",(unsigned)imu_index); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index); // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors posTimeout = true; velTimeout = true; @@ -293,15 +300,23 @@ void NavEKF3_core::setAidingMode() meaHgtAtTakeOff = baroDataDelayed.hgt; // reset the vertical position state to faster recover from baro errors experienced during touchdown stateStruct.position.z = -meaHgtAtTakeOff; + // reset relative aiding sensor fusion activity status + flowFusionActive = false; + bodyVelFusionActive = false; } else if (PV_AidingMode == AID_RELATIVE) { - // We have commenced aiding, but GPS usage has been prohibited so use optical flow only - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using optical flow",(unsigned)imu_index); + // We are doing relative position navigation where velocity errors are constrained, but position drift will occur + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index); + if (readyToUseOptFlow()) { + // Reset time stamps + flowValidMeaTime_ms = imuSampleTime_ms; + prevFlowFuseTime_ms = imuSampleTime_ms; + } else if (readyToUseBodyOdm()) { + // Reset time stamps + lastbodyVelPassTime_ms = imuSampleTime_ms; + prevBodyVelFuseTime_ms = imuSampleTime_ms; + } posTimeout = true; velTimeout = true; - // Reset the last valid flow measurement time - flowValidMeaTime_ms = imuSampleTime_ms; - // Reset the last valid flow fusion time - prevFlowFuseTime_ms = imuSampleTime_ms; } else if (PV_AidingMode == AID_ABSOLUTE) { if (readyToUseGPS()) { // We are commencing aiding using GPS - this is the preferred method @@ -376,6 +391,16 @@ bool NavEKF3_core::readyToUseOptFlow(void) const return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned; } +// return true if the filter is ready to start using body frame odometry measurements +bool NavEKF3_core::readyToUseBodyOdm(void) const +{ + // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use these measurements + return (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) + && bodyOdmDataNew.velErr < 1.0f + && tiltAlignComplete + && delAngBiasLearned; +} + // return true if the filter to be ready to use gps bool NavEKF3_core::readyToUseGPS(void) const { @@ -467,13 +492,12 @@ void NavEKF3_core::updateFilterStatus(void) { // init return value filterStatus.value = 0; - bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid; + bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000); + bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid; bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; - bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; - bool optFlowNavPossible = flowDataValid && delAngBiasLearned; - bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned; + bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav || doingBodyVelNav; bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks @@ -483,13 +507,13 @@ void NavEKF3_core::updateFilterStatus(void) filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid - filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid + filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode - filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode - filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode + filterStatus.flags.pred_horiz_pos_rel = filterStatus.flags.horiz_pos_rel; // EKF3 enters the required mode before flight + filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs; // EKF3 enters the required mode before flight filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 52ea4abdf2..ad1dea5e24 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -106,6 +106,30 @@ void NavEKF3_core::readRangeFinder(void) } } +void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset) +{ + // limit update rate to maximum allowed by sensor buffers and fusion process + // don't try to write to buffer until the filter has been initialised + if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) { + return; + } + + bodyOdmDataNew.body_offset = &posOffset; + bodyOdmDataNew.vel = delPos * (1.0f/delTime); + bodyOdmDataNew.time_ms = timeStamp_ms; + bodyOdmDataNew.angRate = delAng * (1.0f/delTime); + bodyOdmMeasTime_ms = timeStamp_ms; + + // simple model of accuracy + // TODO move this calculation outside of EKF into the sensor driver + const float minVelErr = 0.5f; + const float maxVelErr = 10.0f; + bodyOdmDataNew.velErr = minVelErr + (maxVelErr - minVelErr) * (1.0f - 0.01f * quality); + + storedBodyOdm.push(bodyOdmDataNew); + +} + // write the raw optical flow measurements // this needs to be called externally. void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index adc45ddf15..770e063cd2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -6,6 +6,7 @@ #include "AP_NavEKF3_core.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -647,7 +648,11 @@ void NavEKF3_core::FuseOptFlow() if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) { // record the last time observations were accepted for fusion prevFlowFuseTime_ms = imuSampleTime_ms; - + // notify first time only + if (!flowFusionActive) { + flowFusionActive = true; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index); + } // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 3ea89317f8..72fa6de890 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -67,6 +67,18 @@ void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset() } +// return data for debugging body frame odometry fusion +uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar) +{ + velInnov.x = innovBodyVel[0]; + velInnov.y = innovBodyVel[1]; + velInnov.z = innovBodyVel[2]; + velInnovVar.x = varInnovBodyVel[0]; + velInnovVar.y = varInnovBodyVel[1]; + velInnovVar.z = varInnovBodyVel[2]; + return bodyOdmDataDelayed.time_ms; +} + // return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow, Vector3f &posNED) @@ -339,7 +351,13 @@ bool NavEKF3_core::getLLH(struct Location &loc) const // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const { - if (PV_AidingMode == AID_RELATIVE) { + // If in the last 10 seconds we have received flow data and no odometry data, then we are relying on optical flow + bool relyingOnFlowData = (imuSampleTime_ms - prevBodyVelFuseTime_ms > 1000) + && (imuSampleTime_ms - flowValidMeaTime_ms <= 10000); + + // If relying on optical flow, limit speed to prevent sensor limit being exceeded and adjust + // nav gains to prevent body rate feedback into flow rates destabilising the control loop + if (PV_AidingMode == AID_RELATIVE && relyingOnFlowData) { // allow 1.0 rad/sec margin for angular motion ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd); // use standard gains up to 5.0 metres height and reduce above that diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 535491104c..a65a7f5f21 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -8,6 +8,7 @@ #include "AP_NavEKF3_core.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -886,4 +887,627 @@ void NavEKF3_core::selectHeightForFusion() } } +/* + * Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m +*/ +void NavEKF3_core::FuseBodyVel() +{ + Vector24 H_VEL; + Vector3f bodyVelPred; + + // Copy required states to local variable names + float q0 = stateStruct.quat[0]; + float q1 = stateStruct.quat[1]; + float q2 = stateStruct.quat[2]; + float q3 = stateStruct.quat[3]; + float vn = stateStruct.velocity.x; + float ve = stateStruct.velocity.y; + float vd = stateStruct.velocity.z; + + // Fuse X, Y and Z axis measurements sequentially assuming observation errors are uncorrelated + for (uint8_t obsIndex=0; obsIndex<=2; obsIndex++) { + + // calculate relative velocity in sensor frame including the relative motion due to rotation + bodyVelPred = (prevTnb * stateStruct.velocity); + + // correct sensor offset body frame position offset relative to IMU + Vector3f posOffsetBody = (*bodyOdmDataDelayed.body_offset) - accelPosOffset; + + // correct prediction for relative motion due to rotation + // note - % operator overloaded for cross product + if (imuDataDelayed.delAngDT > 0.001f) { + bodyVelPred += (imuDataDelayed.delAng * (1.0f / imuDataDelayed.delAngDT)) % posOffsetBody; + } + + // calculate observation jacobians and Kalman gains + if (obsIndex == 0) { + // calculate X axis observation Jacobian + H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f; + H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f; + H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f; + H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f; + H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3; + H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f; + H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f; + for (uint8_t index = 7; index < 24; index++) { + H_VEL[index] = 0.0f; + } + + // calculate intermediate expressions for X axis Kalman gains + float R_VEL = bodyOdmDataDelayed.velErr; + float t2 = q0*q3*2.0f; + float t3 = q1*q2*2.0f; + float t4 = t2+t3; + float t5 = q0*q0; + float t6 = q1*q1; + float t7 = q2*q2; + float t8 = q3*q3; + float t9 = t5+t6-t7-t8; + float t10 = q0*q2*2.0f; + float t25 = q1*q3*2.0f; + float t11 = t10-t25; + float t12 = q3*ve*2.0f; + float t13 = q0*vn*2.0f; + float t26 = q2*vd*2.0f; + float t14 = t12+t13-t26; + float t15 = q3*vd*2.0f; + float t16 = q2*ve*2.0f; + float t17 = q1*vn*2.0f; + float t18 = t15+t16+t17; + float t19 = q0*vd*2.0f; + float t20 = q2*vn*2.0f; + float t27 = q1*ve*2.0f; + float t21 = t19+t20-t27; + float t22 = q1*vd*2.0f; + float t23 = q0*ve*2.0f; + float t28 = q3*vn*2.0f; + float t24 = t22+t23-t28; + float t29 = P[0][0]*t14; + float t30 = P[1][1]*t18; + float t31 = P[4][5]*t9; + float t32 = P[5][5]*t4; + float t33 = P[0][5]*t14; + float t34 = P[1][5]*t18; + float t35 = P[3][5]*t24; + float t79 = P[6][5]*t11; + float t80 = P[2][5]*t21; + float t36 = t31+t32+t33+t34+t35-t79-t80; + float t37 = t4*t36; + float t38 = P[4][6]*t9; + float t39 = P[5][6]*t4; + float t40 = P[0][6]*t14; + float t41 = P[1][6]*t18; + float t42 = P[3][6]*t24; + float t81 = P[6][6]*t11; + float t82 = P[2][6]*t21; + float t43 = t38+t39+t40+t41+t42-t81-t82; + float t44 = P[4][0]*t9; + float t45 = P[5][0]*t4; + float t46 = P[1][0]*t18; + float t47 = P[3][0]*t24; + float t84 = P[6][0]*t11; + float t85 = P[2][0]*t21; + float t48 = t29+t44+t45+t46+t47-t84-t85; + float t49 = t14*t48; + float t50 = P[4][1]*t9; + float t51 = P[5][1]*t4; + float t52 = P[0][1]*t14; + float t53 = P[3][1]*t24; + float t86 = P[6][1]*t11; + float t87 = P[2][1]*t21; + float t54 = t30+t50+t51+t52+t53-t86-t87; + float t55 = t18*t54; + float t56 = P[4][2]*t9; + float t57 = P[5][2]*t4; + float t58 = P[0][2]*t14; + float t59 = P[1][2]*t18; + float t60 = P[3][2]*t24; + float t78 = P[2][2]*t21; + float t88 = P[6][2]*t11; + float t61 = t56+t57+t58+t59+t60-t78-t88; + float t62 = P[4][3]*t9; + float t63 = P[5][3]*t4; + float t64 = P[0][3]*t14; + float t65 = P[1][3]*t18; + float t66 = P[3][3]*t24; + float t90 = P[6][3]*t11; + float t91 = P[2][3]*t21; + float t67 = t62+t63+t64+t65+t66-t90-t91; + float t68 = t24*t67; + float t69 = P[4][4]*t9; + float t70 = P[5][4]*t4; + float t71 = P[0][4]*t14; + float t72 = P[1][4]*t18; + float t73 = P[3][4]*t24; + float t92 = P[6][4]*t11; + float t93 = P[2][4]*t21; + float t74 = t69+t70+t71+t72+t73-t92-t93; + float t75 = t9*t74; + float t83 = t11*t43; + float t89 = t21*t61; + float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89; + float t77; + + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation + if (t76 > R_VEL) { + t77 = 1.0f/t76; + faultStatus.bad_xvel = false; + } else { + t76 = R_VEL; + t77 = 1.0f/R_VEL; + faultStatus.bad_xvel = true; + return; + } + varInnovBodyVel[0] = t77; + + // calculate innovation for X axis observation + innovBodyVel[0] = bodyVelPred.x - bodyOdmDataDelayed.vel.x; + + // calculate Kalman gains for X-axis observation + Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24); + Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24); + Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24); + Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21); + Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24); + Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24); + Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24); + Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24); + Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24); + Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24); + Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24); + Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24); + Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24); + Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24); + Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24); + Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24); + if (!inhibitMagStates) { + Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24); + Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24); + Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24); + Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24); + Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24); + Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + if (!inhibitWindStates) { + Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24); + Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + } else if (obsIndex == 1) { + // calculate Y axis observation Jacobian + H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f; + H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f; + H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f; + H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f; + H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f; + H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3; + H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f; + for (uint8_t index = 7; index < 24; index++) { + H_VEL[index] = 0.0f; + } + + // calculate intermediate expressions for Y axis Kalman gains + float R_VEL = bodyOdmDataDelayed.velErr; + float t2 = q0*q3*2.0f; + float t9 = q1*q2*2.0f; + float t3 = t2-t9; + float t4 = q0*q0; + float t5 = q1*q1; + float t6 = q2*q2; + float t7 = q3*q3; + float t8 = t4-t5+t6-t7; + float t10 = q0*q1*2.0f; + float t11 = q2*q3*2.0f; + float t12 = t10+t11; + float t13 = q1*vd*2.0f; + float t14 = q0*ve*2.0f; + float t26 = q3*vn*2.0f; + float t15 = t13+t14-t26; + float t16 = q0*vd*2.0f; + float t17 = q2*vn*2.0f; + float t27 = q1*ve*2.0f; + float t18 = t16+t17-t27; + float t19 = q3*vd*2.0f; + float t20 = q2*ve*2.0f; + float t21 = q1*vn*2.0f; + float t22 = t19+t20+t21; + float t23 = q3*ve*2.0f; + float t24 = q0*vn*2.0f; + float t28 = q2*vd*2.0f; + float t25 = t23+t24-t28; + float t29 = P[0][0]*t15; + float t30 = P[1][1]*t18; + float t31 = P[5][4]*t8; + float t32 = P[6][4]*t12; + float t33 = P[0][4]*t15; + float t34 = P[1][4]*t18; + float t35 = P[2][4]*t22; + float t78 = P[4][4]*t3; + float t79 = P[3][4]*t25; + float t36 = t31+t32+t33+t34+t35-t78-t79; + float t37 = P[5][6]*t8; + float t38 = P[6][6]*t12; + float t39 = P[0][6]*t15; + float t40 = P[1][6]*t18; + float t41 = P[2][6]*t22; + float t81 = P[4][6]*t3; + float t82 = P[3][6]*t25; + float t42 = t37+t38+t39+t40+t41-t81-t82; + float t43 = t12*t42; + float t44 = P[5][0]*t8; + float t45 = P[6][0]*t12; + float t46 = P[1][0]*t18; + float t47 = P[2][0]*t22; + float t83 = P[4][0]*t3; + float t84 = P[3][0]*t25; + float t48 = t29+t44+t45+t46+t47-t83-t84; + float t49 = t15*t48; + float t50 = P[5][1]*t8; + float t51 = P[6][1]*t12; + float t52 = P[0][1]*t15; + float t53 = P[2][1]*t22; + float t85 = P[4][1]*t3; + float t86 = P[3][1]*t25; + float t54 = t30+t50+t51+t52+t53-t85-t86; + float t55 = t18*t54; + float t56 = P[5][2]*t8; + float t57 = P[6][2]*t12; + float t58 = P[0][2]*t15; + float t59 = P[1][2]*t18; + float t60 = P[2][2]*t22; + float t87 = P[4][2]*t3; + float t88 = P[3][2]*t25; + float t61 = t56+t57+t58+t59+t60-t87-t88; + float t62 = t22*t61; + float t63 = P[5][3]*t8; + float t64 = P[6][3]*t12; + float t65 = P[0][3]*t15; + float t66 = P[1][3]*t18; + float t67 = P[2][3]*t22; + float t89 = P[4][3]*t3; + float t90 = P[3][3]*t25; + float t68 = t63+t64+t65+t66+t67-t89-t90; + float t69 = P[5][5]*t8; + float t70 = P[6][5]*t12; + float t71 = P[0][5]*t15; + float t72 = P[1][5]*t18; + float t73 = P[2][5]*t22; + float t92 = P[4][5]*t3; + float t93 = P[3][5]*t25; + float t74 = t69+t70+t71+t72+t73-t92-t93; + float t75 = t8*t74; + float t80 = t3*t36; + float t91 = t25*t68; + float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91; + float t77; + + // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation + if (t76 > R_VEL) { + t77 = 1.0f/t76; + faultStatus.bad_yvel = false; + } else { + t76 = R_VEL; + t77 = 1.0f/R_VEL; + faultStatus.bad_yvel = true; + return; + } + varInnovBodyVel[1] = t77; + + // calculate innovation for Y axis observation + innovBodyVel[1] = bodyVelPred.y - bodyOdmDataDelayed.vel.y; + + // calculate Kalman gains for Y-axis observation + Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25); + Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25); + Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25); + Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22); + Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25); + Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25); + Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25); + Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25); + Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25); + Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25); + Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25); + Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25); + Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25); + Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25); + Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25); + Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25); + if (!inhibitMagStates) { + Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25); + Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25); + Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25); + Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25); + Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25); + Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + if (!inhibitWindStates) { + Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25); + Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + } else if (obsIndex == 2) { + // calculate Z axis observation Jacobian + H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f; + H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f; + H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f; + H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f; + H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f; + H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f; + H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3; + for (uint8_t index = 7; index < 24; index++) { + H_VEL[index] = 0.0f; + } + + // calculate intermediate expressions for Z axis Kalman gains + float R_VEL = bodyOdmDataDelayed.velErr; + float t2 = q0*q2*2.0f; + float t3 = q1*q3*2.0f; + float t4 = t2+t3; + float t5 = q0*q0; + float t6 = q1*q1; + float t7 = q2*q2; + float t8 = q3*q3; + float t9 = t5-t6-t7+t8; + float t10 = q0*q1*2.0f; + float t25 = q2*q3*2.0f; + float t11 = t10-t25; + float t12 = q0*vd*2.0f; + float t13 = q2*vn*2.0f; + float t26 = q1*ve*2.0f; + float t14 = t12+t13-t26; + float t15 = q1*vd*2.0f; + float t16 = q0*ve*2.0f; + float t27 = q3*vn*2.0f; + float t17 = t15+t16-t27; + float t18 = q3*ve*2.0f; + float t19 = q0*vn*2.0f; + float t28 = q2*vd*2.0f; + float t20 = t18+t19-t28; + float t21 = q3*vd*2.0f; + float t22 = q2*ve*2.0f; + float t23 = q1*vn*2.0f; + float t24 = t21+t22+t23; + float t29 = P[0][0]*t14; + float t30 = P[6][4]*t9; + float t31 = P[4][4]*t4; + float t32 = P[0][4]*t14; + float t33 = P[2][4]*t20; + float t34 = P[3][4]*t24; + float t78 = P[5][4]*t11; + float t79 = P[1][4]*t17; + float t35 = t30+t31+t32+t33+t34-t78-t79; + float t36 = t4*t35; + float t37 = P[6][5]*t9; + float t38 = P[4][5]*t4; + float t39 = P[0][5]*t14; + float t40 = P[2][5]*t20; + float t41 = P[3][5]*t24; + float t80 = P[5][5]*t11; + float t81 = P[1][5]*t17; + float t42 = t37+t38+t39+t40+t41-t80-t81; + float t43 = P[6][0]*t9; + float t44 = P[4][0]*t4; + float t45 = P[2][0]*t20; + float t46 = P[3][0]*t24; + float t83 = P[5][0]*t11; + float t84 = P[1][0]*t17; + float t47 = t29+t43+t44+t45+t46-t83-t84; + float t48 = t14*t47; + float t49 = P[6][1]*t9; + float t50 = P[4][1]*t4; + float t51 = P[0][1]*t14; + float t52 = P[2][1]*t20; + float t53 = P[3][1]*t24; + float t85 = P[5][1]*t11; + float t86 = P[1][1]*t17; + float t54 = t49+t50+t51+t52+t53-t85-t86; + float t55 = P[6][2]*t9; + float t56 = P[4][2]*t4; + float t57 = P[0][2]*t14; + float t58 = P[2][2]*t20; + float t59 = P[3][2]*t24; + float t88 = P[5][2]*t11; + float t89 = P[1][2]*t17; + float t60 = t55+t56+t57+t58+t59-t88-t89; + float t61 = t20*t60; + float t62 = P[6][3]*t9; + float t63 = P[4][3]*t4; + float t64 = P[0][3]*t14; + float t65 = P[2][3]*t20; + float t66 = P[3][3]*t24; + float t90 = P[5][3]*t11; + float t91 = P[1][3]*t17; + float t67 = t62+t63+t64+t65+t66-t90-t91; + float t68 = t24*t67; + float t69 = P[6][6]*t9; + float t70 = P[4][6]*t4; + float t71 = P[0][6]*t14; + float t72 = P[2][6]*t20; + float t73 = P[3][6]*t24; + float t92 = P[5][6]*t11; + float t93 = P[1][6]*t17; + float t74 = t69+t70+t71+t72+t73-t92-t93; + float t75 = t9*t74; + float t82 = t11*t42; + float t87 = t17*t54; + float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87; + float t77; + + // calculate innovation variance for Z axis observation and protect against a badly conditioned calculation + if (t76 > R_VEL) { + t77 = 1.0f/t76; + faultStatus.bad_zvel = false; + } else { + t76 = R_VEL; + t77 = 1.0f/R_VEL; + faultStatus.bad_zvel = true; + return; + } + varInnovBodyVel[2] = t77; + + // calculate innovation for Z axis observation + innovBodyVel[2] = bodyVelPred.z - bodyOdmDataDelayed.vel.z; + + // calculate Kalman gains for X-axis observation + Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24); + Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24); + Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24); + Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20); + Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24); + Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24); + Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24); + Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24); + Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24); + Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24); + Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24); + Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24); + Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24); + Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24); + Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24); + Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24); + if (!inhibitMagStates) { + Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24); + Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24); + Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24); + Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24); + Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24); + Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + if (!inhibitWindStates) { + Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24); + Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + } else { + return; + } + + // calculate the innovation consistency test ratio + // TODO add tuning parameter for gate + bodyVelTestRatio[obsIndex] = sq(innovBodyVel[obsIndex]) / (sq(5.0f) * varInnovBodyVel[obsIndex]); + + // Check the innovation for consistency and don't fuse if out of bounds + // TODO also apply angular velocity magnitude check + if ((bodyVelTestRatio[obsIndex]) < 1.0f) { + // record the last time observations were accepted for fusion + prevBodyVelFuseTime_ms = imuSampleTime_ms; + // notify first time only + if (!bodyVelFusionActive) { + bodyVelFusionActive = true; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing odometry",(unsigned)imu_index); + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=6; j++) { + KH[i][j] = Kfusion[i] * H_VEL[j]; + } + for (unsigned j = 7; j<=stateIndexLim; j++) { + KH[i][j] = 0.0f; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][0] * P[0][j]; + res += KH[i][1] * P[1][j]; + res += KH[i][2] * P[2][j]; + res += KH[i][3] * P[3][j]; + res += KH[i][4] * P[4][j]; + res += KH[i][5] * P[5][j]; + res += KH[i][6] * P[6][j]; + KHP[i][j] = res; + } + } + + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovBodyVel[obsIndex]; + } + stateStruct.quat.normalize(); + + } else { + // record bad axis + if (obsIndex == 0) { + faultStatus.bad_xvel = true; + } else if (obsIndex == 1) { + faultStatus.bad_yvel = true; + } else if (obsIndex == 2) { + faultStatus.bad_zvel = true; + } + + } + } + } +} + +// select fusion of body odometry measurements +void NavEKF3_core::SelectBodyOdomFusion() +{ + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz + // If so, don't fuse measurements on this time step to reduce frame over-runs + // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements + if (magFusePerformed && (dtIMUavg < 0.005f) && !bodyVelFusionDelayed) { + bodyVelFusionDelayed = true; + return; + } else { + bodyVelFusionDelayed = false; + } + + // Check for data at the fusion time horizon + if (storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms)) { + + // start performance timer + hal.util->perf_begin(_perf_FuseBodyOdom); + + // Fuse data into the main filter + FuseBodyVel(); + + // stop the performance timer + hal.util->perf_end(_perf_FuseBodyOdom); + } +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 5fe564e312..640cbe4d6b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -23,7 +23,8 @@ NavEKF3_core::NavEKF3_core(void) : _perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseAirspeed")), _perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseSideslip")), _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")), - _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")) + _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")), + _perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom")) { _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0"); _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1"); @@ -122,6 +123,9 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c if(!storedOF.init(obs_buffer_length)) { return false; } + if(!storedBodyOdm.init(obs_buffer_length)) { + return false; + } // Note: the use of dual range finders potentially doubles the amount of data to be stored if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) { return false; @@ -279,6 +283,7 @@ void NavEKF3_core::InitialiseVariables() posDown = 0.0f; posVelFusionDelayed = false; optFlowFusionDelayed = false; + flowFusionActive = false; airSpdFusionDelayed = false; sideSlipFusionDelayed = false; posResetNE.zero(); @@ -356,6 +361,19 @@ void NavEKF3_core::InitialiseVariables() bcnPosOffsetNED.zero(); bcnOriginEstInit = false; + // body frame displacement fusion + memset(&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew)); + memset(&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed)); + bodyOdmStoreIndex = 0; + lastbodyVelPassTime_ms = 0; + memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio)); + memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel)); + memset(&innovBodyVel, 0, sizeof(innovBodyVel)); + prevBodyVelFuseTime_ms = 0; + bodyOdmMeasTime_ms = 0; + bodyVelFusionDelayed = false; + bodyVelFusionActive = false; + // zero data buffers storedIMU.reset(); storedGPS.reset(); @@ -365,6 +383,7 @@ void NavEKF3_core::InitialiseVariables() storedRange.reset(); storedOutput.reset(); storedRangeBeacon.reset(); + storedBodyOdm.reset(); } // Initialise the states from accelerometer and magnetometer data (if present) @@ -548,6 +567,9 @@ void NavEKF3_core::UpdateFilter(bool predict) // Update states using optical flow data SelectFlowFusion(); + // Update states using body frame odometry data + SelectBodyOdomFusion(); + // Update states using airspeed data SelectTasFusion(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 28ac5504ae..ba7572c5f3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -202,6 +202,28 @@ public: // return data for debugging optical flow fusion void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; + /* + * Write body frame linear and angular displacement measurements from a visual odometry sensor + * + * quality is a normalised confidence value from 0 to 100 + * delPos is the XYZ change in linear position meaasured in body frame and relative to the inertial reference at time_ms (m) + * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad) + * delTime is the time interval for the measurement of delPos and delAng (sec) + * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec) + * posOffset is the XYZ body frame position of the camera focal point (m) + */ + void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset); + + /* + * Return data for debugging body frame odometry fusion: + * + * velInnov are the XYZ body frame velocity innovations (m/s) + * velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2 + * + * Return the time stamp of the last odometry fusion update (msec) + */ + uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar); + /* Returns the following data for debugging range beacon fusion ID : beacon identifier @@ -376,73 +398,81 @@ private: // memory) Vector24 statesArray; struct state_elements { - Quaternion quat; // 0..3 - Vector3f velocity; // 4..6 - Vector3f position; // 7..9 - Vector3f gyro_bias; // 10..12 - Vector3f accel_bias; // 13..15 - Vector3f earth_magfield; // 16..18 - Vector3f body_magfield; // 19..21 - Vector2f wind_vel; // 22..23 + Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame + Vector3f velocity; // velocity of IMU in local NED earth frame (m/sec) + Vector3f position; // position of IMU in local NED earth frame (m) + Vector3f gyro_bias; // body frame delta angle IMU bias vector (rad) + Vector3f accel_bias; // body frame delta velocity IMU bias vector (m/sec) + Vector3f earth_magfield; // earth frame magnetic field vector (Gauss) + Vector3f body_magfield; // body frame magnetic field vector (Gauss) + Vector2f wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) } &stateStruct; struct output_elements { - Quaternion quat; // 0..3 - Vector3f velocity; // 4..6 - Vector3f position; // 7..9 + Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame + Vector3f velocity; // velocity of body frame origin in local NED earth frame (m/sec) + Vector3f position; // position of body frame origin in local NED earth frame (m) }; struct imu_elements { - Vector3f delAng; // 0..2 - Vector3f delVel; // 3..5 - float delAngDT; // 6 - float delVelDT; // 7 - uint32_t time_ms; // 8 + Vector3f delAng; // IMU delta angle measurements in body frame (rad) + Vector3f delVel; // IMU delta velocity measurements in body frame (m/sec) + float delAngDT; // time interval over which delAng has been measured (sec) + float delVelDT; // time interval over which delVelDT has been measured (sec) + uint32_t time_ms; // measurement timestamp (msec) }; struct gps_elements { - Vector2f pos; // 0..1 - float hgt; // 2 - Vector3f vel; // 3..5 - uint32_t time_ms; // 6 - uint8_t sensor_idx; // 7 + Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m) + float hgt; // height of the GPS antenna in local NED earth frame (m) + Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec) + uint32_t time_ms; // measurement timestamp (msec) + uint8_t sensor_idx; // unique integer identifying the GPS sensor }; struct mag_elements { - Vector3f mag; // 0..2 - uint32_t time_ms; // 3 + Vector3f mag; // body frame magnetic field measurements (Gauss) + uint32_t time_ms; // measurement timestamp (msec) }; struct baro_elements { - float hgt; // 0 - uint32_t time_ms; // 1 + float hgt; // height of the pressure sensor in local NED earth frame (m) + uint32_t time_ms; // measurement timestamp (msec) }; struct range_elements { - float rng; // 0 - uint32_t time_ms; // 1 - uint8_t sensor_idx; // 2 + float rng; // distance measured by the range sensor (m) + uint32_t time_ms; // measurement timestamp (msec) + uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors }; struct rng_bcn_elements { - float rng; // range measurement to each beacon (m) - Vector3f beacon_posNED; // NED position of the beacon (m) - float rngErr; // range measurement error 1-std (m) - uint8_t beacon_ID; // beacon identification number - uint32_t time_ms; // measurement timestamp (msec) + float rng; // range measurement to each beacon (m) + Vector3f beacon_posNED; // NED position of the beacon (m) + float rngErr; // range measurement error 1-std (m) + uint8_t beacon_ID; // beacon identification number + uint32_t time_ms; // measurement timestamp (msec) }; struct tas_elements { - float tas; // 0 - uint32_t time_ms; // 1 + float tas; // true airspeed measurement (m/sec) + uint32_t time_ms; // measurement timestamp (msec) }; struct of_elements { - Vector2f flowRadXY; // 0..1 - Vector2f flowRadXYcomp; // 2..3 - uint32_t time_ms; // 4 - Vector3f bodyRadXYZ; //8..10 - const Vector3f *body_offset;// 5..7 + Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec) + Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec) + uint32_t time_ms; // measurement timestamp (msec) + Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec) + const Vector3f *body_offset;// pointer to XYZ position of the optical flow sensor in body frame (m) + }; + + struct bfodm_elements { + Vector3f vel; // XYZ velocity measured in body frame (m/s) + float velErr; // velocity measurement error 1-std (m/s) + const Vector3f *body_offset;// pointer to XYZ position of the velocity sensor in body frame (m) + Vector3f angRate; // angular rate estimated from odometry (rad/sec) + uint32_t time_ms; // measurement timestamp (msec) }; // update the navigation filter status @@ -469,6 +499,9 @@ private: // fuse selected position, velocity and height measurements void FuseVelPosNED(); + // fuse body frame velocity measurements + void FuseBodyVel(); + // fuse range beacon measurements void FuseRngBcn(); @@ -624,12 +657,18 @@ private: // return true if the filter is ready to start using optical flow measurements bool readyToUseOptFlow(void) const; + // return true if the filter is ready to start using body frame odometry measurements + bool readyToUseBodyOdm(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(); + // determine when to perform fusion of body frame odometry measurements + void SelectBodyOdomFusion(); + // Estimate terrain offset using a single state EKF void EstimateTerrainOffset(); @@ -986,8 +1025,9 @@ private: bool gndOffsetValid; // true when the ground offset state can still be considered valid Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement float delTimeOF; // time that delAngBodyOF is summed across - Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m) + bool flowFusionActive; // true when optical flow fusion is active + Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m) // Range finder float baroHgtOffset; // offset applied when when switching to use of Baro height @@ -999,6 +1039,21 @@ private: bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference uint32_t terrainHgtStableSet_ms; // system time at which terrainHgtStable was set + // body frame odometry fusion + obs_ring_buffer_t storedBodyOdm; // body velocity data buffer + bfodm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon + bfodm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon + uint8_t bodyOdmStoreIndex; // Body frame odometry data storage index + uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innvovation consistency checks (msec) + Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements + Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (rad/sec)^2 + Vector3 innovBodyVel; // Body velocity XYZ innovations (rad/sec) + uint32_t prevBodyVelFuseTime_ms; // previous time all body velocity measurement components passed their innovation consistency checks (msec) + uint32_t bodyOdmMeasTime_ms; // time body velocity measurements were accepted for input to the data buffer (msec) + bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed + bool bodyVelFusionActive; // true when body frame velocity fusion is active + + // Range Beacon Sensor Fusion obs_ring_buffer_t storedRangeBeacon; // Beacon range buffer rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon @@ -1094,6 +1149,9 @@ private: bool bad_xflow:1; bool bad_yflow:1; bool bad_rngbcn:1; + bool bad_xvel:1; + bool bad_yvel:1; + bool bad_zvel:1; } faultStatus; // flags indicating which GPS quality checks are failing @@ -1144,6 +1202,7 @@ private: AP_HAL::Util::perf_counter_t _perf_FuseSideslip; AP_HAL::Util::perf_counter_t _perf_TerrainOffset; AP_HAL::Util::perf_counter_t _perf_FuseOptFlow; + AP_HAL::Util::perf_counter_t _perf_FuseBodyOdom; AP_HAL::Util::perf_counter_t _perf_test[10]; // should we assume zero sideslip?