Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 23:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Allow range beacon use with intermittent GPS

Browse Source
This commit is contained in:
priseborough 2016-12-17 12:22:07 +11:00 committed by Randy Mackay
parent e5e4883c72
commit 7c53573a0c
7 changed files with 89 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -182,15 +182,16 @@ void NavEKF3_core::setAidingMode()
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode;
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
// Determine if we should change aiding mode
if (PV_AidingMode == AID_NONE) {
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
// and IMU gyro bias estimates have stabilised
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
// 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
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
if(canUseGPS || canUseRangeBeacon) {
PV_AidingMode = AID_ABSOLUTE;
} else if (optFlowDataPresent() && filterIsStable) {
@ -201,9 +202,9 @@ void NavEKF3_core::setAidingMode()
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS is available
// If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
if(canUseGPS || canUseRangeBeacon) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
@ -306,20 +307,24 @@ void NavEKF3_core::setAidingMode()
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
} else if (PV_AidingMode == AID_ABSOLUTE) {
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon();
// We have commenced aiding and GPS usage is allowed
if (canUseGPS) {
// We are commencing aiding using GPS - this is the preferred method
posResetSource = GPS;
velResetSource = GPS;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
}
posTimeout = false;
velTimeout = false;
// We have commenced aiding and range beacon usage is allowed
if (canUseRangeBeacon) {
} else if (canUseRangeBeacon) {
// We are commencing aiding using range beacons
posResetSource = RNGBCN;
velResetSource = DEFAULT;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosDownOffset);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
}
// clear timeout flags as a precaution to avoid triggering any additional transitions
posTimeout = false;
velTimeout = false;
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;

4
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -178,8 +178,10 @@ void NavEKF3_core::realignYawGPS()
// calculate new filter quaternion states from Euler angles
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
// reset the velocity and position states as they will be inaccurate due to bad yaw
velResetSource = GPS;
ResetVelocity();
posResetSource = GPS;
ResetPosition();
// set the yaw angle variance to a larger value to reflect the uncertainty in yaw

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -755,6 +755,11 @@ void NavEKF3_core::readRngBcnData()
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
// Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
if (rngBcnDataToFuse) {
rngBcnDataDelayed.beacon_posNED += bcnPosOffsetNED;
}
}
#endif // HAL_CPU_CLASS

11
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -35,7 +35,7 @@ void NavEKF3_core::ResetVelocity(void)
P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
} else {
// reset horizontal velocity states to the GPS velocity if available
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) {
stateStruct.velocity.x = gpsDataNew.vel.x;
stateStruct.velocity.y = gpsDataNew.vel.y;
// set the variances using the reported GPS speed accuracy
@ -69,6 +69,8 @@ void NavEKF3_core::ResetVelocity(void)
// store the time of the reset
lastVelReset_ms = imuSampleTime_ms;
// clear reset data source preference
velResetSource = DEFAULT;
}
@ -91,7 +93,7 @@ void NavEKF3_core::ResetPosition(void)
P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
} else {
// Use GPS data as first preference if fresh data is available
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) {
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
@ -100,7 +102,7 @@ void NavEKF3_core::ResetPosition(void)
// clear the timeout flags and counters
posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
} else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) {
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == DEFAULT) || posResetSource == RNGBCN) {
// use the range beacon data as a second preference
stateStruct.position.x = receiverPos.x;
stateStruct.position.y = receiverPos.y;
@ -128,6 +130,9 @@ void NavEKF3_core::ResetPosition(void)
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// clear reset source preference
posResetSource = DEFAULT;
}
// reset the vertical position state using the last height measurement

