diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 0ec84e15e7..46e91bd304 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1468,4 +1468,24 @@ void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing) } } +/* + * Write position and quaternion data from an external navigation system + * + * pos : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw. + * quat : quaternion describing the the rotation from navigation frame to body frame + * posErr : 1-sigma spherical position error (m) + * angErr : 1-sigma spherical angle error (rad) + * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) + * resetTime_ms : system time of the last position reset request (mSec) + * +*/ +void NavEKF2::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) +{ + if (core) { + for (uint8_t i=0; i_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable; - if(canUseGPS || canUseRangeBeacon) { + bool canUseExtNav = readyToUseExtNav(); + if(canUseGPS || canUseRangeBeacon || canUseExtNav) { PV_AidingMode = AID_ABSOLUTE; } else if (optFlowDataPresent() && filterIsStable) { PV_AidingMode = AID_RELATIVE; @@ -206,17 +207,17 @@ void NavEKF2_core::setAidingMode() bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms); // Check if GPS is being used - bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); + bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); 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 = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; // check if velocity drift has been constrained by a measurement source bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed; // check if position drift has been constrained by a measurement source - bool posAiding = gpsPosUsed || rngBcnUsed; + bool posAiding = posUsed || rngBcnUsed; // Check if the loss of attitude aiding has become critical bool attAidLossCritical = false; @@ -300,6 +301,7 @@ void NavEKF2_core::setAidingMode() case AID_ABSOLUTE: { bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon(); + bool canUseExtNav = readyToUseExtNav(); // We have commenced aiding and GPS usage is allowed if (canUseGPS) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index); @@ -312,6 +314,18 @@ void NavEKF2_core::setAidingMode() gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); } + // We have commenced aiding and external nav usage is allowed + if (canUseExtNav) { + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); + // handle yaw reset as special case + extNavYawResetRequest = true; + controlMagYawReset(); + // handle height reset as special case + hgtMea = -extNavDataDelayed.pos.z; + posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); + ResetHeight(); + } // reset the last fusion accepted times to prevent unwanted activation of timeout logic lastPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; @@ -385,6 +399,12 @@ bool NavEKF2_core::readyToUseRangeBeacon(void) const return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse; } +// return true if the filter to be ready to use external nav data +bool NavEKF2_core::readyToUseExtNav(void) const +{ + return tiltAlignComplete && extNavDataToFuse; +} + // return true if we should use the compass bool NavEKF2_core::use_compass(void) const { @@ -405,7 +425,7 @@ bool NavEKF2_core::assume_zero_sideslip(void) const // set the LLH location of the filters NED origin bool NavEKF2_core::setOriginLLH(const Location &loc) { - if (PV_AidingMode == AID_ABSOLUTE) { + if (PV_AidingMode == AID_ABSOLUTE && !extNavUsedForPos) { return false; } EKF_origin = loc; @@ -500,7 +520,7 @@ void NavEKF2_core::updateFilterStatus(void) 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 filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); - filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching + filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // The GPS is glitching } #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index bd769120ce..9c5d3f9319 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -87,23 +87,23 @@ void NavEKF2_core::controlMagYawReset() finalResetRequest; // the final reset when we have acheived enough height to be in stable magnetic field environment // Perform a reset of magnetic field states and reset yaw to corrected magnetic heading - if (magYawResetRequest || magStateResetRequest) { - - // get the euler angles from the current state estimate - Vector3f eulerAngles; - stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); - - // Use the Euler angles and magnetometer measurement to update the magnetic field states - // and get an updated quaternion - Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); + if (magYawResetRequest || magStateResetRequest || extNavYawResetRequest) { // if a yaw reset has been requested, apply the updated quaternion to the current state - if (magYawResetRequest) { + if (extNavYawResetRequest) { + // get the euler angles from the current state estimate + Vector3f eulerAnglesOld; + stateStruct.quat.to_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesOld.z); + // previous value used to calculate a reset delta Quaternion prevQuat = stateStruct.quat; - // update the quaternion states using the new yaw angle - stateStruct.quat = newQuat; + // Get the Euler angles from the external vision data + Vector3f eulerAnglesNew; + extNavDataDelayed.quat.to_euler(eulerAnglesNew.x, eulerAnglesNew.y, eulerAnglesNew.z); + + // the new quaternion uses the old roll/pitch and new yaw angle + stateStruct.quat.from_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesNew.z); // calculate the change in the quaternion state and apply it to the ouput history buffer prevQuat = stateStruct.quat/prevQuat; @@ -111,27 +111,61 @@ void NavEKF2_core::controlMagYawReset() // send initial alignment status to console if (!yawAlignComplete) { - gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u ext nav yaw alignment complete",(unsigned)imu_index); } - // send in-flight yaw alignment status to console - if (finalResetRequest) { - gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index); - } else if (interimResetRequest) { - gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index); - } - - // update the yaw reset completed status - recordYawReset(); + // record the reset as complete and also record the in-flight reset as complete to stop further resets when hight is gained + // in-flight reset is unnecessary because we do not need to consider groudn based magnetic anomaly effects + yawAlignComplete = true; + finalInflightYawInit = true; // clear the yaw reset request flag - magYawResetRequest = false; + extNavYawResetRequest = false; + + } else if (magYawResetRequest || magStateResetRequest) { + // get the euler angles from the current state estimate + Vector3f eulerAngles; + stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); + + // Use the Euler angles and magnetometer measurement to update the magnetic field states + // and get an updated quaternion + Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); + + if (magYawResetRequest) { + // previous value used to calculate a reset delta + Quaternion prevQuat = stateStruct.quat; + + // update the quaternion states using the new yaw angle + stateStruct.quat = newQuat; + + // calculate the change in the quaternion state and apply it to the ouput history buffer + prevQuat = stateStruct.quat/prevQuat; + StoreQuatRotate(prevQuat); + + // send initial alignment status to console + if (!yawAlignComplete) { + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index); + } + + // send in-flight yaw alignment status to console + if (finalResetRequest) { + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index); + } else if (interimResetRequest) { + gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index); + } - // clear the complete flags if an interim reset has been performed to allow subsequent - // and final reset to occur - if (interimResetRequest) { - finalInflightYawInit = false; - finalInflightMagInit = false; + // update the yaw reset completed status + recordYawReset(); + + // clear the yaw reset request flag + magYawResetRequest = false; + + // clear the complete flags if an interim reset has been performed to allow subsequent + // and final reset to occur + if (interimResetRequest) { + finalInflightYawInit = false; + finalInflightMagInit = false; + } } } } @@ -256,11 +290,10 @@ void NavEKF2_core::SelectMagFusion() // If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the // filter covariances from becoming badly conditioned if (!use_compass()) { - if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) { + if (onGround && (imuSampleTime_ms - lastYawTime_ms > 1000)) { fuseEulerYaw(); magTestRatio.zero(); yawTestRatio = 0.0f; - lastSynthYawTime_ms = imuSampleTime_ms; } } @@ -739,6 +772,7 @@ void NavEKF2_core::fuseEulerYaw() // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix // determine if a 321 or 312 Euler sequence is best float predicted_yaw; + float measured_yaw; float H_YAW[3]; Matrix3f Tbn_zeroYaw; if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { @@ -771,14 +805,23 @@ void NavEKF2_core::fuseEulerYaw() H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f)); H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); - // Get the 321 euler angles + // calculate predicted and measured yaw angle Vector3f euler321; stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); predicted_yaw = euler321.z; - - // set the yaw to zero and calculate the zero yaw rotation from body to earth frame - Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); - + if (use_compass() && yawAlignComplete && magStateInitComplete) { + // Use measured mag components rotated into earth frame to measure yaw + Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); + Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + } else if (extNavUsedForYaw) { + // Get the yaw angle from the external vision data + extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z); + measured_yaw = euler321.z; + } else { + // no data so use predicted to prevent unconstrained variance growth + measured_yaw = predicted_yaw; + } } else { // calculate observaton jacobian when we are observing a rotation in a 312 sequence float t2 = q0*q0; @@ -809,25 +852,22 @@ void NavEKF2_core::fuseEulerYaw() H_YAW[1] = 0.0f; H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10)); - // Get the 321 euler angles + // calculate predicted and measured yaw angle Vector3f euler312 = stateStruct.quat.to_vector312(); predicted_yaw = euler312.z; - - // set the yaw to zero and calculate the zero yaw rotation from body to earth frame - Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); - } - - // rotate measured mag components into earth frame - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - - // Use the difference between the horizontal projection and declination to give the measured yaw - // If we can't use compass data, set the meaurement to the predicted - // to prevent uncontrolled variance growth whilst on ground without magnetometer - float measured_yaw; - if (use_compass() && yawAlignComplete && magStateInitComplete) { - measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); - } else { - measured_yaw = predicted_yaw; + if (use_compass() && yawAlignComplete && magStateInitComplete) { + // Use measured mag components rotated into earth frame to measure yaw + Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); + Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + } else if (extNavUsedForYaw) { + // Get the yaw angle from the external vision data + euler312 = extNavDataDelayed.quat.to_vector312(); + measured_yaw = euler312.z; + } else { + // no data so use predicted to prevent unconstrained variance growth + measured_yaw = predicted_yaw; + } } // Calculate the innovation @@ -938,8 +978,10 @@ void NavEKF2_core::fuseEulerYaw() // is used to correct the estimated quaternion on the current time step stateStruct.quat.rotate(stateStruct.angErr); - // record fusion numerical health status + // record fusion event faultStatus.bad_yaw = false; + lastYawTime_ms = imuSampleTime_ms; + } else { // record fusion numerical health status diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 964cacf5f1..8099f0d41d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -803,4 +803,32 @@ void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing) memset(&timing, 0, sizeof(timing)); } +void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) +{ + // 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 - extNavMeasTime_ms) < 70) { + return; + } else { + extNavMeasTime_ms = timeStamp_ms; + } + + if (resetTime_ms > extNavLastPosResetTime_ms) { + extNavDataNew.posReset = true; + extNavLastPosResetTime_ms = resetTime_ms; + } else { + extNavDataNew.posReset = false; + } + + extNavDataNew.pos = pos; + extNavDataNew.quat = quat; + extNavDataNew.posErr = posErr; + extNavDataNew.angErr = angErr; + extNavDataNew.body_offset = &sensOffset; + extNavDataNew.time_ms = timeStamp_ms; + + storedExtNav.push(extNavDataNew); + +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index e1a5efd6a1..76da95606d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -101,8 +101,8 @@ bool NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl // this is needed to ensure the vehicle does not fly too high when using optical flow navigation bool NavEKF2_core::getHeightControlLimit(float &height) const { - // only ask for limiting if we are doing optical flow navigation - if (frontend->_fusionModeGPS == 3) { + // only ask for limiting if we are doing optical flow only navigation + if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors height = MAX(float(frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 715f755469..fc09bc6bd7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -111,6 +111,12 @@ void NavEKF2_core::ResetPosition(void) // clear the timeout flags and counters rngBcnTimeout = false; lastRngBcnPassTime_ms = imuSampleTime_ms; + } else if (imuSampleTime_ms - extNavDataDelayed.time_ms < 250) { + // use the range beacon data as a second preference + stateStruct.position.x = extNavDataDelayed.pos.x; + stateStruct.position.y = extNavDataDelayed.pos.y; + // set the variances from the beacon alignment filter + P[7][7] = P[6][6] = sq(extNavDataDelayed.posErr); } } for (uint8_t i=0; i_fusionModeGPS <= 1) { + fuseVelData = true; + } else { + fuseVelData = false; + } + fusePosData = true; + extNavUsedForPos = false; + // correct GPS data for position offset of antenna phase centre relative to the IMU Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; if (!posOffsetBody.is_zero()) { + // Don't fuse velocity data if GPS doesn't support it if (fuseVelData) { // TODO use a filtered angular rate with a group delay that matches the GPS delay Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); Vector3f velOffsetBody = angRate % posOffsetBody; Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); - gpsDataDelayed.vel -= velOffsetEarth; + gpsDataDelayed.vel.x -= velOffsetEarth.x; + gpsDataDelayed.vel.y -= velOffsetEarth.y; + gpsDataDelayed.vel.z -= velOffsetEarth.z; } + Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); gpsDataDelayed.pos.x -= posOffsetEarth.x; gpsDataDelayed.pos.y -= posOffsetEarth.y; gpsDataDelayed.hgt += posOffsetEarth.z; } + // copy corrected GPS data to observation vector + if (fuseVelData) { + velPosObs[0] = gpsDataDelayed.vel.x; + velPosObs[1] = gpsDataDelayed.vel.y; + velPosObs[2] = gpsDataDelayed.vel.z; + } + velPosObs[3] = gpsDataDelayed.pos.x; + velPosObs[4] = gpsDataDelayed.pos.y; - // Don't fuse velocity data if GPS doesn't support it - if (frontend->_fusionModeGPS <= 1) { - fuseVelData = true; + } else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) { + // This is a special case that uses and external nav system for position + extNavUsedForPos = true; + activeHgtSource = HGT_SOURCE_EV; + fuseVelData = false; + fuseHgtData = true; + fusePosData = true; + velPosObs[3] = extNavDataDelayed.pos.x; + velPosObs[4] = extNavDataDelayed.pos.y; + velPosObs[5] = extNavDataDelayed.pos.z; + + // if compass is disabled, also use it for yaw + if (!use_compass()) { + extNavUsedForYaw = true; + if (!yawAlignComplete) { + extNavYawResetRequest = true; + magYawResetRequest = false; + gpsYawResetRequest = false; + controlMagYawReset(); + finalInflightYawInit = true; + } else { + fuseEulerYaw(); + } } else { - fuseVelData = false; + extNavUsedForYaw = false; } - fusePosData = true; } else { fuseVelData = false; @@ -273,6 +322,7 @@ void NavEKF2_core::SelectVelPosFusion() } // Select height data to be fused from the available baro, range finder and GPS sources + selectHeightForFusion(); // if we are using GPS, check for a change in receiver and reset position and height @@ -332,10 +382,8 @@ void NavEKF2_core::SelectVelPosFusion() // to constrain tilt drift. This assumes a non-manoeuvring vehicle // Do this to coincide with the height fusion if (fuseHgtData && PV_AidingMode == AID_NONE) { - gpsDataDelayed.vel.zero(); - gpsDataDelayed.pos.x = lastKnownPositionNE.x; - gpsDataDelayed.pos.y = lastKnownPositionNE.y; - + velPosObs[3] = lastKnownPositionNE.x; + velPosObs[4] = lastKnownPositionNE.y; fusePosData = true; fuseVelData = false; } @@ -372,7 +420,6 @@ void NavEKF2_core::FuseVelPosNED() // declare variables used by state and covariance update calculations Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only - Vector6 observation; float SK; // perform sequential fusion of GPS measurements. This assumes that the @@ -382,13 +429,6 @@ void NavEKF2_core::FuseVelPosNED() // so we might as well take advantage of the computational efficiencies // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { - // form the observation vector - observation[0] = gpsDataDelayed.vel.x; - observation[1] = gpsDataDelayed.vel.y; - observation[2] = gpsDataDelayed.vel.z; - observation[3] = gpsDataDelayed.pos.x; - observation[4] = gpsDataDelayed.pos.y; - observation[5] = -hgtMea; // calculate additional error in GPS position caused by manoeuvring float posErr = frontend->gpsPosVarAccScale * accNavMag; @@ -439,8 +479,8 @@ void NavEKF2_core::FuseVelPosNED() // the accelerometers and we should disable the GPS and barometer innovation consistency checks. if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { // calculate innovations for height and vertical GPS vel measurements - float hgtErr = stateStruct.position.z - observation[5]; - float velDErr = stateStruct.velocity.z - observation[2]; + float hgtErr = stateStruct.position.z - velPosObs[5]; + float velDErr = stateStruct.velocity.z - velPosObs[2]; // check if they are the same sign and both more than 3-sigma out of bounds if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) { badIMUdata = true; @@ -453,8 +493,8 @@ void NavEKF2_core::FuseVelPosNED() // test position measurements if (fusePosData) { // test horizontal position measurements - innovVelPos[3] = stateStruct.position.x - observation[3]; - innovVelPos[4] = stateStruct.position.y - observation[4]; + innovVelPos[3] = stateStruct.position.x - velPosObs[3]; + innovVelPos[4] = stateStruct.position.y - velPosObs[4]; varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; // apply an innovation consistency threshold test, but don't fail if bad IMU data @@ -505,7 +545,7 @@ void NavEKF2_core::FuseVelPosNED() // velocity states start at index 3 stateIndex = i + 3; // calculate innovations using blended and single IMU predicted states - velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended + velInnov[i] = stateStruct.velocity[i] - velPosObs[i]; // blended // calculate innovation variance varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; // sum the innovation and innovation variances @@ -539,7 +579,7 @@ void NavEKF2_core::FuseVelPosNED() // test height measurements if (fuseHgtData) { // calculate height innovations - innovVelPos[5] = stateStruct.position.z - observation[5]; + innovVelPos[5] = stateStruct.position.z - velPosObs[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); @@ -595,14 +635,14 @@ void NavEKF2_core::FuseVelPosNED() // adjust scaling on GPS measurement noise variances if not enough satellites if (obsIndex <= 2) { - innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex]; + innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 3 || obsIndex == 4) { - innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; + innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { - innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; + innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; @@ -760,7 +800,10 @@ void NavEKF2_core::selectHeightForFusion() baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); // select height source - if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + if (extNavUsedForPos) { + // always use external vision as the hight source if using for position. + activeHgtSource = HGT_SOURCE_EV; + } else if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { if (frontend->_altSource == 1) { // always use range finder activeHgtSource = HGT_SOURCE_RNG; @@ -803,10 +846,11 @@ void NavEKF2_core::selectHeightForFusion() activeHgtSource = HGT_SOURCE_BARO; } - // Use Baro alt as a fallback if we lose range finder or GPS + // Use Baro alt as a fallback if we lose range finder, GPS or external nav bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500)); bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); - if (lostRngHgt || lostGpsHgt) { + bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000)); + if (lostRngHgt || lostGpsHgt || lostExtNavHgt) { activeHgtSource = HGT_SOURCE_BARO; } @@ -837,7 +881,10 @@ void NavEKF2_core::selectHeightForFusion() } // Select the height measurement source - if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { + if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EV)) { + hgtMea = -extNavDataDelayed.pos.z; + posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); + } else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { // using range finder data // correct for tilt using a flat earth model if (prevTnb.c.z >= 0.7) { @@ -847,6 +894,7 @@ void NavEKF2_core::selectHeightForFusion() hgtMea -= terrainState; // enable fusion fuseHgtData = true; + velPosObs[5] = -hgtMea; // set the observation noise posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f)); // add uncertainty created by terrain gradient and vehicle tilt @@ -859,6 +907,7 @@ void NavEKF2_core::selectHeightForFusion() // using GPS data hgtMea = gpsDataDelayed.hgt; // enable fusion + velPosObs[5] = -hgtMea; fuseHgtData = true; // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio if (gpsHgtAccuracy > 0.0f) { @@ -870,6 +919,7 @@ void NavEKF2_core::selectHeightForFusion() // using Baro data hgtMea = baroDataDelayed.hgt - baroHgtOffset; // enable fusion + velPosObs[5] = -hgtMea; fuseHgtData = true; // set the observation noise posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 870548eeeb..b45aaa70cf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -91,6 +91,9 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c if(!storedRangeBeacon.init(imu_buffer_length)) { return false; } + if(!storedExtNav.init(OBS_BUFFER_LENGTH)) { + return false; + } if(!storedIMU.init(imu_buffer_length)) { return false; } @@ -125,7 +128,7 @@ void NavEKF2_core::InitialiseVariables() lastPosPassTime_ms = 0; lastHgtPassTime_ms = 0; lastTasPassTime_ms = 0; - lastSynthYawTime_ms = imuSampleTime_ms; + lastYawTime_ms = imuSampleTime_ms; lastTimeGpsReceived_ms = 0; secondLastGpsTime_ms = 0; lastDecayTime_ms = imuSampleTime_ms; @@ -277,6 +280,7 @@ void NavEKF2_core::InitialiseVariables() ekfGpsRefHgt = 0.0; velOffsetNED.zero(); posOffsetNED.zero(); + memset(&velPosObs, 0, sizeof(velPosObs)); // range beacon fusion variables memset(&rngBcnDataNew, 0, sizeof(rngBcnDataNew)); @@ -317,6 +321,16 @@ void NavEKF2_core::InitialiseVariables() memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport)); last_gps_idx = 0; + // external nav data fusion + memset(&extNavDataNew, 0, sizeof(extNavDataNew)); + memset(&extNavDataDelayed, 0, sizeof(extNavDataDelayed)); + extNavDataToFuse = false; + extNavMeasTime_ms = 0; + extNavLastPosResetTime_ms = 0; + extNavUsedForYaw = false; + extNavUsedForPos = false; + extNavYawResetRequest = false; + // zero data buffers storedIMU.reset(); storedGPS.reset(); @@ -326,6 +340,7 @@ void NavEKF2_core::InitialiseVariables() storedRange.reset(); storedOutput.reset(); storedRangeBeacon.reset(); + storedExtNav.reset(); } // Initialise the states from accelerometer and magnetometer data (if present) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 6c20958d7d..f17b95ef63 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -46,6 +46,7 @@ #define HGT_SOURCE_RNG 1 #define HGT_SOURCE_GPS 2 #define HGT_SOURCE_BCN 3 +#define HGT_SOURCE_EV 4 // target EKF update time step #define EKF_TARGET_DT 0.01f @@ -306,6 +307,20 @@ public: // get timing statistics structure void getTimingStatistics(struct ekf_timing &timing); + /* + * Write position and quaternion data from an external navigation system + * + * sensOffset : position of the external navigatoin sensor in body frame (m) + * pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m) + * quat : quaternion desribing the the rotation from navigation frame to body frame + * posErr : 1-sigma spherical position error (m) + * angErr : 1-sigma spherical angle error (rad) + * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) + * resetTime_ms : system time of the last position reset request (mSec) + * + */ + void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms); + private: // Reference to the global EKF frontend for parameters NavEKF2 *frontend; @@ -441,6 +456,18 @@ private: const Vector3f *body_offset;// 5..7 }; + struct ext_nav_elements { + bool frameIsNED; // true if the data is in a NED navigation frame + bool unitsAreSI; // true if the data length units are scaled in metres + Vector3f pos; // XYZ position measured in a RH navigation frame (m) + Quaternion quat; // quaternion describing the rotation from navigation to body frame + float posErr; // spherical poition measurement error 1-std (m) + float angErr; // spherical angular measurement error 1-std (rad) + const Vector3f *body_offset;// pointer to XYZ position of the sensor in body frame (m) + uint32_t time_ms; // measurement timestamp (msec) + bool posReset; // true when the position measurement has been reset + }; + // update the navigation filter status void updateFilterStatus(void); @@ -611,6 +638,9 @@ private: // return true if the filter to be ready to use the beacon range measurements bool readyToUseRangeBeacon(void) const; + // return true if the filter to be ready to use external nav data + bool readyToUseExtNav(void) const; + // Check for filter divergence void checkDivergence(void); @@ -762,6 +792,7 @@ private: uint32_t airborneDetectTime_ms; // last time flight movement was detected Vector6 innovVelPos; // innovation output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements + Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s) bool fuseVelData; // this boolean causes the velNED measurements to be fused bool fusePosData; // this boolean causes the posNE measurements to be fused bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused @@ -789,7 +820,7 @@ private: uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data - uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec) + uint32_t lastYawTime_ms; // time stamp when yaw observation was last fused (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec) Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix @@ -1052,6 +1083,17 @@ private: float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad) Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset + // external navigation fusion + obs_ring_buffer_t storedExtNav; // external navigation data buffer + ext_nav_elements extNavDataNew; // External nav data at the current time horizon + ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon + uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec) + uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec) + bool extNavDataToFuse; // true when there is new external nav data to fuse + bool extNavUsedForYaw; // true when the external nav data is also being used as a yaw observation + bool extNavUsedForPos; // true when the external nav data is being used as a position reference. + bool extNavYawResetRequest; // true when a reset of vehicle yaw using the external nav data is requested + // flags indicating severe numerical errors in innovation variance calculation for different fusion operations struct { bool bad_xmag:1;