mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-28 19:48:31 -04:00
AP_NavEKF3: add external nav system support
includes decoupling height source from use of external nav data
This commit is contained in:
priseborough
Randy Mackay
parent
471372cc7f
commit
c5e465aec9
@ -196,8 +196,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
||||
|
||||
// @Param: ALT_SOURCE
|
||||
// @DisplayName: Primary altitude sensor source
|
||||
// @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK2_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
|
||||
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
|
||||
// @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK3_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. Setting 4 uses external nav system data, but only if data is also being used for horizontal position
|
||||
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon, 4:Use External Nav
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("ALT_SOURCE", 9, NavEKF3, _altSource, 0),
|
||||
@ -1315,6 +1315,26 @@ void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t tim
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* 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 NavEKF3::writeExtNavData(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(pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// return data for debugging optical flow fusion
|
||||
void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov,
|
||||
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
|
||||
|
||||
@ -281,6 +281,19 @@ public:
|
||||
*/
|
||||
void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
|
||||
|
||||
/*
|
||||
* 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 &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
|
||||
|
||||
// called by vehicle code to specify that a takeoff is happening
|
||||
// causes the EKF to compensate for expected barometer errors due to ground effect
|
||||
void setTakeoffExpected(bool val);
|
||||
@ -439,7 +452,7 @@ private:
|
||||
AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec)
|
||||
AP_Int16 _rngInnovGate; // Percentage number of standard deviations applied to range finder innovation consistency check
|
||||
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
|
||||
AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
|
||||
AP_Int8 _altSource; // Primary alt source. 0 = Baro, 1 = range finder, 2 = GPS, 3 = range beacons, 4 = external nav
|
||||
AP_Float _rngNoise; // Range finder noise : m
|
||||
AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
|
||||
AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF3 for
|
||||
|
||||
@ -210,7 +210,7 @@ void NavEKF3_core::setAidingMode()
|
||||
// and IMU gyro bias estimates have stabilised
|
||||
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
||||
// GPS aiding is the preferred option unless excluded by the user
|
||||
if(readyToUseGPS() || readyToUseRangeBeacon()) {
|
||||
if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
|
||||
PV_AidingMode = AID_ABSOLUTE;
|
||||
} else if ((readyToUseOptFlow() && (frontend->_flowUse == FLOW_USE_NAV)) || readyToUseBodyOdm()) {
|
||||
PV_AidingMode = AID_RELATIVE;
|
||||
@ -251,6 +251,9 @@ void NavEKF3_core::setAidingMode()
|
||||
bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
|
||||
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
|
||||
|
||||
// Check if external nav position is being used
|
||||
bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms);
|
||||
|
||||
// Check if attitude drift has been constrained by a measurement source
|
||||
bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed;
|
||||
|
||||
@ -258,7 +261,7 @@ void NavEKF3_core::setAidingMode()
|
||||
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
|
||||
|
||||
// check if position drift has been constrained by a measurement source
|
||||
bool posAiding = gpsPosUsed || rngBcnUsed;
|
||||
bool posAiding = gpsPosUsed || rngBcnUsed || extNavUsed;
|
||||
|
||||
// Check if the loss of attitude aiding has become critical
|
||||
bool attAidLossCritical = false;
|
||||
@ -294,12 +297,14 @@ void NavEKF3_core::setAidingMode()
|
||||
rngBcnTimeout = true;
|
||||
tasTimeout = true;
|
||||
gpsNotAvailable = true;
|
||||
extNavTimeout = true;
|
||||
} else if (posAidLossCritical) {
|
||||
// if the loss of position is critical, declare all sources of position aiding as being timed out
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
rngBcnTimeout = true;
|
||||
gpsNotAvailable = true;
|
||||
extNavTimeout = true;
|
||||
|
||||
} else if (posAidLossPending) {
|
||||
// attempt to reset the yaw to the estimate from the EKF-GSF algorithm
|
||||
@ -364,6 +369,16 @@ void NavEKF3_core::setAidingMode()
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 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, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
|
||||
} else if (readyToUseExtNav()) {
|
||||
// we are commencing aiding using external nav
|
||||
posResetSource = EXTNAV;
|
||||
velResetSource = DEFAULT;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index);
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 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 height reset as special case
|
||||
hgtMea = -extNavDataDelayed.pos.z;
|
||||
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
|
||||
ResetHeight();
|
||||
}
|
||||
|
||||
// clear timeout flags as a precaution to avoid triggering any additional transitions
|
||||
@ -456,6 +471,12 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const
|
||||
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse;
|
||||
}
|
||||
|
||||
// return true if the filter is ready to use external nav data
|
||||
bool NavEKF3_core::readyToUseExtNav(void) const
|
||||
{
|
||||
return tiltAlignComplete && extNavDataToFuse;
|
||||
}
|
||||
|
||||
// return true if we should use the compass
|
||||
bool NavEKF3_core::use_compass(void) const
|
||||
{
|
||||
@ -489,7 +510,8 @@ bool NavEKF3_core::assume_zero_sideslip(void) const
|
||||
// set the LLH location of the filters NED origin
|
||||
bool NavEKF3_core::setOriginLLH(const Location &loc)
|
||||
{
|
||||
if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) {
|
||||
// reject attempt to set origin if GPS is being used
|
||||
return false;
|
||||
}
|
||||
EKF_origin = loc;
|
||||
|
||||
@ -920,7 +920,7 @@ void NavEKF3_core::FuseMagnetometer()
|
||||
* usePredictedYaw - Set this to true if no valid yaw measurement will be available for an extended periods.
|
||||
* This uses an innovation set to zero which prevents uncontrolled quaternion covariance
|
||||
* growth or if available, a yaw estimate from the Gaussian Sum Filter.
|
||||
* UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor is being used instead of the magnetometer.
|
||||
* UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor (GPS or external nav) is being used instead of the magnetometer.
|
||||
*/
|
||||
void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
|
||||
{
|
||||
|
||||
@ -936,6 +936,51 @@ void NavEKF3_core::writeDefaultAirSpeed(float airspeed)
|
||||
defaultAirSpeed = airspeed;
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* External Navigation Measurements *
|
||||
********************************************************/
|
||||
|
||||
void NavEKF3_core::writeExtNavData(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) || !statesInitialised) {
|
||||
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;
|
||||
if (posErr > 0) {
|
||||
extNavDataNew.posErr = posErr;
|
||||
} else {
|
||||
extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
|
||||
}
|
||||
|
||||
// calculate timestamp
|
||||
const uint32_t extnav_delay_ms = 10;
|
||||
timeStamp_ms = timeStamp_ms - extnav_delay_ms;
|
||||
// Correct for the average intersampling delay due to the filter update rate
|
||||
timeStamp_ms -= localFilterTimeStep_ms/2;
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
|
||||
extNavDataNew.time_ms = timeStamp_ms;
|
||||
|
||||
// extract yaw from the attitude
|
||||
float roll_rad, pitch_rad, yaw_rad;
|
||||
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
|
||||
writeEulerYawAngle(yaw_rad, angErr, timeStamp_ms, 2);
|
||||
|
||||
storedExtNav.push(extNavDataNew);
|
||||
}
|
||||
|
||||
/*
|
||||
update timing statistics structure
|
||||
*/
|
||||
|
||||
@ -111,7 +111,7 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
|
||||
bool NavEKF3_core::getHeightControlLimit(float &height) const
|
||||
{
|
||||
// only ask for limiting if we are doing optical flow navigation
|
||||
if (frontend->_fusionModeGPS == 3) {
|
||||
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
|
||||
const RangeFinder *_rng = AP::rangefinder();
|
||||
if (_rng == nullptr) {
|
||||
|
||||
@ -119,6 +119,15 @@ void NavEKF3_core::ResetPosition(void)
|
||||
// clear the timeout flags and counters
|
||||
rngBcnTimeout = false;
|
||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == DEFAULT) || posResetSource == EXTNAV) {
|
||||
// use external nav system data as the third preference
|
||||
stateStruct.position.x = extNavDataDelayed.pos.x;
|
||||
stateStruct.position.y = extNavDataDelayed.pos.y;
|
||||
// set the variances as received from external nav system data
|
||||
P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr);
|
||||
// clear the timeout flags and counters
|
||||
extNavTimeout = false;
|
||||
lastExtNavPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||
@ -284,6 +293,9 @@ void NavEKF3_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
|
||||
readGpsData();
|
||||
|
||||
@ -291,7 +303,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
||||
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) {
|
||||
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) {
|
||||
|
||||
// Don't fuse velocity data if GPS doesn't support it
|
||||
if (frontend->_fusionModeGPS <= 1) {
|
||||
@ -300,8 +312,28 @@ void NavEKF3_core::SelectVelPosFusion()
|
||||
fuseVelData = false;
|
||||
}
|
||||
fusePosData = true;
|
||||
extNavUsedForPos = false;
|
||||
|
||||
CorrectGPSForAntennaOffset(gpsDataDelayed);
|
||||
|
||||
// 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) && (frontend->_fusionModeGPS == 3)) {
|
||||
// use external nav system for position
|
||||
extNavUsedForPos = true;
|
||||
activeHgtSource = HGT_SOURCE_EXTNAV;
|
||||
fuseVelData = false;
|
||||
fuseHgtData = true;
|
||||
fusePosData = true;
|
||||
velPosObs[3] = extNavDataDelayed.pos.x;
|
||||
velPosObs[4] = extNavDataDelayed.pos.y;
|
||||
velPosObs[5] = extNavDataDelayed.pos.z;
|
||||
} else {
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
@ -369,13 +401,14 @@ void NavEKF3_core::SelectVelPosFusion()
|
||||
}
|
||||
}
|
||||
|
||||
// To-Do: add external nav position reset handling here?
|
||||
|
||||
// If we are operating without any aiding, fuse in the last known position
|
||||
// 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;
|
||||
@ -413,7 +446,6 @@ void NavEKF3_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
|
||||
@ -423,17 +455,11 @@ void NavEKF3_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;
|
||||
|
||||
// To-Do: this posErr should come from external nav when fusing external nav position
|
||||
|
||||
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
// Use different errors if operating without external aiding using an assumed position or velocity of zero
|
||||
if (PV_AidingMode == AID_NONE) {
|
||||
@ -463,6 +489,8 @@ void NavEKF3_core::FuseVelPosNED()
|
||||
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
|
||||
if (gpsPosAccuracy > 0.0f) {
|
||||
R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
|
||||
} else if (extNavUsedForPos) {
|
||||
R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f));
|
||||
} else {
|
||||
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||||
}
|
||||
@ -480,8 +508,8 @@ void NavEKF3_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];
|
||||
const float hgtErr = stateStruct.position.z - velPosObs[5];
|
||||
const 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[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
|
||||
badIMUdata = true;
|
||||
@ -494,8 +522,8 @@ void NavEKF3_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[7][7] + R_OBS_DATA_CHECKS[3];
|
||||
varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4];
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
@ -546,7 +574,7 @@ void NavEKF3_core::FuseVelPosNED()
|
||||
// velocity states start at index 4
|
||||
stateIndex = i + 4;
|
||||
// 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
|
||||
@ -580,7 +608,7 @@ void NavEKF3_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[9][9] + 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]);
|
||||
@ -639,16 +667,14 @@ void NavEKF3_core::FuseVelPosNED()
|
||||
stateIndex = 4 + obsIndex;
|
||||
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
|
||||
// adjust scaling on GPS measurement noise variances if not enough satellites
|
||||
if (obsIndex <= 2)
|
||||
{
|
||||
innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
|
||||
if (obsIndex <= 2) {
|
||||
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];
|
||||
} else if (obsIndex == 3 || obsIndex == 4) {
|
||||
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;
|
||||
|
||||
@ -814,7 +840,10 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
||||
|
||||
// select height source
|
||||
if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
|
||||
if (extNavUsedForPos && (frontend->_altSource == 4)) {
|
||||
// always use external navigation as the height source if using for position.
|
||||
activeHgtSource = HGT_SOURCE_EXTNAV;
|
||||
} else if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
|
||||
if (frontend->_altSource == 1) {
|
||||
// always use range finder
|
||||
activeHgtSource = HGT_SOURCE_RNG;
|
||||
@ -865,10 +894,11 @@ void NavEKF3_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_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000));
|
||||
if (lostRngHgt || lostGpsHgt || lostExtNavHgt) {
|
||||
activeHgtSource = HGT_SOURCE_BARO;
|
||||
}
|
||||
|
||||
@ -899,7 +929,10 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
}
|
||||
|
||||
// Select the height measurement source
|
||||
if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
|
||||
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
|
||||
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) {
|
||||
@ -907,6 +940,7 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
|
||||
// correct for terrain position relative to datum
|
||||
hgtMea -= terrainState;
|
||||
velPosObs[5] = -hgtMea;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise
|
||||
@ -920,6 +954,7 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
} else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) {
|
||||
// using GPS data
|
||||
hgtMea = gpsDataDelayed.hgt;
|
||||
velPosObs[5] = -hgtMea;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
|
||||
@ -944,6 +979,7 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
if (motorsArmed && getTakeoffExpected()) {
|
||||
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
|
||||
}
|
||||
velPosObs[5] = -hgtMea;
|
||||
} else {
|
||||
fuseHgtData = false;
|
||||
}
|
||||
|
||||
@ -140,6 +140,9 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
||||
if(!storedRangeBeacon.init(imu_buffer_length)) {
|
||||
return false;
|
||||
}
|
||||
if (!storedExtNav.init(obs_buffer_length)) {
|
||||
return false;
|
||||
}
|
||||
if(!storedIMU.init(imu_buffer_length)) {
|
||||
return false;
|
||||
}
|
||||
@ -329,6 +332,7 @@ void NavEKF3_core::InitialiseVariables()
|
||||
ekfGpsRefHgt = 0.0;
|
||||
velOffsetNED.zero();
|
||||
posOffsetNED.zero();
|
||||
memset(&velPosObs, 0, sizeof(velPosObs));
|
||||
posResetSource = DEFAULT;
|
||||
velResetSource = DEFAULT;
|
||||
|
||||
@ -390,6 +394,16 @@ void NavEKF3_core::InitialiseVariables()
|
||||
memset(&yawAngDataNew, 0, sizeof(yawAngDataNew));
|
||||
memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed));
|
||||
|
||||
// external nav data fusion
|
||||
memset(&extNavDataNew, 0, sizeof(extNavDataNew));
|
||||
memset(&extNavDataDelayed, 0, sizeof(extNavDataDelayed));
|
||||
extNavMeasTime_ms = 0;
|
||||
extNavLastPosResetTime_ms = 0;
|
||||
lastExtNavPassTime_ms = 0;
|
||||
extNavDataToFuse = false;
|
||||
extNavUsedForPos = false;
|
||||
extNavTimeout = false;
|
||||
|
||||
// zero data buffers
|
||||
storedIMU.reset();
|
||||
storedGPS.reset();
|
||||
@ -400,6 +414,7 @@ void NavEKF3_core::InitialiseVariables()
|
||||
storedRangeBeacon.reset();
|
||||
storedBodyOdm.reset();
|
||||
storedWheelOdm.reset();
|
||||
storedExtNav.reset();
|
||||
|
||||
// initialise pre-arm message
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");
|
||||
|
||||
@ -43,10 +43,11 @@
|
||||
#define MASK_GPS_HORIZ_SPD (1<<7)
|
||||
|
||||
// active height source
|
||||
#define HGT_SOURCE_BARO 0
|
||||
#define HGT_SOURCE_RNG 1
|
||||
#define HGT_SOURCE_GPS 2
|
||||
#define HGT_SOURCE_BCN 3
|
||||
#define HGT_SOURCE_BARO 0
|
||||
#define HGT_SOURCE_RNG 1
|
||||
#define HGT_SOURCE_GPS 2
|
||||
#define HGT_SOURCE_BCN 3
|
||||
#define HGT_SOURCE_EXTNAV 4
|
||||
|
||||
#define earthRate 0.000072921f // earth rotation rate (rad/sec)
|
||||
|
||||
@ -286,6 +287,19 @@ public:
|
||||
*/
|
||||
void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
|
||||
|
||||
/*
|
||||
* 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 &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
|
||||
|
||||
// called by vehicle code to specify that a takeoff is happening
|
||||
// causes the EKF to compensate for expected barometer errors due to ground effect
|
||||
void setTakeoffExpected(bool val);
|
||||
@ -572,6 +586,13 @@ private:
|
||||
uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX)
|
||||
};
|
||||
|
||||
struct ext_nav_elements {
|
||||
Vector3f pos; // XYZ position measured in a RH navigation frame (m)
|
||||
float posErr; // spherical poition measurement error 1-std (m)
|
||||
uint32_t time_ms; // measurement timestamp (msec)
|
||||
bool posReset; // true when the position measurement has been reset
|
||||
};
|
||||
|
||||
// bias estimates for the IMUs that are enabled but not being used
|
||||
// by this core.
|
||||
struct {
|
||||
@ -777,6 +798,9 @@ private:
|
||||
// return true if the filter is ready to start using body frame odometry measurements
|
||||
bool readyToUseBodyOdm(void) const;
|
||||
|
||||
// return true if the filter to be ready to use external nav data
|
||||
bool readyToUseExtNav(void) const;
|
||||
|
||||
// return true if we should use the range finder sensor
|
||||
bool useRngFinder(void) const;
|
||||
|
||||
@ -949,6 +973,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
|
||||
@ -978,7 +1003,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 lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
|
||||
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
|
||||
Vector2f lastKnownPositionNE; // last known position
|
||||
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
|
||||
@ -1089,7 +1114,8 @@ private:
|
||||
FLOW=3, // Use optical flow rates
|
||||
BARO=4, // Use Baro height
|
||||
MAG=5, // Use magnetometer data
|
||||
RNGFND=6 // Use rangefinder data
|
||||
RNGFND=6, // Use rangefinder data
|
||||
EXTNAV=7 // Use external nav data
|
||||
};
|
||||
resetDataSource posResetSource; // preferred source of data for position reset
|
||||
resetDataSource velResetSource; // preferred source of data for a velocity reset
|
||||
@ -1292,6 +1318,17 @@ private:
|
||||
bool onGroundNotMoving; // true when on the ground and not moving
|
||||
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
|
||||
|
||||
// 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)
|
||||
uint32_t lastExtNavPassTime_ms; // time stamp when external nav position measurement last passed innovation consistency check (msec)
|
||||
bool extNavDataToFuse; // true when there is new external nav data to fuse
|
||||
bool extNavUsedForPos; // true when the external nav data is being used as a position reference.
|
||||
bool extNavTimeout; // true if external nav measurements have failed innovation consistency checks for too long
|
||||
|
||||
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
|
||||
struct {
|
||||
bool bad_xmag:1;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user