AP_NavEKF3: move beacon variables into structure
AP_NavEKF3: change beacon variable names
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
2529c7fc40
commit
5d68f44694
@ -305,7 +305,7 @@ void NavEKF3_core::setAidingMode()
|
||||
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
// Check if range beacon data is being used
|
||||
const bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
|
||||
const bool rngBcnUsed = (imuSampleTime_ms - rngBcn.lastPassTime_ms <= minTestTime_ms);
|
||||
#else
|
||||
const bool rngBcnUsed = false;
|
||||
#endif
|
||||
@ -329,7 +329,7 @@ void NavEKF3_core::setAidingMode()
|
||||
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - rngBcn.lastPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
#endif
|
||||
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
|
||||
@ -346,7 +346,7 @@ void NavEKF3_core::setAidingMode()
|
||||
}
|
||||
posAidLossCritical =
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
(imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
|
||||
(imuSampleTime_ms - rngBcn.lastPassTime_ms > maxLossTime_ms) &&
|
||||
#endif
|
||||
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
|
||||
}
|
||||
@ -426,8 +426,8 @@ void NavEKF3_core::setAidingMode()
|
||||
// We are commencing aiding using range beacons
|
||||
posResetSource = resetDataSource::RNGBCN;
|
||||
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);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)rngBcn.receiverPos.x,(double)rngBcn.receiverPos.y);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)rngBcn.posOffsetNED.z);
|
||||
#endif // EK3_FEATURE_BEACON_FUSION
|
||||
#if EK3_FEATURE_EXTERNAL_NAV
|
||||
} else if (readyToUseExtNav()) {
|
||||
@ -454,7 +454,7 @@ void NavEKF3_core::setAidingMode()
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
lastVelPassTime_ms = imuSampleTime_ms;
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||
rngBcn.lastPassTime_ms = imuSampleTime_ms;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
@ -560,7 +560,7 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const
|
||||
return false;
|
||||
}
|
||||
|
||||
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse;
|
||||
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcn.alignmentCompleted && rngBcn.dataToFuse;
|
||||
#else
|
||||
return false;
|
||||
#endif // EK3_FEATURE_BEACON_FUSION
|
||||
|
||||
@ -229,20 +229,20 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
|
||||
return;
|
||||
}
|
||||
|
||||
if (!statesInitialised || N_beacons == 0 || rngBcnFusionReport == nullptr) {
|
||||
if (!statesInitialised || rngBcn.N == 0 || rngBcn.fusionReport == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
|
||||
if (rngBcnFuseDataReportIndex >= N_beacons) {
|
||||
rngBcnFuseDataReportIndex = 0;
|
||||
if (rngBcn.fuseDataReportIndex >= rngBcn.N) {
|
||||
rngBcn.fuseDataReportIndex = 0;
|
||||
}
|
||||
|
||||
const rngBcnFusionReport_t &report = rngBcnFusionReport[rngBcnFuseDataReportIndex];
|
||||
const auto &report = rngBcn.fusionReport[rngBcn.fuseDataReportIndex];
|
||||
|
||||
// write range beacon fusion debug packet if the range value is non-zero
|
||||
if (report.rng <= 0.0f) {
|
||||
rngBcnFuseDataReportIndex++;
|
||||
rngBcn.fuseDataReportIndex++;
|
||||
return;
|
||||
}
|
||||
|
||||
@ -250,7 +250,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
|
||||
LOG_PACKET_HEADER_INIT(LOG_XKF0_MSG),
|
||||
time_us : time_us,
|
||||
core : DAL_CORE(core_index),
|
||||
ID : rngBcnFuseDataReportIndex,
|
||||
ID : rngBcn.fuseDataReportIndex,
|
||||
rng : (int16_t)(100*report.rng),
|
||||
innov : (int16_t)(100*report.innov),
|
||||
sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)),
|
||||
@ -258,14 +258,14 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
|
||||
beaconPosN : (int16_t)(100*report.beaconPosNED.x),
|
||||
beaconPosE : (int16_t)(100*report.beaconPosNED.y),
|
||||
beaconPosD : (int16_t)(100*report.beaconPosNED.z),
|
||||
offsetHigh : (int16_t)(100*bcnPosDownOffsetMax),
|
||||
offsetLow : (int16_t)(100*bcnPosDownOffsetMin),
|
||||
posN : (int16_t)(100*receiverPos.x),
|
||||
posE : (int16_t)(100*receiverPos.y),
|
||||
posD : (int16_t)(100*receiverPos.z)
|
||||
offsetHigh : (int16_t)(100*rngBcn.posDownOffsetMax),
|
||||
offsetLow : (int16_t)(100*rngBcn.posDownOffsetMin),
|
||||
posN : (int16_t)(100*rngBcn.receiverPos.x),
|
||||
posE : (int16_t)(100*rngBcn.receiverPos.y),
|
||||
posD : (int16_t)(100*rngBcn.receiverPos.z)
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
|
||||
rngBcnFuseDataReportIndex++;
|
||||
rngBcn.fuseDataReportIndex++;
|
||||
}
|
||||
#endif // EK3_FEATURE_BEACON_FUSION
|
||||
|
||||
|
||||
@ -875,7 +875,7 @@ void NavEKF3_core::readAirSpdData()
|
||||
void NavEKF3_core::readRngBcnData()
|
||||
{
|
||||
// check that arrays are large enough
|
||||
static_assert(ARRAY_SIZE(lastTimeRngBcn_ms) >= AP_BEACON_MAX_BEACONS, "lastTimeRngBcn_ms should have at least AP_BEACON_MAX_BEACONS elements");
|
||||
static_assert(ARRAY_SIZE(rngBcn.lastTime_ms) >= AP_BEACON_MAX_BEACONS, "lastTimeRngBcn_ms should have at least AP_BEACON_MAX_BEACONS elements");
|
||||
|
||||
// get the location of the beacon data
|
||||
const AP_DAL_Beacon *beacon = dal.beacon();
|
||||
@ -886,30 +886,30 @@ void NavEKF3_core::readRngBcnData()
|
||||
}
|
||||
|
||||
// get the number of beacons in use
|
||||
N_beacons = MIN(beacon->count(), ARRAY_SIZE(lastTimeRngBcn_ms));
|
||||
rngBcn.N = MIN(beacon->count(), ARRAY_SIZE(rngBcn.lastTime_ms));
|
||||
|
||||
// search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
|
||||
bool newDataPushed = false;
|
||||
uint8_t numRngBcnsChecked = 0;
|
||||
// start the search one index up from where we left it last time
|
||||
uint8_t index = lastRngBcnChecked;
|
||||
while (!newDataPushed && (numRngBcnsChecked < N_beacons)) {
|
||||
uint8_t index = rngBcn.lastChecked;
|
||||
while (!newDataPushed && (numRngBcnsChecked < rngBcn.N)) {
|
||||
// track the number of beacons checked
|
||||
numRngBcnsChecked++;
|
||||
|
||||
// move to next beacon, wrap index if necessary
|
||||
index++;
|
||||
if (index >= N_beacons) {
|
||||
if (index >= rngBcn.N) {
|
||||
index = 0;
|
||||
}
|
||||
|
||||
// check that the beacon is healthy and has new data
|
||||
if (beacon->beacon_healthy(index) && beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index]) {
|
||||
if (beacon->beacon_healthy(index) && beacon->beacon_last_update_ms(index) != rngBcn.lastTime_ms[index]) {
|
||||
rng_bcn_elements rngBcnDataNew = {};
|
||||
|
||||
// set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
|
||||
lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index);
|
||||
rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
|
||||
rngBcn.lastTime_ms[index] = beacon->beacon_last_update_ms(index);
|
||||
rngBcnDataNew.time_ms = rngBcn.lastTime_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
|
||||
|
||||
// set the range noise
|
||||
// TODO the range library should provide the noise/accuracy estimate for each beacon
|
||||
@ -928,10 +928,10 @@ void NavEKF3_core::readRngBcnData()
|
||||
newDataPushed = true;
|
||||
|
||||
// update the last checked index
|
||||
lastRngBcnChecked = index;
|
||||
rngBcn.lastChecked = index;
|
||||
|
||||
// Save data into the buffer to be fused when the fusion time horizon catches up with it
|
||||
storedRangeBeacon.push(rngBcnDataNew);
|
||||
rngBcn.storedRange.push(rngBcnDataNew);
|
||||
}
|
||||
}
|
||||
|
||||
@ -939,18 +939,18 @@ void NavEKF3_core::readRngBcnData()
|
||||
Vector3f bp;
|
||||
float bperr;
|
||||
if (beacon->get_vehicle_position_ned(bp, bperr)) {
|
||||
rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
|
||||
rngBcn.last3DmeasTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
beaconVehiclePosNED = bp.toftype();
|
||||
beaconVehiclePosErr = bperr;
|
||||
rngBcn.vehiclePosNED = bp.toftype();
|
||||
rngBcn.vehiclePosErr = bperr;
|
||||
|
||||
// Check if the range beacon data can be used to align the vehicle position
|
||||
if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) && (beaconVehiclePosErr < 1.0f) && rngBcnAlignmentCompleted) {
|
||||
if ((imuSampleTime_ms - rngBcn.last3DmeasTime_ms < 250) && (rngBcn.vehiclePosErr < 1.0f) && rngBcn.alignmentCompleted) {
|
||||
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
|
||||
const ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
|
||||
const ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
|
||||
const ftype posDiffSq = sq(rngBcn.receiverPos.x - rngBcn.vehiclePosNED.x) + sq(rngBcn.receiverPos.y - rngBcn.vehiclePosNED.y);
|
||||
const ftype posDiffVar = sq(rngBcn.vehiclePosErr) + rngBcn.receiverPosCov[0][0] + rngBcn.receiverPosCov[1][1];
|
||||
if (posDiffSq < 9.0f * posDiffVar) {
|
||||
rngBcnGoodToAlign = true;
|
||||
rngBcn.goodToAlign = true;
|
||||
// Set the EKF origin and magnetic field declination if not previously set
|
||||
if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) {
|
||||
// get origin from beacon system
|
||||
@ -963,23 +963,23 @@ void NavEKF3_core::readRngBcnData()
|
||||
alignMagStateDeclination();
|
||||
|
||||
// Set the uncertainty of the origin height
|
||||
ekfOriginHgtVar = sq(beaconVehiclePosErr);
|
||||
ekfOriginHgtVar = sq(rngBcn.vehiclePosErr);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rngBcnGoodToAlign = false;
|
||||
rngBcn.goodToAlign = false;
|
||||
}
|
||||
} else {
|
||||
rngBcnGoodToAlign = false;
|
||||
rngBcn.goodToAlign = false;
|
||||
}
|
||||
|
||||
// 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);
|
||||
rngBcn.dataToFuse = rngBcn.storedRange.recall(rngBcn.dataDelayed, 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.x += bcnPosOffsetNED.x;
|
||||
rngBcnDataDelayed.beacon_posNED.y += bcnPosOffsetNED.y;
|
||||
if (rngBcn.dataToFuse) {
|
||||
rngBcn.dataDelayed.beacon_posNED.x += rngBcn.posOffsetNED.x;
|
||||
rngBcn.dataDelayed.beacon_posNED.y += rngBcn.posOffsetNED.y;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -236,10 +236,10 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
|
||||
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
|
||||
return false;
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
} else if (rngBcnAlignmentStarted) {
|
||||
} else if (rngBcn.alignmentStarted) {
|
||||
// If we are attempting alignment using range beacon data, then report the position
|
||||
posNE.x = receiverPos.x;
|
||||
posNE.y = receiverPos.y;
|
||||
posNE.x = rngBcn.receiverPos.x;
|
||||
posNE.y = rngBcn.receiverPos.y;
|
||||
return false;
|
||||
#endif
|
||||
} else {
|
||||
|
||||
@ -105,13 +105,13 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
|
||||
// set the variances using the position measurement noise parameter
|
||||
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
|
||||
} else if ((imuSampleTime_ms - rngBcn.last3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
|
||||
// use the range beacon data as a second preference
|
||||
stateStruct.position.x = receiverPos.x;
|
||||
stateStruct.position.y = receiverPos.y;
|
||||
stateStruct.position.x = rngBcn.receiverPos.x;
|
||||
stateStruct.position.y = rngBcn.receiverPos.y;
|
||||
// set the variances from the beacon alignment filter
|
||||
P[7][7] = receiverPosCov[0][0];
|
||||
P[8][8] = receiverPosCov[1][1];
|
||||
P[7][7] = rngBcn.receiverPosCov[0][0];
|
||||
P[8][8] = rngBcn.receiverPosCov[1][1];
|
||||
#endif
|
||||
#if EK3_FEATURE_EXTERNAL_NAV
|
||||
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
|
||||
@ -1128,7 +1128,7 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
|
||||
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) {
|
||||
} else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcn.goodToAlign) {
|
||||
activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
|
||||
#endif
|
||||
#if EK3_FEATURE_EXTERNAL_NAV
|
||||
@ -1141,7 +1141,7 @@ void NavEKF3_core::selectHeightForFusion()
|
||||
bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
|
||||
bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000 || !gpsAccuracyGoodForAltitude));
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
|
||||
bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcn.dataDelayed.time_ms) > 2000));
|
||||
#endif
|
||||
bool fallback_to_baro =
|
||||
lostRngHgt
|
||||
|
||||
@ -14,13 +14,13 @@ void NavEKF3_core::SelectRngBcnFusion()
|
||||
readRngBcnData();
|
||||
|
||||
// Determine if we need to fuse range beacon data on this time step
|
||||
if (rngBcnDataToFuse) {
|
||||
if (rngBcn.dataToFuse) {
|
||||
if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
if ((frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcnAlignmentCompleted) {
|
||||
if (!bcnOriginEstInit) {
|
||||
bcnOriginEstInit = true;
|
||||
bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x;
|
||||
bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y;
|
||||
if ((frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcn.alignmentCompleted) {
|
||||
if (!rngBcn.originEstInit) {
|
||||
rngBcn.originEstInit = true;
|
||||
rngBcn.posOffsetNED.x = rngBcn.receiverPos.x - stateStruct.position.x;
|
||||
rngBcn.posOffsetNED.y = rngBcn.receiverPos.y - stateStruct.position.y;
|
||||
}
|
||||
// beacons are used as the primary means of position reference
|
||||
FuseRngBcn();
|
||||
@ -30,13 +30,13 @@ void NavEKF3_core::SelectRngBcnFusion()
|
||||
// start using beacon data as the primary reference.
|
||||
FuseRngBcnStatic();
|
||||
// record that the beacon origin needs to be initialised
|
||||
bcnOriginEstInit = false;
|
||||
rngBcn.originEstInit = false;
|
||||
}
|
||||
} else {
|
||||
// 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;
|
||||
rngBcn.originEstInit = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -50,33 +50,33 @@ void NavEKF3_core::FuseRngBcn()
|
||||
ftype bcn_pn;
|
||||
ftype bcn_pe;
|
||||
ftype bcn_pd;
|
||||
const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
const ftype R_BCN = sq(MAX(rngBcn.dataDelayed.rngErr , 0.1f));
|
||||
ftype rngPred;
|
||||
|
||||
// health is set bad until test passed
|
||||
rngBcnHealth = false;
|
||||
rngBcn.health = false;
|
||||
|
||||
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
|
||||
// calculate the vertical offset from EKF datum to beacon datum
|
||||
CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
|
||||
} else {
|
||||
bcnPosOffsetNED.z = 0.0f;
|
||||
rngBcn.posOffsetNED.z = 0.0f;
|
||||
}
|
||||
|
||||
// copy required states to local variable names
|
||||
pn = stateStruct.position.x;
|
||||
pe = stateStruct.position.y;
|
||||
pd = stateStruct.position.z;
|
||||
bcn_pn = rngBcnDataDelayed.beacon_posNED.x;
|
||||
bcn_pe = rngBcnDataDelayed.beacon_posNED.y;
|
||||
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z;
|
||||
bcn_pn = rngBcn.dataDelayed.beacon_posNED.x;
|
||||
bcn_pe = rngBcn.dataDelayed.beacon_posNED.y;
|
||||
bcn_pd = rngBcn.dataDelayed.beacon_posNED.z + rngBcn.posOffsetNED.z;
|
||||
|
||||
// predicted range
|
||||
Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
Vector3F deltaPosNED = stateStruct.position - rngBcn.dataDelayed.beacon_posNED;
|
||||
rngPred = deltaPosNED.length();
|
||||
|
||||
// calculate measurement innovation
|
||||
innovRngBcn = rngPred - rngBcnDataDelayed.rng;
|
||||
rngBcn.innov = rngPred - rngBcn.dataDelayed.rng;
|
||||
|
||||
// perform fusion of range measurement
|
||||
if (rngPred > 0.1f)
|
||||
@ -119,10 +119,10 @@ void NavEKF3_core::FuseRngBcn()
|
||||
ftype t22 = P[7][7]*t4*t9;
|
||||
ftype t23 = t20+t21+t22;
|
||||
ftype t24 = t4*t9*t23;
|
||||
varInnovRngBcn = R_BCN+t14+t19+t24;
|
||||
rngBcn.varInnov = R_BCN+t14+t19+t24;
|
||||
ftype t26;
|
||||
if (varInnovRngBcn >= R_BCN) {
|
||||
t26 = 1.0f/varInnovRngBcn;
|
||||
if (rngBcn.varInnov >= R_BCN) {
|
||||
t26 = 1.0f/rngBcn.varInnov;
|
||||
faultStatus.bad_rngbcn = false;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
@ -194,20 +194,20 @@ void NavEKF3_core::FuseRngBcn()
|
||||
}
|
||||
|
||||
// Calculate innovation using the selected offset value
|
||||
Vector3F delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
|
||||
Vector3F delta = stateStruct.position - rngBcn.dataDelayed.beacon_posNED;
|
||||
rngBcn.innov = delta.length() - rngBcn.dataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
rngBcn.testRatio = sq(rngBcn.innov) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * rngBcn.varInnov);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
|
||||
rngBcn.health = ((rngBcn.testRatio < 1.0f) || badIMUdata);
|
||||
|
||||
// test the ratio before fusing data
|
||||
if (rngBcnHealth) {
|
||||
if (rngBcn.health) {
|
||||
|
||||
// restart the counter
|
||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||
rngBcn.lastPassTime_ms = imuSampleTime_ms;
|
||||
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
@ -253,7 +253,7 @@ void NavEKF3_core::FuseRngBcn()
|
||||
|
||||
// correct the state vector
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||
statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn;
|
||||
statesArray[j] = statesArray[j] - Kfusion[j] * rngBcn.innov;
|
||||
}
|
||||
|
||||
// record healthy fusion
|
||||
@ -267,12 +267,12 @@ void NavEKF3_core::FuseRngBcn()
|
||||
}
|
||||
|
||||
// Update the fusion report
|
||||
if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) {
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
|
||||
if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].testRatio = rngBcn.testRatio;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -285,7 +285,7 @@ https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon
|
||||
void NavEKF3_core::FuseRngBcnStatic()
|
||||
{
|
||||
// get the estimated range measurement variance
|
||||
const ftype R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
const ftype R_RNG = sq(MAX(rngBcn.dataDelayed.rngErr , 0.1f));
|
||||
|
||||
/*
|
||||
The first thing to do is to check if we have started the alignment and if not, initialise the
|
||||
@ -293,60 +293,60 @@ void NavEKF3_core::FuseRngBcnStatic()
|
||||
initialise the initial position to the mean beacon position. The initial position uncertainty
|
||||
is set to the mean range measurement.
|
||||
*/
|
||||
if (!rngBcnAlignmentStarted) {
|
||||
if (rngBcnDataDelayed.beacon_ID != lastBeaconIndex) {
|
||||
rngBcnPosSum += rngBcnDataDelayed.beacon_posNED;
|
||||
lastBeaconIndex = rngBcnDataDelayed.beacon_ID;
|
||||
rngSum += rngBcnDataDelayed.rng;
|
||||
numBcnMeas++;
|
||||
if (!rngBcn.alignmentStarted) {
|
||||
if (rngBcn.dataDelayed.beacon_ID != rngBcn.lastIndex) {
|
||||
rngBcn.posSum += rngBcn.dataDelayed.beacon_posNED;
|
||||
rngBcn.lastIndex = rngBcn.dataDelayed.beacon_ID;
|
||||
rngBcn.sum += rngBcn.dataDelayed.rng;
|
||||
rngBcn.numMeas++;
|
||||
|
||||
// capture the beacon vertical spread
|
||||
if (rngBcnDataDelayed.beacon_posNED.z > maxBcnPosD) {
|
||||
maxBcnPosD = rngBcnDataDelayed.beacon_posNED.z;
|
||||
} else if(rngBcnDataDelayed.beacon_posNED.z < minBcnPosD) {
|
||||
minBcnPosD = rngBcnDataDelayed.beacon_posNED.z;
|
||||
if (rngBcn.dataDelayed.beacon_posNED.z > rngBcn.maxPosD) {
|
||||
rngBcn.maxPosD = rngBcn.dataDelayed.beacon_posNED.z;
|
||||
} else if(rngBcn.dataDelayed.beacon_posNED.z < rngBcn.minPosD) {
|
||||
rngBcn.minPosD = rngBcn.dataDelayed.beacon_posNED.z;
|
||||
}
|
||||
}
|
||||
if (numBcnMeas >= 100) {
|
||||
rngBcnAlignmentStarted = true;
|
||||
ftype tempVar = 1.0f / (ftype)numBcnMeas;
|
||||
if (rngBcn.numMeas >= 100) {
|
||||
rngBcn.alignmentStarted = true;
|
||||
ftype tempVar = 1.0f / (ftype)rngBcn.numMeas;
|
||||
// initialise the receiver position to the centre of the beacons and at zero height
|
||||
receiverPos.x = rngBcnPosSum.x * tempVar;
|
||||
receiverPos.y = rngBcnPosSum.y * tempVar;
|
||||
receiverPos.z = 0.0f;
|
||||
receiverPosCov[2][2] = receiverPosCov[1][1] = receiverPosCov[0][0] = rngSum * tempVar;
|
||||
lastBeaconIndex = 0;
|
||||
numBcnMeas = 0;
|
||||
rngBcnPosSum.zero();
|
||||
rngSum = 0.0f;
|
||||
rngBcn.receiverPos.x = rngBcn.posSum.x * tempVar;
|
||||
rngBcn.receiverPos.y = rngBcn.posSum.y * tempVar;
|
||||
rngBcn.receiverPos.z = 0.0f;
|
||||
rngBcn.receiverPosCov[2][2] = rngBcn.receiverPosCov[1][1] = rngBcn.receiverPosCov[0][0] = rngBcn.sum * tempVar;
|
||||
rngBcn.lastIndex = 0;
|
||||
rngBcn.numMeas = 0;
|
||||
rngBcn.posSum.zero();
|
||||
rngBcn.sum = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
if (rngBcnAlignmentStarted) {
|
||||
numBcnMeas++;
|
||||
if (rngBcn.alignmentStarted) {
|
||||
rngBcn.numMeas++;
|
||||
|
||||
if (numBcnMeas >= 100) {
|
||||
if (rngBcn.numMeas >= 100) {
|
||||
// 100 observations is enough for a stable estimate under most conditions
|
||||
// TODO monitor stability of the position estimate
|
||||
rngBcnAlignmentCompleted = true;
|
||||
rngBcn.alignmentCompleted = true;
|
||||
|
||||
}
|
||||
|
||||
if (rngBcnAlignmentCompleted) {
|
||||
if (rngBcn.alignmentCompleted) {
|
||||
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
|
||||
// We are using a different height reference for the main EKF so need to estimate a vertical
|
||||
// position offset to be applied to the beacon system that minimises the range innovations
|
||||
// The position estimate should be stable after 100 iterations so we use a simple dual
|
||||
// hypothesis 1-state EKF to estimate the offset
|
||||
Vector3F refPosNED;
|
||||
refPosNED.x = receiverPos.x;
|
||||
refPosNED.y = receiverPos.y;
|
||||
refPosNED.x = rngBcn.receiverPos.x;
|
||||
refPosNED.y = rngBcn.receiverPos.y;
|
||||
refPosNED.z = stateStruct.position.z;
|
||||
CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true);
|
||||
|
||||
} else {
|
||||
// we are using the beacons as the primary height source, so don't modify their vertical position
|
||||
bcnPosOffsetNED.z = 0.0f;
|
||||
rngBcn.posOffsetNED.z = 0.0f;
|
||||
|
||||
}
|
||||
} else {
|
||||
@ -357,39 +357,39 @@ void NavEKF3_core::FuseRngBcnStatic()
|
||||
// and the main EKF vertical position
|
||||
|
||||
// Calculate the mid vertical position of all beacons
|
||||
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
ftype bcnMidPosD = 0.5f * (rngBcn.minPosD + rngBcn.maxPosD);
|
||||
|
||||
// calculate the delta to the estimated receiver position
|
||||
ftype delta = receiverPos.z - bcnMidPosD;
|
||||
ftype delta = rngBcn.receiverPos.z - bcnMidPosD;
|
||||
|
||||
// calculate the two hypothesis for our vertical position
|
||||
ftype receiverPosDownMax;
|
||||
ftype receiverPosDownMin;
|
||||
if (delta >= 0.0f) {
|
||||
receiverPosDownMax = receiverPos.z;
|
||||
receiverPosDownMin = receiverPos.z - 2.0f * delta;
|
||||
receiverPosDownMax = rngBcn.receiverPos.z;
|
||||
receiverPosDownMin = rngBcn.receiverPos.z - 2.0f * delta;
|
||||
} else {
|
||||
receiverPosDownMax = receiverPos.z - 2.0f * delta;
|
||||
receiverPosDownMin = receiverPos.z;
|
||||
receiverPosDownMax = rngBcn.receiverPos.z - 2.0f * delta;
|
||||
receiverPosDownMin = rngBcn.receiverPos.z;
|
||||
}
|
||||
|
||||
bcnPosDownOffsetMax = stateStruct.position.z - receiverPosDownMin;
|
||||
bcnPosDownOffsetMin = stateStruct.position.z - receiverPosDownMax;
|
||||
rngBcn.posDownOffsetMax = stateStruct.position.z - receiverPosDownMin;
|
||||
rngBcn.posDownOffsetMin = stateStruct.position.z - receiverPosDownMax;
|
||||
} else {
|
||||
// We are using the beacons as the primary height reference, so don't modify their vertical position
|
||||
bcnPosOffsetNED.z = 0.0f;
|
||||
rngBcn.posOffsetNED.z = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// Add some process noise to the states at each time step
|
||||
for (uint8_t i= 0; i<=2; i++) {
|
||||
receiverPosCov[i][i] += 0.1f;
|
||||
rngBcn.receiverPosCov[i][i] += 0.1f;
|
||||
}
|
||||
|
||||
// calculate the observation jacobian
|
||||
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z;
|
||||
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
|
||||
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
|
||||
ftype t2 = rngBcn.dataDelayed.beacon_posNED.z - rngBcn.receiverPos.z + rngBcn.posOffsetNED.z;
|
||||
ftype t3 = rngBcn.dataDelayed.beacon_posNED.y - rngBcn.receiverPos.y;
|
||||
ftype t4 = rngBcn.dataDelayed.beacon_posNED.x - rngBcn.receiverPos.x;
|
||||
ftype t5 = t2*t2;
|
||||
ftype t6 = t3*t3;
|
||||
ftype t7 = t4*t4;
|
||||
@ -399,14 +399,14 @@ void NavEKF3_core::FuseRngBcnStatic()
|
||||
return;
|
||||
}
|
||||
ftype t9 = 1.0f/sqrtF(t8);
|
||||
ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
|
||||
ftype t15 = receiverPos.x*2.0f;
|
||||
ftype t10 = rngBcn.dataDelayed.beacon_posNED.x*2.0f;
|
||||
ftype t15 = rngBcn.receiverPos.x*2.0f;
|
||||
ftype t11 = t10-t15;
|
||||
ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
|
||||
ftype t14 = receiverPos.y*2.0f;
|
||||
ftype t12 = rngBcn.dataDelayed.beacon_posNED.y*2.0f;
|
||||
ftype t14 = rngBcn.receiverPos.y*2.0f;
|
||||
ftype t13 = t12-t14;
|
||||
ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
|
||||
ftype t18 = receiverPos.z*2.0f;
|
||||
ftype t16 = rngBcn.dataDelayed.beacon_posNED.z*2.0f;
|
||||
ftype t18 = rngBcn.receiverPos.z*2.0f;
|
||||
ftype t17 = t16-t18;
|
||||
ftype H_RNG[3];
|
||||
H_RNG[0] = -t9*t11*0.5f;
|
||||
@ -414,46 +414,46 @@ void NavEKF3_core::FuseRngBcnStatic()
|
||||
H_RNG[2] = -t9*t17*0.5f;
|
||||
|
||||
// calculate the Kalman gains
|
||||
ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
|
||||
ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
|
||||
ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
|
||||
ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
|
||||
ftype t19 = rngBcn.receiverPosCov[0][0]*t9*t11*0.5f;
|
||||
ftype t20 = rngBcn.receiverPosCov[1][1]*t9*t13*0.5f;
|
||||
ftype t21 = rngBcn.receiverPosCov[0][1]*t9*t11*0.5f;
|
||||
ftype t22 = rngBcn.receiverPosCov[2][1]*t9*t17*0.5f;
|
||||
ftype t23 = t20+t21+t22;
|
||||
ftype t24 = t9*t13*t23*0.5f;
|
||||
ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
|
||||
ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
|
||||
ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
|
||||
ftype t25 = rngBcn.receiverPosCov[1][2]*t9*t13*0.5f;
|
||||
ftype t26 = rngBcn.receiverPosCov[0][2]*t9*t11*0.5f;
|
||||
ftype t27 = rngBcn.receiverPosCov[2][2]*t9*t17*0.5f;
|
||||
ftype t28 = t25+t26+t27;
|
||||
ftype t29 = t9*t17*t28*0.5f;
|
||||
ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
|
||||
ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
|
||||
ftype t30 = rngBcn.receiverPosCov[1][0]*t9*t13*0.5f;
|
||||
ftype t31 = rngBcn.receiverPosCov[2][0]*t9*t17*0.5f;
|
||||
ftype t32 = t19+t30+t31;
|
||||
ftype t33 = t9*t11*t32*0.5f;
|
||||
varInnovRngBcn = R_RNG+t24+t29+t33;
|
||||
ftype t35 = 1.0f/varInnovRngBcn;
|
||||
rngBcn.varInnov = R_RNG+t24+t29+t33;
|
||||
ftype t35 = 1.0f/rngBcn.varInnov;
|
||||
ftype K_RNG[3];
|
||||
K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f);
|
||||
K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f);
|
||||
K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
|
||||
K_RNG[0] = -t35*(t19+rngBcn.receiverPosCov[0][1]*t9*t13*0.5f+rngBcn.receiverPosCov[0][2]*t9*t17*0.5f);
|
||||
K_RNG[1] = -t35*(t20+rngBcn.receiverPosCov[1][0]*t9*t11*0.5f+rngBcn.receiverPosCov[1][2]*t9*t17*0.5f);
|
||||
K_RNG[2] = -t35*(t27+rngBcn.receiverPosCov[2][0]*t9*t11*0.5f+rngBcn.receiverPosCov[2][1]*t9*t13*0.5f);
|
||||
|
||||
// calculate range measurement innovation
|
||||
Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
|
||||
deltaPosNED.z -= bcnPosOffsetNED.z;
|
||||
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
|
||||
Vector3F deltaPosNED = rngBcn.receiverPos - rngBcn.dataDelayed.beacon_posNED;
|
||||
deltaPosNED.z -= rngBcn.posOffsetNED.z;
|
||||
rngBcn.innov = deltaPosNED.length() - rngBcn.dataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
rngBcn.testRatio = sq(rngBcn.innov) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * rngBcn.varInnov);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted);
|
||||
rngBcn.health = ((rngBcn.testRatio < 1.0f) || badIMUdata || !rngBcn.alignmentCompleted);
|
||||
|
||||
// test the ratio before fusing data
|
||||
if (rngBcnHealth) {
|
||||
if (rngBcn.health) {
|
||||
|
||||
// update the state
|
||||
receiverPos.x -= K_RNG[0] * innovRngBcn;
|
||||
receiverPos.y -= K_RNG[1] * innovRngBcn;
|
||||
receiverPos.z -= K_RNG[2] * innovRngBcn;
|
||||
rngBcn.receiverPos.x -= K_RNG[0] * rngBcn.innov;
|
||||
rngBcn.receiverPos.y -= K_RNG[1] * rngBcn.innov;
|
||||
rngBcn.receiverPos.z -= K_RNG[2] * rngBcn.innov;
|
||||
|
||||
// calculate the covariance correction
|
||||
for (unsigned i = 0; i<=2; i++) {
|
||||
@ -464,53 +464,53 @@ void NavEKF3_core::FuseRngBcnStatic()
|
||||
for (unsigned j = 0; j<=2; j++) {
|
||||
for (unsigned i = 0; i<=2; i++) {
|
||||
ftype res = 0;
|
||||
res += KH[i][0] * receiverPosCov[0][j];
|
||||
res += KH[i][1] * receiverPosCov[1][j];
|
||||
res += KH[i][2] * receiverPosCov[2][j];
|
||||
res += KH[i][0] * rngBcn.receiverPosCov[0][j];
|
||||
res += KH[i][1] * rngBcn.receiverPosCov[1][j];
|
||||
res += KH[i][2] * rngBcn.receiverPosCov[2][j];
|
||||
KHP[i][j] = res;
|
||||
}
|
||||
}
|
||||
|
||||
// prevent negative variances
|
||||
for (uint8_t i= 0; i<=2; i++) {
|
||||
if (receiverPosCov[i][i] < 0.0f) {
|
||||
receiverPosCov[i][i] = 0.0f;
|
||||
if (rngBcn.receiverPosCov[i][i] < 0.0f) {
|
||||
rngBcn.receiverPosCov[i][i] = 0.0f;
|
||||
KHP[i][i] = 0.0f;
|
||||
} else if (KHP[i][i] > receiverPosCov[i][i]) {
|
||||
KHP[i][i] = receiverPosCov[i][i];
|
||||
} else if (KHP[i][i] > rngBcn.receiverPosCov[i][i]) {
|
||||
KHP[i][i] = rngBcn.receiverPosCov[i][i];
|
||||
}
|
||||
}
|
||||
|
||||
// apply the covariance correction
|
||||
for (uint8_t i= 0; i<=2; i++) {
|
||||
for (uint8_t j= 0; j<=2; j++) {
|
||||
receiverPosCov[i][j] -= KHP[i][j];
|
||||
rngBcn.receiverPosCov[i][j] -= KHP[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// ensure the covariance matrix is symmetric
|
||||
for (uint8_t i=1; i<=2; i++) {
|
||||
for (uint8_t j=0; j<=i-1; j++) {
|
||||
ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
|
||||
receiverPosCov[i][j] = temp;
|
||||
receiverPosCov[j][i] = temp;
|
||||
ftype temp = 0.5f*(rngBcn.receiverPosCov[i][j] + rngBcn.receiverPosCov[j][i]);
|
||||
rngBcn.receiverPosCov[i][j] = temp;
|
||||
rngBcn.receiverPosCov[j][i] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (numBcnMeas >= 100) {
|
||||
if (rngBcn.numMeas >= 100) {
|
||||
// 100 observations is enough for a stable estimate under most conditions
|
||||
// TODO monitor stability of the position estimate
|
||||
rngBcnAlignmentCompleted = true;
|
||||
rngBcn.alignmentCompleted = true;
|
||||
}
|
||||
// Update the fusion report
|
||||
if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) {
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
|
||||
if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng;
|
||||
rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].testRatio = rngBcn.testRatio;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -539,9 +539,9 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
|
||||
// estimate upper value for offset
|
||||
|
||||
// calculate observation derivative
|
||||
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
|
||||
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
|
||||
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
|
||||
ftype t2 = rngBcn.dataDelayed.beacon_posNED.z - vehiclePosNED.z + rngBcn.posDownOffsetMax;
|
||||
ftype t3 = rngBcn.dataDelayed.beacon_posNED.y - vehiclePosNED.y;
|
||||
ftype t4 = rngBcn.dataDelayed.beacon_posNED.x - vehiclePosNED.x;
|
||||
ftype t5 = t2*t2;
|
||||
ftype t6 = t3*t3;
|
||||
ftype t7 = t4*t4;
|
||||
@ -552,38 +552,38 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
|
||||
obsDeriv = t2*t9;
|
||||
|
||||
// Calculate innovation
|
||||
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
|
||||
innov = sqrtF(t8) - rngBcn.dataDelayed.rng;
|
||||
|
||||
// covariance prediction
|
||||
bcnPosOffsetMaxVar += stateNoiseVar;
|
||||
rngBcn.posOffsetMaxVar += stateNoiseVar;
|
||||
|
||||
// calculate the innovation variance
|
||||
innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar;
|
||||
innovVar = obsDeriv * rngBcn.posOffsetMaxVar * obsDeriv + obsVar;
|
||||
innovVar = MAX(innovVar, obsVar);
|
||||
|
||||
// calculate the Kalman gain
|
||||
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
|
||||
gain = (rngBcn.posOffsetMaxVar * obsDeriv) / innovVar;
|
||||
|
||||
// calculate a filtered state change magnitude to be used to select between the high or low offset
|
||||
ftype stateChange = innov * gain;
|
||||
maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
|
||||
rngBcn.maxOffsetStateChangeFilt = (1.0f - filtAlpha) * rngBcn.maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
|
||||
|
||||
// Reject range innovation spikes using a 5-sigma threshold unless aligning
|
||||
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
|
||||
|
||||
// state update
|
||||
bcnPosDownOffsetMax -= stateChange;
|
||||
rngBcn.posDownOffsetMax -= stateChange;
|
||||
|
||||
// covariance update
|
||||
bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar;
|
||||
bcnPosOffsetMaxVar = MAX(bcnPosOffsetMaxVar, 0.0f);
|
||||
rngBcn.posOffsetMaxVar -= gain * obsDeriv * rngBcn.posOffsetMaxVar;
|
||||
rngBcn.posOffsetMaxVar = MAX(rngBcn.posOffsetMaxVar, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// estimate lower value for offset
|
||||
|
||||
// calculate observation derivative
|
||||
t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMin;
|
||||
t2 = rngBcn.dataDelayed.beacon_posNED.z - vehiclePosNED.z + rngBcn.posDownOffsetMin;
|
||||
t5 = t2*t2;
|
||||
t8 = t5+t6+t7;
|
||||
if (t8 > 0.1f) {
|
||||
@ -591,56 +591,56 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
|
||||
obsDeriv = t2*t9;
|
||||
|
||||
// Calculate innovation
|
||||
innov = sqrtF(t8) - rngBcnDataDelayed.rng;
|
||||
innov = sqrtF(t8) - rngBcn.dataDelayed.rng;
|
||||
|
||||
// covariance prediction
|
||||
bcnPosOffsetMinVar += stateNoiseVar;
|
||||
rngBcn.posOffsetMinVar += stateNoiseVar;
|
||||
|
||||
// calculate the innovation variance
|
||||
innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar;
|
||||
innovVar = obsDeriv * rngBcn.posOffsetMinVar * obsDeriv + obsVar;
|
||||
innovVar = MAX(innovVar, obsVar);
|
||||
|
||||
// calculate the Kalman gain
|
||||
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
|
||||
gain = (rngBcn.posOffsetMinVar * obsDeriv) / innovVar;
|
||||
|
||||
// calculate a filtered state change magnitude to be used to select between the high or low offset
|
||||
ftype stateChange = innov * gain;
|
||||
minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
|
||||
rngBcn.minOffsetStateChangeFilt = (1.0f - filtAlpha) * rngBcn.minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
|
||||
|
||||
// Reject range innovation spikes using a 5-sigma threshold unless aligning
|
||||
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
|
||||
|
||||
// state update
|
||||
bcnPosDownOffsetMin -= stateChange;
|
||||
rngBcn.posDownOffsetMin -= stateChange;
|
||||
|
||||
// covariance update
|
||||
bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar;
|
||||
bcnPosOffsetMinVar = MAX(bcnPosOffsetMinVar, 0.0f);
|
||||
rngBcn.posOffsetMinVar -= gain * obsDeriv * rngBcn.posOffsetMinVar;
|
||||
rngBcn.posOffsetMinVar = MAX(rngBcn.posOffsetMinVar, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the mid vertical position of all beacons
|
||||
ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
ftype bcnMidPosD = 0.5f * (rngBcn.minPosD + rngBcn.maxPosD);
|
||||
|
||||
// ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
|
||||
bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
|
||||
bcnPosDownOffsetMin = MIN(bcnPosDownOffsetMin, vehiclePosNED.z - bcnMidPosD - 0.5f);
|
||||
rngBcn.posDownOffsetMax = MAX(rngBcn.posDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
|
||||
rngBcn.posDownOffsetMin = MIN(rngBcn.posDownOffsetMin, vehiclePosNED.z - bcnMidPosD - 0.5f);
|
||||
|
||||
// calculate the innovation for the main filter using the offset that is most stable
|
||||
// apply hysteresis to prevent rapid switching
|
||||
if (!usingMinHypothesis && (minOffsetStateChangeFilt < (0.8f * maxOffsetStateChangeFilt))) {
|
||||
usingMinHypothesis = true;
|
||||
} else if (usingMinHypothesis && (maxOffsetStateChangeFilt < (0.8f * minOffsetStateChangeFilt))) {
|
||||
usingMinHypothesis = false;
|
||||
if (!rngBcn.usingMinHypothesis && (rngBcn.minOffsetStateChangeFilt < (0.8f * rngBcn.maxOffsetStateChangeFilt))) {
|
||||
rngBcn.usingMinHypothesis = true;
|
||||
} else if (rngBcn.usingMinHypothesis && (rngBcn.maxOffsetStateChangeFilt < (0.8f * rngBcn.minOffsetStateChangeFilt))) {
|
||||
rngBcn.usingMinHypothesis = false;
|
||||
}
|
||||
if (usingMinHypothesis) {
|
||||
bcnPosOffsetNED.z = bcnPosDownOffsetMin;
|
||||
if (rngBcn.usingMinHypothesis) {
|
||||
rngBcn.posOffsetNED.z = rngBcn.posDownOffsetMin;
|
||||
} else {
|
||||
bcnPosOffsetNED.z = bcnPosDownOffsetMax;
|
||||
rngBcn.posOffsetNED.z = rngBcn.posDownOffsetMax;
|
||||
}
|
||||
|
||||
// apply the vertical offset to the beacon positions
|
||||
rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z;
|
||||
rngBcn.dataDelayed.beacon_posNED.z += rngBcn.posOffsetNED.z;
|
||||
}
|
||||
|
||||
#endif // EK3_FEATURE_BEACON_FUSION
|
||||
|
||||
@ -137,7 +137,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
||||
}
|
||||
// Note: range beacon data is read one beacon at a time and can arrive at a high rate
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
if(dal.beacon() && !storedRangeBeacon.init(imu_buffer_length+1)) {
|
||||
if(dal.beacon() && !rngBcn.storedRange.init(imu_buffer_length+1)) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
@ -345,44 +345,44 @@ void NavEKF3_core::InitialiseVariables()
|
||||
|
||||
// range beacon fusion variables
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
|
||||
lastRngBcnPassTime_ms = 0;
|
||||
rngBcnTestRatio = 0.0f;
|
||||
rngBcnHealth = false;
|
||||
varInnovRngBcn = 0.0f;
|
||||
innovRngBcn = 0.0f;
|
||||
memset(&lastTimeRngBcn_ms, 0, sizeof(lastTimeRngBcn_ms));
|
||||
rngBcnDataToFuse = false;
|
||||
beaconVehiclePosNED.zero();
|
||||
beaconVehiclePosErr = 1.0f;
|
||||
rngBcnLast3DmeasTime_ms = 0;
|
||||
rngBcnGoodToAlign = false;
|
||||
lastRngBcnChecked = 0;
|
||||
receiverPos.zero();
|
||||
memset(&receiverPosCov, 0, sizeof(receiverPosCov));
|
||||
rngBcnAlignmentStarted = false;
|
||||
rngBcnAlignmentCompleted = false;
|
||||
lastBeaconIndex = 0;
|
||||
rngBcnPosSum.zero();
|
||||
numBcnMeas = 0;
|
||||
rngSum = 0.0f;
|
||||
N_beacons = 0;
|
||||
maxBcnPosD = 0.0f;
|
||||
minBcnPosD = 0.0f;
|
||||
bcnPosDownOffsetMax = 0.0f;
|
||||
bcnPosOffsetMaxVar = 0.0f;
|
||||
maxOffsetStateChangeFilt = 0.0f;
|
||||
bcnPosDownOffsetMin = 0.0f;
|
||||
bcnPosOffsetMinVar = 0.0f;
|
||||
minOffsetStateChangeFilt = 0.0f;
|
||||
rngBcnFuseDataReportIndex = 0;
|
||||
memset((void *)&rngBcn.dataDelayed, 0, sizeof(rngBcn.dataDelayed));
|
||||
rngBcn.lastPassTime_ms = 0;
|
||||
rngBcn.testRatio = 0.0f;
|
||||
rngBcn.health = false;
|
||||
rngBcn.varInnov = 0.0f;
|
||||
rngBcn.innov = 0.0f;
|
||||
memset(&rngBcn.lastTime_ms, 0, sizeof(rngBcn.lastTime_ms));
|
||||
rngBcn.dataToFuse = false;
|
||||
rngBcn.vehiclePosNED.zero();
|
||||
rngBcn.vehiclePosErr = 1.0f;
|
||||
rngBcn.last3DmeasTime_ms = 0;
|
||||
rngBcn.goodToAlign = false;
|
||||
rngBcn.lastChecked = 0;
|
||||
rngBcn.receiverPos.zero();
|
||||
memset(&rngBcn.receiverPosCov, 0, sizeof(rngBcn.receiverPosCov));
|
||||
rngBcn.alignmentStarted = false;
|
||||
rngBcn.alignmentCompleted = false;
|
||||
rngBcn.lastIndex = 0;
|
||||
rngBcn.posSum.zero();
|
||||
rngBcn.numMeas = 0;
|
||||
rngBcn.sum = 0.0f;
|
||||
rngBcn.N = 0;
|
||||
rngBcn.maxPosD = 0.0f;
|
||||
rngBcn.minPosD = 0.0f;
|
||||
rngBcn.posDownOffsetMax = 0.0f;
|
||||
rngBcn.posOffsetMaxVar = 0.0f;
|
||||
rngBcn.maxOffsetStateChangeFilt = 0.0f;
|
||||
rngBcn.posDownOffsetMin = 0.0f;
|
||||
rngBcn.posOffsetMinVar = 0.0f;
|
||||
rngBcn.minOffsetStateChangeFilt = 0.0f;
|
||||
rngBcn.fuseDataReportIndex = 0;
|
||||
if (dal.beacon()) {
|
||||
if (rngBcnFusionReport == nullptr) {
|
||||
rngBcnFusionReport = new rngBcnFusionReport_t[dal.beacon()->count()];
|
||||
if (rngBcn.fusionReport == nullptr) {
|
||||
rngBcn.fusionReport = new BeaconFusion::FusionReport[dal.beacon()->count()];
|
||||
}
|
||||
}
|
||||
bcnPosOffsetNED.zero();
|
||||
bcnOriginEstInit = false;
|
||||
rngBcn.posOffsetNED.zero();
|
||||
rngBcn.originEstInit = false;
|
||||
#endif // EK3_FEATURE_BEACON_FUSION
|
||||
|
||||
#if EK3_FEATURE_BODY_ODOM
|
||||
@ -425,7 +425,7 @@ void NavEKF3_core::InitialiseVariables()
|
||||
storedRange.reset();
|
||||
storedOutput.reset();
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
storedRangeBeacon.reset();
|
||||
rngBcn.storedRange.reset();
|
||||
#endif
|
||||
#if EK3_FEATURE_BODY_ODOM
|
||||
storedBodyOdm.reset();
|
||||
@ -451,7 +451,6 @@ void NavEKF3_core::InitialiseVariables()
|
||||
effectiveMagCal = effective_magCal();
|
||||
}
|
||||
|
||||
|
||||
// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
|
||||
void NavEKF3_core::InitialiseVariablesMag()
|
||||
{
|
||||
@ -822,7 +821,7 @@ void NavEKF3_core::UpdateStrapdownEquationsNED()
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
// 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);
|
||||
rngBcn.receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -1316,53 +1316,56 @@ private:
|
||||
|
||||
// Range Beacon Sensor Fusion
|
||||
#if EK3_FEATURE_BEACON_FUSION
|
||||
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
|
||||
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
|
||||
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
|
||||
ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements
|
||||
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
|
||||
ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
|
||||
ftype innovRngBcn; // range beacon observation innovation (m)
|
||||
uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec)
|
||||
bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
|
||||
Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
|
||||
ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
|
||||
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
|
||||
bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
|
||||
uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
|
||||
Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
|
||||
ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
|
||||
bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started
|
||||
bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished
|
||||
uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter
|
||||
Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
|
||||
uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
|
||||
ftype rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
|
||||
uint8_t N_beacons; // Number of range beacons in use
|
||||
ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
|
||||
ftype minBcnPosD; // minimum position of all beacons in the down direction (m)
|
||||
bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used
|
||||
class BeaconFusion {
|
||||
public:
|
||||
EKF_obs_buffer_t<rng_bcn_elements> storedRange; // Beacon range buffer
|
||||
rng_bcn_elements dataDelayed; // Range beacon data at the fusion time horizon
|
||||
uint32_t lastPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
|
||||
ftype testRatio; // Innovation test ratio for range beacon measurements
|
||||
bool health; // boolean true if range beacon measurements have passed innovation consistency check
|
||||
ftype varInnov; // range beacon observation innovation variance (m^2)
|
||||
ftype innov; // range beacon observation innovation (m)
|
||||
uint32_t lastTime_ms[4]; // last time we received a range beacon measurement (msec)
|
||||
bool dataToFuse; // true when there is new range beacon data to fuse
|
||||
Vector3F vehiclePosNED; // NED position estimate from the beacon system (NED)
|
||||
ftype vehiclePosErr; // estimated position error from the beacon system (m)
|
||||
uint32_t last3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
|
||||
bool goodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
|
||||
uint8_t lastChecked; // index of the last range beacon checked for data
|
||||
Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
|
||||
ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
|
||||
bool alignmentStarted; // True when the initial position alignment using range measurements has started
|
||||
bool alignmentCompleted; // True when the initial position alignment using range measurements has finished
|
||||
uint8_t lastIndex; // Range beacon index last read - used during initialisation of the 3-state filter
|
||||
Vector3F posSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
|
||||
uint8_t numMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
|
||||
ftype sum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
|
||||
uint8_t N; // Number of range beacons in use
|
||||
ftype maxPosD; // maximum position of all beacons in the down direction (m)
|
||||
ftype minPosD; // minimum position of all beacons in the down direction (m)
|
||||
bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used
|
||||
|
||||
ftype bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
ftype bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
|
||||
ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh
|
||||
ftype posDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
ftype posOffsetMaxVar; // Variance of the PosDownOffsetMax state (m)
|
||||
ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in PosOffsetHigh
|
||||
|
||||
ftype bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
ftype bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
|
||||
ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow
|
||||
ftype posDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
|
||||
ftype posOffsetMinVar; // Variance of the PosDownOffsetMin state (m)
|
||||
ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in PosOffsetLow
|
||||
|
||||
Vector3F bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
|
||||
bool bcnOriginEstInit; // True when the beacon origin has been initialised
|
||||
Vector3F posOffsetNED; // NED position of the beacon origin in earth frame (m)
|
||||
bool originEstInit; // 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 rngBcnFusionReport_t {
|
||||
ftype rng; // measured range to beacon (m)
|
||||
ftype innov; // range innovation (m)
|
||||
ftype innovVar; // innovation variance (m^2)
|
||||
ftype testRatio; // innovation consistency test ratio
|
||||
Vector3F beaconPosNED; // beacon NED position
|
||||
} *rngBcnFusionReport;
|
||||
// Range Beacon Fusion Debug Reporting
|
||||
uint8_t fuseDataReportIndex;// index of range beacon fusion data last reported
|
||||
struct FusionReport {
|
||||
ftype rng; // measured range to beacon (m)
|
||||
ftype innov; // range innovation (m)
|
||||
ftype innovVar; // innovation variance (m^2)
|
||||
ftype testRatio; // innovation consistency test ratio
|
||||
Vector3F beaconPosNED; // beacon NED position
|
||||
} *fusionReport;
|
||||
} rngBcn;
|
||||
#endif // if EK3_FEATURE_BEACON_FUSION
|
||||
|
||||
#if EK3_FEATURE_DRAG_FUSION
|
||||
|
||||
Loading…
Reference in New Issue
Block a user