39
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -25,11 +25,27 @@ void NavEKF3_core::SelectRngBcnFusion()
// Determine if we need to fuse range beacon data on this time step
if (rngBcnDataToFuse) {
if (PV_AidingMode == AID_ABSOLUTE) {
// Normal operating mode is to fuse the range data into the main filter
FuseRngBcn();
if (!filterStatus.flags.using_gps) {
if (!bcnOriginEstInit && rngBcnAlignmentCompleted) {
bcnOriginEstInit = true;
bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x;
bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y;
}
// If we aren't using GPS, then the beacons are used as the primary means of position reference
FuseRngBcn();
} else {
// If we are using GPS, then GPS is the primary reference, but we continue to use the beacon data
// to calculate an indpendant position that is used to update the beacon position offset if we need to
// start using beacon data as the primary reference.
FuseRngBcnStatic();
// record that the beacon origin needs to be initialised
bcnOriginEstInit = false;
}
} else {
// If we aren't able to use the data in the main filter, use a simple 3-state filter to estimte position only
// If we aren't able to use the data in the main filter, use a simple 3-state filter to estimate position only
FuseRngBcnStatic();
// record that the beacon origin needs to be initialised
bcnOriginEstInit = false;
}
}
}
@ -53,7 +69,7 @@ void NavEKF3_core::FuseRngBcn()
// calculate the vertical offset from EKF datum to beacon datum
CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
} else {
bcnPosDownOffset = 0.0f;
bcnPosOffsetNED.z = 0.0f;
}
// copy required states to local variable names
@ -62,7 +78,7 @@ void NavEKF3_core::FuseRngBcn()
pd = stateStruct.position.z;
bcn_pn = rngBcnDataDelayed.beacon_posNED.x;
bcn_pe = rngBcnDataDelayed.beacon_posNED.y;
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosDownOffset;
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
// predicted range
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
@ -313,7 +329,7 @@ void NavEKF3_core::FuseRngBcnStatic()
} else {
// we are using the beacons as the primary height source, so don't modify their vertical position
bcnPosDownOffset = 0.0f;
bcnPosOffsetNED.z = 0.0f;
}
} else {
@ -344,7 +360,7 @@ void NavEKF3_core::FuseRngBcnStatic()
bcnPosDownOffsetMin = stateStruct.position.z - receverPosDownMax;
} else {
// We are using the beacons as the primary height reference, so don't modify their vertical position
bcnPosDownOffset = 0.0f;
bcnPosOffsetNED.z = 0.0f;
}
}
@ -354,7 +370,7 @@ void NavEKF3_core::FuseRngBcnStatic()
}
// calculate the observation jacobian
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosDownOffset;
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
float t5 = t2*t2;
@ -405,14 +421,13 @@ void NavEKF3_core::FuseRngBcnStatic()
// calculate range measurement innovation
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
deltaPosNED.z -= bcnPosDownOffset;
deltaPosNED.z -= bcnPosOffsetNED.z;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
// update the state
receiverPos.x -= K_RNG[0] * innovRngBcn;
receiverPos.y -= K_RNG[1] * innovRngBcn;
receiverPos.z -= K_RNG[2] * innovRngBcn;
receiverPos.z = MAX(receiverPos.z, minBcnPosD + 1.2f);
// calculate the covariance correction
for (unsigned i = 0; i<=2; i++) {
@ -571,10 +586,10 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// calculate the innovation for the main filter using the offset with the smallest innovation history
// apply hysteresis to prevent rapid switching
if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) {
bcnPosDownOffset = bcnPosDownOffsetMin;
bcnPosOffsetNED.z = bcnPosDownOffsetMin;
usingMinHypothesis = true;
} else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) {
bcnPosDownOffset = bcnPosDownOffsetMax;
bcnPosOffsetNED.z = bcnPosDownOffsetMax;
usingMinHypothesis = false;
}

10
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -265,6 +265,8 @@ void NavEKF3_core::InitialiseVariables()
ekfOriginHgtVar = 0.0f;
velOffsetNED.zero();
posOffsetNED.zero();
posResetSource = DEFAULT;
velResetSource = DEFAULT;
// range beacon fusion variables
memset(&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
@ -294,7 +296,6 @@ void NavEKF3_core::InitialiseVariables()
N_beacons = 0;
maxBcnPosD = 0.0f;
minBcnPosD = 0.0f;
bcnPosDownOffset = 0.0f;
bcnPosDownOffsetMax = 0.0f;
bcnPosOffsetMaxVar = 0.0f;
OffsetMaxInnovFilt = 0.0f;
@ -303,6 +304,8 @@ void NavEKF3_core::InitialiseVariables()
OffsetMinInnovFilt = 0.0f;
rngBcnFuseDataReportIndex = 0;
memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
bcnPosOffsetNED.zero();
bcnOriginEstInit = false;
// zero data buffers
storedIMU.reset();
@ -588,6 +591,11 @@ void NavEKF3_core::UpdateStrapdownEquationsNED()
// limit states to protect against divergence
ConstrainStates();
// If main filter velocity states are valid, update the range beacon receiver position states
if (filterStatus.flags.horiz_vel) {
receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
}
}
/*

19
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -468,7 +468,7 @@ private:
// use range beaon measurements to calculate a static position
void FuseRngBcnStatic();
// calculate the offset from EKF vetical position datum to the range beacon system datum
// calculate the offset from EKF vertical position datum to the range beacon system datum
void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning);
// fuse magnetometer measurements
@ -894,6 +894,19 @@ private:
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
// Specify preferred source of data to be used for a state reset
enum resetDataSource {
DEFAULT=0, // Use best available data
GPS=1, // Use GPS if available
RNGBCN=2, // Use beacon range data if available
FLOW=3, // Use optical flow rates if available
BARO=4, // Use Baro height if available
MAG=5, // Use magnetometer data if available
RNGFND=6 // Use rangefinder data if available
};
resetDataSource posResetSource; // preferred soure of data for position reset
resetDataSource velResetSource; // preferred source of data for a velocity reset
// variables used to calculate a vertical velocity that is kinematically consistent with the verical position
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
float posDown; // Down position state used in calculation of posDownRate
@ -1009,7 +1022,6 @@ private:
uint8_t N_beacons; // Number of range beacons in use
float maxBcnPosD; // maximum position of all beacons in the down direction (m)
float minBcnPosD; // minimum position of all beacons in the down direction (m)
float bcnPosDownOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
bool usingMinHypothesis; // true when the min beacob constellatio offset hyopthesis is being used
float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
@ -1020,6 +1032,9 @@ private:
float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
float OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow
Vector3f bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
bool bcnOriginEstInit; // True when the beacon origin has been initialised
// Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
struct {