aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Enable fusion of external nav position data

Browse Source
This commit is contained in:
Paul Riseborough 2018-03-07 15:42:31 +11:00 committed by Randy Mackay
parent aae4ed2553
commit c680b931dc
9 changed files with 336 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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<num_cores; i++) {
core[i].writeExtNavData(sensOffset, pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
}
}
}
#endif //HAL_CPU_CLASS

13
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -323,6 +323,19 @@ public:
// get timing statistics structure
void getTimingStatistics(int8_t instance, struct ekf_timing &timing);
/*
* Write position and quaternion data from an external navigation system
*
* 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:
uint8_t num_cores; // number of allocated cores
uint8_t primary; // current primary core

32
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -169,7 +169,8 @@ void NavEKF2_core::setAidingMode()
// GPS aiding is the preferred option unless excluded by the user
bool canUseGPS = ((frontend->_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

148
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -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;
// 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;
} 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);
}
// 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

28
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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

4
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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

136
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -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<imu_buffer_length; i++) {
@ -232,35 +238,78 @@ void NavEKF2_core::SelectVelPosFusion()
posVelFusionDelayed = false;
}
// Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
// read GPS data from the sensor and check for new data in the buffer
readGpsData();
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// 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()) {
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;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gpsDataDelayed.pos.x -= posOffsetEarth.x;
gpsDataDelayed.pos.y -= posOffsetEarth.y;
gpsDataDelayed.hgt += posOffsetEarth.z;
}
// Don't fuse velocity data if GPS doesn't support it
// set fusion request flags
if (frontend->_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.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;
} 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 {
extNavUsedForYaw = false;
}
} 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));

17
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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)

44
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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<ext_nav_elements> 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;