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

AP_NavEKF3: move beacon variables into structure

Browse Source 
AP_NavEKF3: change beacon variable names
This commit is contained in:
Peter Barker 2022-04-15 18:38:34 +10:00 committed by Andrew Tridgell
parent 2529c7fc40
commit 5d68f44694
8 changed files with 291 additions and 289 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -305,7 +305,7 @@ void NavEKF3_core::setAidingMode()
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
// Check if range beacon data is being used // 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 #else
const bool rngBcnUsed = false; const bool rngBcnUsed = false;
#endif #endif
@ -329,7 +329,7 @@ void NavEKF3_core::setAidingMode()
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) && attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - rngBcn.lastPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
#endif #endif
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
@ -346,7 +346,7 @@ void NavEKF3_core::setAidingMode()
} }
posAidLossCritical = posAidLossCritical =
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
(imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && (imuSampleTime_ms - rngBcn.lastPassTime_ms > maxLossTime_ms) &&
#endif #endif
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
} }
@ -426,8 +426,8 @@ void NavEKF3_core::setAidingMode()
// We are commencing aiding using range beacons // We are commencing aiding using range beacons
posResetSource = resetDataSource::RNGBCN; 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 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 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)bcnPosOffsetNED.z); 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 #endif // EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
} else if (readyToUseExtNav()) { } else if (readyToUseExtNav()) {
@ -454,7 +454,7 @@ void NavEKF3_core::setAidingMode()
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
lastRngBcnPassTime_ms = imuSampleTime_ms; rngBcn.lastPassTime_ms = imuSampleTime_ms;
#endif #endif
break; break;
} }
@ -560,7 +560,7 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const
return false; return false;
} }
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse; return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcn.alignmentCompleted && rngBcn.dataToFuse;
#else #else
return false; return false;
#endif // EK3_FEATURE_BEACON_FUSION #endif // EK3_FEATURE_BEACON_FUSION

24
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -229,20 +229,20 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
return; return;
} }
if (!statesInitialised || N_beacons == 0 || rngBcnFusionReport == nullptr) { if (!statesInitialised || rngBcn.N == 0 || rngBcn.fusionReport == nullptr) {
return; return;
} }
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
if (rngBcnFuseDataReportIndex >= N_beacons) { if (rngBcn.fuseDataReportIndex >= rngBcn.N) {
rngBcnFuseDataReportIndex = 0; 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 // write range beacon fusion debug packet if the range value is non-zero
if (report.rng <= 0.0f) { if (report.rng <= 0.0f) {
rngBcnFuseDataReportIndex++; rngBcn.fuseDataReportIndex++;
return; return;
} }
@ -250,7 +250,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
LOG_PACKET_HEADER_INIT(LOG_XKF0_MSG), LOG_PACKET_HEADER_INIT(LOG_XKF0_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(core_index), core : DAL_CORE(core_index),
ID : rngBcnFuseDataReportIndex, ID : rngBcn.fuseDataReportIndex,
rng : (int16_t)(100*report.rng), rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov), innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)), 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), beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y), beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z), beaconPosD : (int16_t)(100*report.beaconPosNED.z),
offsetHigh : (int16_t)(100*bcnPosDownOffsetMax), offsetHigh : (int16_t)(100*rngBcn.posDownOffsetMax),
offsetLow : (int16_t)(100*bcnPosDownOffsetMin), offsetLow : (int16_t)(100*rngBcn.posDownOffsetMin),
posN : (int16_t)(100*receiverPos.x), posN : (int16_t)(100*rngBcn.receiverPos.x),
posE : (int16_t)(100*receiverPos.y), posE : (int16_t)(100*rngBcn.receiverPos.y),
posD : (int16_t)(100*receiverPos.z) posD : (int16_t)(100*rngBcn.receiverPos.z)
}; };
AP::logger().WriteBlock(&pkt10, sizeof(pkt10)); AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
rngBcnFuseDataReportIndex++; rngBcn.fuseDataReportIndex++;
} }
#endif // EK3_FEATURE_BEACON_FUSION #endif // EK3_FEATURE_BEACON_FUSION

48
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -875,7 +875,7 @@ void NavEKF3_core::readAirSpdData()
void NavEKF3_core::readRngBcnData() void NavEKF3_core::readRngBcnData()
{ {
// check that arrays are large enough // 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 // get the location of the beacon data
const AP_DAL_Beacon *beacon = dal.beacon(); const AP_DAL_Beacon *beacon = dal.beacon();
@ -886,30 +886,30 @@ void NavEKF3_core::readRngBcnData()
} }
// get the number of beacons in use // 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 // 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; bool newDataPushed = false;
uint8_t numRngBcnsChecked = 0; uint8_t numRngBcnsChecked = 0;
// start the search one index up from where we left it last time // start the search one index up from where we left it last time
uint8_t index = lastRngBcnChecked; uint8_t index = rngBcn.lastChecked;
while (!newDataPushed && (numRngBcnsChecked < N_beacons)) { while (!newDataPushed && (numRngBcnsChecked < rngBcn.N)) {
// track the number of beacons checked // track the number of beacons checked
numRngBcnsChecked++; numRngBcnsChecked++;
// move to next beacon, wrap index if necessary // move to next beacon, wrap index if necessary
index++; index++;
if (index >= N_beacons) { if (index >= rngBcn.N) {
index = 0; index = 0;
} }
// check that the beacon is healthy and has new data // 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 = {}; rng_bcn_elements rngBcnDataNew = {};
// set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate // 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); rngBcn.lastTime_ms[index] = beacon->beacon_last_update_ms(index);
rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2; rngBcnDataNew.time_ms = rngBcn.lastTime_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
// set the range noise // set the range noise
// TODO the range library should provide the noise/accuracy estimate for each beacon // TODO the range library should provide the noise/accuracy estimate for each beacon
@ -928,10 +928,10 @@ void NavEKF3_core::readRngBcnData()
newDataPushed = true; newDataPushed = true;
// update the last checked index // 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 // 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; Vector3f bp;
float bperr; float bperr;
if (beacon->get_vehicle_position_ned(bp, bperr)) { if (beacon->get_vehicle_position_ned(bp, bperr)) {
rngBcnLast3DmeasTime_ms = imuSampleTime_ms; rngBcn.last3DmeasTime_ms = imuSampleTime_ms;
} }
beaconVehiclePosNED = bp.toftype(); rngBcn.vehiclePosNED = bp.toftype();
beaconVehiclePosErr = bperr; rngBcn.vehiclePosErr = bperr;
// Check if the range beacon data can be used to align the vehicle position // 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 // 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 posDiffSq = sq(rngBcn.receiverPos.x - rngBcn.vehiclePosNED.x) + sq(rngBcn.receiverPos.y - rngBcn.vehiclePosNED.y);
const ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; const ftype posDiffVar = sq(rngBcn.vehiclePosErr) + rngBcn.receiverPosCov[0][0] + rngBcn.receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) { if (posDiffSq < 9.0f * posDiffVar) {
rngBcnGoodToAlign = true; rngBcn.goodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set // Set the EKF origin and magnetic field declination if not previously set
if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) { if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) {
// get origin from beacon system // get origin from beacon system
@ -963,23 +963,23 @@ void NavEKF3_core::readRngBcnData()
alignMagStateDeclination(); alignMagStateDeclination();
// Set the uncertainty of the origin height // Set the uncertainty of the origin height
ekfOriginHgtVar = sq(beaconVehiclePosErr); ekfOriginHgtVar = sq(rngBcn.vehiclePosErr);
} }
} }
} else { } else {
rngBcnGoodToAlign = false; rngBcn.goodToAlign = false;
} }
} else { } 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 // 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 // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
if (rngBcnDataToFuse) { if (rngBcn.dataToFuse) {
rngBcnDataDelayed.beacon_posNED.x += bcnPosOffsetNED.x; rngBcn.dataDelayed.beacon_posNED.x += rngBcn.posOffsetNED.x;
rngBcnDataDelayed.beacon_posNED.y += bcnPosOffsetNED.y; rngBcn.dataDelayed.beacon_posNED.y += rngBcn.posOffsetNED.y;
} }
} }

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -236,10 +236,10 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat(); posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
return false; return false;
#if EK3_FEATURE_BEACON_FUSION #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 // If we are attempting alignment using range beacon data, then report the position
posNE.x = receiverPos.x; posNE.x = rngBcn.receiverPos.x;
posNE.y = receiverPos.y; posNE.y = rngBcn.receiverPos.y;
return false; return false;
#endif #endif
} else { } else {

14
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -105,13 +105,13 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
// set the variances using the position measurement noise parameter // set the variances using the position measurement noise parameter
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise)); P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
#if EK3_FEATURE_BEACON_FUSION #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 // use the range beacon data as a second preference
stateStruct.position.x = receiverPos.x; stateStruct.position.x = rngBcn.receiverPos.x;
stateStruct.position.y = receiverPos.y; stateStruct.position.y = rngBcn.receiverPos.y;
// set the variances from the beacon alignment filter // set the variances from the beacon alignment filter
P[7][7] = receiverPosCov[0][0]; P[7][7] = rngBcn.receiverPosCov[0][0];
P[8][8] = receiverPosCov[1][1]; P[8][8] = rngBcn.receiverPosCov[1][1];
#endif #endif
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) { } 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) { } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = AP_NavEKF_Source::SourceZ::GPS; activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
#if EK3_FEATURE_BEACON_FUSION #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; activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
#endif #endif
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
@ -1141,7 +1141,7 @@ void NavEKF3_core::selectHeightForFusion()
bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh); bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000 || !gpsAccuracyGoodForAltitude)); bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000 || !gpsAccuracyGoodForAltitude));
#if EK3_FEATURE_BEACON_FUSION #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 #endif
bool fallback_to_baro = bool fallback_to_baro =
lostRngHgt lostRngHgt

308
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -14,13 +14,13 @@ void NavEKF3_core::SelectRngBcnFusion()
readRngBcnData(); readRngBcnData();
// Determine if we need to fuse range beacon data on this time step // Determine if we need to fuse range beacon data on this time step
if (rngBcnDataToFuse) { if (rngBcn.dataToFuse) {
if (PV_AidingMode == AID_ABSOLUTE) { if (PV_AidingMode == AID_ABSOLUTE) {
if ((frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcnAlignmentCompleted) { if ((frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcn.alignmentCompleted) {
if (!bcnOriginEstInit) { if (!rngBcn.originEstInit) {
bcnOriginEstInit = true; rngBcn.originEstInit = true;
bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x; rngBcn.posOffsetNED.x = rngBcn.receiverPos.x - stateStruct.position.x;
bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y; rngBcn.posOffsetNED.y = rngBcn.receiverPos.y - stateStruct.position.y;
} }
// beacons are used as the primary means of position reference // beacons are used as the primary means of position reference
FuseRngBcn(); FuseRngBcn();
@ -30,13 +30,13 @@ void NavEKF3_core::SelectRngBcnFusion()
// start using beacon data as the primary reference. // start using beacon data as the primary reference.
FuseRngBcnStatic(); FuseRngBcnStatic();
// record that the beacon origin needs to be initialised // record that the beacon origin needs to be initialised
bcnOriginEstInit = false; rngBcn.originEstInit = false;
} }
} else { } else {
// If we aren't able to use the data in the main filter, use a simple 3-state filter to estimate 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(); FuseRngBcnStatic();
// record that the beacon origin needs to be initialised // 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_pn;
ftype bcn_pe; ftype bcn_pe;
ftype bcn_pd; 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; ftype rngPred;
// health is set bad until test passed // health is set bad until test passed
rngBcnHealth = false; rngBcn.health = false;
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) { if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
// calculate the vertical offset from EKF datum to beacon datum // calculate the vertical offset from EKF datum to beacon datum
CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false); CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
} else { } else {
bcnPosOffsetNED.z = 0.0f; rngBcn.posOffsetNED.z = 0.0f;
} }
// copy required states to local variable names // copy required states to local variable names
pn = stateStruct.position.x; pn = stateStruct.position.x;
pe = stateStruct.position.y; pe = stateStruct.position.y;
pd = stateStruct.position.z; pd = stateStruct.position.z;
bcn_pn = rngBcnDataDelayed.beacon_posNED.x; bcn_pn = rngBcn.dataDelayed.beacon_posNED.x;
bcn_pe = rngBcnDataDelayed.beacon_posNED.y; bcn_pe = rngBcn.dataDelayed.beacon_posNED.y;
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffsetNED.z; bcn_pd = rngBcn.dataDelayed.beacon_posNED.z + rngBcn.posOffsetNED.z;
// predicted range // predicted range
Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; Vector3F deltaPosNED = stateStruct.position - rngBcn.dataDelayed.beacon_posNED;
rngPred = deltaPosNED.length(); rngPred = deltaPosNED.length();
// calculate measurement innovation // calculate measurement innovation
innovRngBcn = rngPred - rngBcnDataDelayed.rng; rngBcn.innov = rngPred - rngBcn.dataDelayed.rng;
// perform fusion of range measurement // perform fusion of range measurement
if (rngPred > 0.1f) if (rngPred > 0.1f)
@ -119,10 +119,10 @@ void NavEKF3_core::FuseRngBcn()
ftype t22 = P[7][7]*t4*t9; ftype t22 = P[7][7]*t4*t9;
ftype t23 = t20+t21+t22; ftype t23 = t20+t21+t22;
ftype t24 = t4*t9*t23; ftype t24 = t4*t9*t23;
varInnovRngBcn = R_BCN+t14+t19+t24; rngBcn.varInnov = R_BCN+t14+t19+t24;
ftype t26; ftype t26;
if (varInnovRngBcn >= R_BCN) { if (rngBcn.varInnov >= R_BCN) {
t26 = 1.0f/varInnovRngBcn; t26 = 1.0f/rngBcn.varInnov;
faultStatus.bad_rngbcn = false; faultStatus.bad_rngbcn = false;
} else { } else {
// the calculation is badly conditioned, so we cannot perform fusion on this step // 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 // Calculate innovation using the selected offset value
Vector3F delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED; Vector3F delta = stateStruct.position - rngBcn.dataDelayed.beacon_posNED;
innovRngBcn = delta.length() - rngBcnDataDelayed.rng; rngBcn.innov = delta.length() - rngBcn.dataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // 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 // test the ratio before fusing data
if (rngBcnHealth) { if (rngBcn.health) {
// restart the counter // restart the counter
lastRngBcnPassTime_ms = imuSampleTime_ms; rngBcn.lastPassTime_ms = imuSampleTime_ms;
// correct the covariance P = (I - K*H)*P // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the // take advantage of the empty columns in KH to reduce the
@ -253,7 +253,7 @@ void NavEKF3_core::FuseRngBcn()
// correct the state vector // correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) { 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 // record healthy fusion
@ -267,12 +267,12 @@ void NavEKF3_core::FuseRngBcn()
} }
// Update the fusion report // Update the fusion report
if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) { if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; 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() void NavEKF3_core::FuseRngBcnStatic()
{ {
// get the estimated range measurement variance // 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 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 initialise the initial position to the mean beacon position. The initial position uncertainty
is set to the mean range measurement. is set to the mean range measurement.
*/ */
if (!rngBcnAlignmentStarted) { if (!rngBcn.alignmentStarted) {
if (rngBcnDataDelayed.beacon_ID != lastBeaconIndex) { if (rngBcn.dataDelayed.beacon_ID != rngBcn.lastIndex) {
rngBcnPosSum += rngBcnDataDelayed.beacon_posNED; rngBcn.posSum += rngBcn.dataDelayed.beacon_posNED;
lastBeaconIndex = rngBcnDataDelayed.beacon_ID; rngBcn.lastIndex = rngBcn.dataDelayed.beacon_ID;
rngSum += rngBcnDataDelayed.rng; rngBcn.sum += rngBcn.dataDelayed.rng;
numBcnMeas++; rngBcn.numMeas++;
// capture the beacon vertical spread // capture the beacon vertical spread
if (rngBcnDataDelayed.beacon_posNED.z > maxBcnPosD) { if (rngBcn.dataDelayed.beacon_posNED.z > rngBcn.maxPosD) {
maxBcnPosD = rngBcnDataDelayed.beacon_posNED.z; rngBcn.maxPosD = rngBcn.dataDelayed.beacon_posNED.z;
} else if(rngBcnDataDelayed.beacon_posNED.z < minBcnPosD) { } else if(rngBcn.dataDelayed.beacon_posNED.z < rngBcn.minPosD) {
minBcnPosD = rngBcnDataDelayed.beacon_posNED.z; rngBcn.minPosD = rngBcn.dataDelayed.beacon_posNED.z;
} }
} }
if (numBcnMeas >= 100) { if (rngBcn.numMeas >= 100) {
rngBcnAlignmentStarted = true; rngBcn.alignmentStarted = true;
ftype tempVar = 1.0f / (ftype)numBcnMeas; ftype tempVar = 1.0f / (ftype)rngBcn.numMeas;
// initialise the receiver position to the centre of the beacons and at zero height // initialise the receiver position to the centre of the beacons and at zero height
receiverPos.x = rngBcnPosSum.x * tempVar; rngBcn.receiverPos.x = rngBcn.posSum.x * tempVar;
receiverPos.y = rngBcnPosSum.y * tempVar; rngBcn.receiverPos.y = rngBcn.posSum.y * tempVar;
receiverPos.z = 0.0f; rngBcn.receiverPos.z = 0.0f;
receiverPosCov[2][2] = receiverPosCov[1][1] = receiverPosCov[0][0] = rngSum * tempVar; rngBcn.receiverPosCov[2][2] = rngBcn.receiverPosCov[1][1] = rngBcn.receiverPosCov[0][0] = rngBcn.sum * tempVar;
lastBeaconIndex = 0; rngBcn.lastIndex = 0;
numBcnMeas = 0; rngBcn.numMeas = 0;
rngBcnPosSum.zero(); rngBcn.posSum.zero();
rngSum = 0.0f; rngBcn.sum = 0.0f;
} }
} }
if (rngBcnAlignmentStarted) { if (rngBcn.alignmentStarted) {
numBcnMeas++; rngBcn.numMeas++;
if (numBcnMeas >= 100) { if (rngBcn.numMeas >= 100) {
// 100 observations is enough for a stable estimate under most conditions // 100 observations is enough for a stable estimate under most conditions
// TODO monitor stability of the position estimate // TODO monitor stability of the position estimate
rngBcnAlignmentCompleted = true; rngBcn.alignmentCompleted = true;
} }
if (rngBcnAlignmentCompleted) { if (rngBcn.alignmentCompleted) {
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) { if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
// We are using a different height reference for the main EKF so need to estimate a vertical // 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 // 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 // The position estimate should be stable after 100 iterations so we use a simple dual
// hypothesis 1-state EKF to estimate the offset // hypothesis 1-state EKF to estimate the offset
Vector3F refPosNED; Vector3F refPosNED;
refPosNED.x = receiverPos.x; refPosNED.x = rngBcn.receiverPos.x;
refPosNED.y = receiverPos.y; refPosNED.y = rngBcn.receiverPos.y;
refPosNED.z = stateStruct.position.z; refPosNED.z = stateStruct.position.z;
CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true); CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true);
} else { } else {
// we are using the beacons as the primary height source, so don't modify their vertical position // 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 { } else {
@ -357,39 +357,39 @@ void NavEKF3_core::FuseRngBcnStatic()
// and the main EKF vertical position // and the main EKF vertical position
// Calculate the mid vertical position of all beacons // 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 // 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 // calculate the two hypothesis for our vertical position
ftype receiverPosDownMax; ftype receiverPosDownMax;
ftype receiverPosDownMin; ftype receiverPosDownMin;
if (delta >= 0.0f) { if (delta >= 0.0f) {
receiverPosDownMax = receiverPos.z; receiverPosDownMax = rngBcn.receiverPos.z;
receiverPosDownMin = receiverPos.z - 2.0f * delta; receiverPosDownMin = rngBcn.receiverPos.z - 2.0f * delta;
} else { } else {
receiverPosDownMax = receiverPos.z - 2.0f * delta; receiverPosDownMax = rngBcn.receiverPos.z - 2.0f * delta;
receiverPosDownMin = receiverPos.z; receiverPosDownMin = rngBcn.receiverPos.z;
} }
bcnPosDownOffsetMax = stateStruct.position.z - receiverPosDownMin; rngBcn.posDownOffsetMax = stateStruct.position.z - receiverPosDownMin;
bcnPosDownOffsetMin = stateStruct.position.z - receiverPosDownMax; rngBcn.posDownOffsetMin = stateStruct.position.z - receiverPosDownMax;
} else { } else {
// We are using the beacons as the primary height reference, so don't modify their vertical position // 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 // Add some process noise to the states at each time step
for (uint8_t i= 0; i<=2; i++) { for (uint8_t i= 0; i<=2; i++) {
receiverPosCov[i][i] += 0.1f; rngBcn.receiverPosCov[i][i] += 0.1f;
} }
// calculate the observation jacobian // calculate the observation jacobian
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffsetNED.z; ftype t2 = rngBcn.dataDelayed.beacon_posNED.z - rngBcn.receiverPos.z + rngBcn.posOffsetNED.z;
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; ftype t3 = rngBcn.dataDelayed.beacon_posNED.y - rngBcn.receiverPos.y;
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; ftype t4 = rngBcn.dataDelayed.beacon_posNED.x - rngBcn.receiverPos.x;
ftype t5 = t2*t2; ftype t5 = t2*t2;
ftype t6 = t3*t3; ftype t6 = t3*t3;
ftype t7 = t4*t4; ftype t7 = t4*t4;
@ -399,14 +399,14 @@ void NavEKF3_core::FuseRngBcnStatic()
return; return;
} }
ftype t9 = 1.0f/sqrtF(t8); ftype t9 = 1.0f/sqrtF(t8);
ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; ftype t10 = rngBcn.dataDelayed.beacon_posNED.x*2.0f;
ftype t15 = receiverPos.x*2.0f; ftype t15 = rngBcn.receiverPos.x*2.0f;
ftype t11 = t10-t15; ftype t11 = t10-t15;
ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f; ftype t12 = rngBcn.dataDelayed.beacon_posNED.y*2.0f;
ftype t14 = receiverPos.y*2.0f; ftype t14 = rngBcn.receiverPos.y*2.0f;
ftype t13 = t12-t14; ftype t13 = t12-t14;
ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f; ftype t16 = rngBcn.dataDelayed.beacon_posNED.z*2.0f;
ftype t18 = receiverPos.z*2.0f; ftype t18 = rngBcn.receiverPos.z*2.0f;
ftype t17 = t16-t18; ftype t17 = t16-t18;
ftype H_RNG[3]; ftype H_RNG[3];
H_RNG[0] = -t9*t11*0.5f; H_RNG[0] = -t9*t11*0.5f;
@ -414,46 +414,46 @@ void NavEKF3_core::FuseRngBcnStatic()
H_RNG[2] = -t9*t17*0.5f; H_RNG[2] = -t9*t17*0.5f;
// calculate the Kalman gains // calculate the Kalman gains
ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f; ftype t19 = rngBcn.receiverPosCov[0][0]*t9*t11*0.5f;
ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f; ftype t20 = rngBcn.receiverPosCov[1][1]*t9*t13*0.5f;
ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f; ftype t21 = rngBcn.receiverPosCov[0][1]*t9*t11*0.5f;
ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f; ftype t22 = rngBcn.receiverPosCov[2][1]*t9*t17*0.5f;
ftype t23 = t20+t21+t22; ftype t23 = t20+t21+t22;
ftype t24 = t9*t13*t23*0.5f; ftype t24 = t9*t13*t23*0.5f;
ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f; ftype t25 = rngBcn.receiverPosCov[1][2]*t9*t13*0.5f;
ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f; ftype t26 = rngBcn.receiverPosCov[0][2]*t9*t11*0.5f;
ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f; ftype t27 = rngBcn.receiverPosCov[2][2]*t9*t17*0.5f;
ftype t28 = t25+t26+t27; ftype t28 = t25+t26+t27;
ftype t29 = t9*t17*t28*0.5f; ftype t29 = t9*t17*t28*0.5f;
ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f; ftype t30 = rngBcn.receiverPosCov[1][0]*t9*t13*0.5f;
ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f; ftype t31 = rngBcn.receiverPosCov[2][0]*t9*t17*0.5f;
ftype t32 = t19+t30+t31; ftype t32 = t19+t30+t31;
ftype t33 = t9*t11*t32*0.5f; ftype t33 = t9*t11*t32*0.5f;
varInnovRngBcn = R_RNG+t24+t29+t33; rngBcn.varInnov = R_RNG+t24+t29+t33;
ftype t35 = 1.0f/varInnovRngBcn; ftype t35 = 1.0f/rngBcn.varInnov;
ftype K_RNG[3]; 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[0] = -t35*(t19+rngBcn.receiverPosCov[0][1]*t9*t13*0.5f+rngBcn.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[1] = -t35*(t20+rngBcn.receiverPosCov[1][0]*t9*t11*0.5f+rngBcn.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[2] = -t35*(t27+rngBcn.receiverPosCov[2][0]*t9*t11*0.5f+rngBcn.receiverPosCov[2][1]*t9*t13*0.5f);
// calculate range measurement innovation // calculate range measurement innovation
Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; Vector3F deltaPosNED = rngBcn.receiverPos - rngBcn.dataDelayed.beacon_posNED;
deltaPosNED.z -= bcnPosOffsetNED.z; deltaPosNED.z -= rngBcn.posOffsetNED.z;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; rngBcn.innov = deltaPosNED.length() - rngBcn.dataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // 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 // test the ratio before fusing data
if (rngBcnHealth) { if (rngBcn.health) {
// update the state // update the state
receiverPos.x -= K_RNG[0] * innovRngBcn; rngBcn.receiverPos.x -= K_RNG[0] * rngBcn.innov;
receiverPos.y -= K_RNG[1] * innovRngBcn; rngBcn.receiverPos.y -= K_RNG[1] * rngBcn.innov;
receiverPos.z -= K_RNG[2] * innovRngBcn; rngBcn.receiverPos.z -= K_RNG[2] * rngBcn.innov;
// calculate the covariance correction // calculate the covariance correction
for (unsigned i = 0; i<=2; i++) { for (unsigned i = 0; i<=2; i++) {
@ -464,53 +464,53 @@ void NavEKF3_core::FuseRngBcnStatic()
for (unsigned j = 0; j<=2; j++) { for (unsigned j = 0; j<=2; j++) {
for (unsigned i = 0; i<=2; i++) { for (unsigned i = 0; i<=2; i++) {
ftype res = 0; ftype res = 0;
res += KH[i][0] * receiverPosCov[0][j]; res += KH[i][0] * rngBcn.receiverPosCov[0][j];
res += KH[i][1] * receiverPosCov[1][j]; res += KH[i][1] * rngBcn.receiverPosCov[1][j];
res += KH[i][2] * receiverPosCov[2][j]; res += KH[i][2] * rngBcn.receiverPosCov[2][j];
KHP[i][j] = res; KHP[i][j] = res;
} }
} }
// prevent negative variances // prevent negative variances
for (uint8_t i= 0; i<=2; i++) { for (uint8_t i= 0; i<=2; i++) {
if (receiverPosCov[i][i] < 0.0f) { if (rngBcn.receiverPosCov[i][i] < 0.0f) {
receiverPosCov[i][i] = 0.0f; rngBcn.receiverPosCov[i][i] = 0.0f;
KHP[i][i] = 0.0f; KHP[i][i] = 0.0f;
} else if (KHP[i][i] > receiverPosCov[i][i]) { } else if (KHP[i][i] > rngBcn.receiverPosCov[i][i]) {
KHP[i][i] = receiverPosCov[i][i]; KHP[i][i] = rngBcn.receiverPosCov[i][i];
} }
} }
// apply the covariance correction // apply the covariance correction
for (uint8_t i= 0; i<=2; i++) { for (uint8_t i= 0; i<=2; i++) {
for (uint8_t j= 0; j<=2; j++) { 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 // ensure the covariance matrix is symmetric
for (uint8_t i=1; i<=2; i++) { for (uint8_t i=1; i<=2; i++) {
for (uint8_t j=0; j<=i-1; j++) { for (uint8_t j=0; j<=i-1; j++) {
ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]); ftype temp = 0.5f*(rngBcn.receiverPosCov[i][j] + rngBcn.receiverPosCov[j][i]);
receiverPosCov[i][j] = temp; rngBcn.receiverPosCov[i][j] = temp;
receiverPosCov[j][i] = temp; rngBcn.receiverPosCov[j][i] = temp;
} }
} }
} }
if (numBcnMeas >= 100) { if (rngBcn.numMeas >= 100) {
// 100 observations is enough for a stable estimate under most conditions // 100 observations is enough for a stable estimate under most conditions
// TODO monitor stability of the position estimate // TODO monitor stability of the position estimate
rngBcnAlignmentCompleted = true; rngBcn.alignmentCompleted = true;
} }
// Update the fusion report // Update the fusion report
if (rngBcnFusionReport && rngBcnDataDelayed.beacon_ID < dal.beacon()->count()) { if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; 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 // estimate upper value for offset
// calculate observation derivative // calculate observation derivative
ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax; ftype t2 = rngBcn.dataDelayed.beacon_posNED.z - vehiclePosNED.z + rngBcn.posDownOffsetMax;
ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y; ftype t3 = rngBcn.dataDelayed.beacon_posNED.y - vehiclePosNED.y;
ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x; ftype t4 = rngBcn.dataDelayed.beacon_posNED.x - vehiclePosNED.x;
ftype t5 = t2*t2; ftype t5 = t2*t2;
ftype t6 = t3*t3; ftype t6 = t3*t3;
ftype t7 = t4*t4; ftype t7 = t4*t4;
@ -552,38 +552,38 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
obsDeriv = t2*t9; obsDeriv = t2*t9;
// Calculate innovation // Calculate innovation
innov = sqrtF(t8) - rngBcnDataDelayed.rng; innov = sqrtF(t8) - rngBcn.dataDelayed.rng;
// covariance prediction // covariance prediction
bcnPosOffsetMaxVar += stateNoiseVar; rngBcn.posOffsetMaxVar += stateNoiseVar;
// calculate the innovation variance // calculate the innovation variance
innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar; innovVar = obsDeriv * rngBcn.posOffsetMaxVar * obsDeriv + obsVar;
innovVar = MAX(innovVar, obsVar); innovVar = MAX(innovVar, obsVar);
// calculate the Kalman gain // 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 // calculate a filtered state change magnitude to be used to select between the high or low offset
ftype stateChange = innov * gain; 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 // Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// state update // state update
bcnPosDownOffsetMax -= stateChange; rngBcn.posDownOffsetMax -= stateChange;
// covariance update // covariance update
bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar; rngBcn.posOffsetMaxVar -= gain * obsDeriv * rngBcn.posOffsetMaxVar;
bcnPosOffsetMaxVar = MAX(bcnPosOffsetMaxVar, 0.0f); rngBcn.posOffsetMaxVar = MAX(rngBcn.posOffsetMaxVar, 0.0f);
} }
} }
// estimate lower value for offset // estimate lower value for offset
// calculate observation derivative // calculate observation derivative
t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMin; t2 = rngBcn.dataDelayed.beacon_posNED.z - vehiclePosNED.z + rngBcn.posDownOffsetMin;
t5 = t2*t2; t5 = t2*t2;
t8 = t5+t6+t7; t8 = t5+t6+t7;
if (t8 > 0.1f) { if (t8 > 0.1f) {
@ -591,56 +591,56 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehicleP
obsDeriv = t2*t9; obsDeriv = t2*t9;
// Calculate innovation // Calculate innovation
innov = sqrtF(t8) - rngBcnDataDelayed.rng; innov = sqrtF(t8) - rngBcn.dataDelayed.rng;
// covariance prediction // covariance prediction
bcnPosOffsetMinVar += stateNoiseVar; rngBcn.posOffsetMinVar += stateNoiseVar;
// calculate the innovation variance // calculate the innovation variance
innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar; innovVar = obsDeriv * rngBcn.posOffsetMinVar * obsDeriv + obsVar;
innovVar = MAX(innovVar, obsVar); innovVar = MAX(innovVar, obsVar);
// calculate the Kalman gain // 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 // calculate a filtered state change magnitude to be used to select between the high or low offset
ftype stateChange = innov * gain; 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 // Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// state update // state update
bcnPosDownOffsetMin -= stateChange; rngBcn.posDownOffsetMin -= stateChange;
// covariance update // covariance update
bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar; rngBcn.posOffsetMinVar -= gain * obsDeriv * rngBcn.posOffsetMinVar;
bcnPosOffsetMinVar = MAX(bcnPosOffsetMinVar, 0.0f); rngBcn.posOffsetMinVar = MAX(rngBcn.posOffsetMinVar, 0.0f);
} }
} }
// calculate the mid vertical position of all beacons // 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 // 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); rngBcn.posDownOffsetMax = MAX(rngBcn.posDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
bcnPosDownOffsetMin = MIN(bcnPosDownOffsetMin, 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 // calculate the innovation for the main filter using the offset that is most stable
// apply hysteresis to prevent rapid switching // apply hysteresis to prevent rapid switching
if (!usingMinHypothesis && (minOffsetStateChangeFilt < (0.8f * maxOffsetStateChangeFilt))) { if (!rngBcn.usingMinHypothesis && (rngBcn.minOffsetStateChangeFilt < (0.8f * rngBcn.maxOffsetStateChangeFilt))) {
usingMinHypothesis = true; rngBcn.usingMinHypothesis = true;
} else if (usingMinHypothesis && (maxOffsetStateChangeFilt < (0.8f * minOffsetStateChangeFilt))) { } else if (rngBcn.usingMinHypothesis && (rngBcn.maxOffsetStateChangeFilt < (0.8f * rngBcn.minOffsetStateChangeFilt))) {
usingMinHypothesis = false; rngBcn.usingMinHypothesis = false;
} }
if (usingMinHypothesis) { if (rngBcn.usingMinHypothesis) {
bcnPosOffsetNED.z = bcnPosDownOffsetMin; rngBcn.posOffsetNED.z = rngBcn.posDownOffsetMin;
} else { } else {
bcnPosOffsetNED.z = bcnPosDownOffsetMax; rngBcn.posOffsetNED.z = rngBcn.posDownOffsetMax;
} }
// apply the vertical offset to the beacon positions // 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 #endif // EK3_FEATURE_BEACON_FUSION

77
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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 // Note: range beacon data is read one beacon at a time and can arrive at a high rate
#if EK3_FEATURE_BEACON_FUSION #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; return false;
} }
#endif #endif
@ -345,44 +345,44 @@ void NavEKF3_core::InitialiseVariables()
// range beacon fusion variables // range beacon fusion variables
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed)); memset((void *)&rngBcn.dataDelayed, 0, sizeof(rngBcn.dataDelayed));
lastRngBcnPassTime_ms = 0; rngBcn.lastPassTime_ms = 0;
rngBcnTestRatio = 0.0f; rngBcn.testRatio = 0.0f;
rngBcnHealth = false; rngBcn.health = false;
varInnovRngBcn = 0.0f; rngBcn.varInnov = 0.0f;
innovRngBcn = 0.0f; rngBcn.innov = 0.0f;
memset(&lastTimeRngBcn_ms, 0, sizeof(lastTimeRngBcn_ms)); memset(&rngBcn.lastTime_ms, 0, sizeof(rngBcn.lastTime_ms));
rngBcnDataToFuse = false; rngBcn.dataToFuse = false;
beaconVehiclePosNED.zero(); rngBcn.vehiclePosNED.zero();
beaconVehiclePosErr = 1.0f; rngBcn.vehiclePosErr = 1.0f;
rngBcnLast3DmeasTime_ms = 0; rngBcn.last3DmeasTime_ms = 0;
rngBcnGoodToAlign = false; rngBcn.goodToAlign = false;
lastRngBcnChecked = 0; rngBcn.lastChecked = 0;
receiverPos.zero(); rngBcn.receiverPos.zero();
memset(&receiverPosCov, 0, sizeof(receiverPosCov)); memset(&rngBcn.receiverPosCov, 0, sizeof(rngBcn.receiverPosCov));
rngBcnAlignmentStarted = false; rngBcn.alignmentStarted = false;
rngBcnAlignmentCompleted = false; rngBcn.alignmentCompleted = false;
lastBeaconIndex = 0; rngBcn.lastIndex = 0;
rngBcnPosSum.zero(); rngBcn.posSum.zero();
numBcnMeas = 0; rngBcn.numMeas = 0;
rngSum = 0.0f; rngBcn.sum = 0.0f;
N_beacons = 0; rngBcn.N = 0;
maxBcnPosD = 0.0f; rngBcn.maxPosD = 0.0f;
minBcnPosD = 0.0f; rngBcn.minPosD = 0.0f;
bcnPosDownOffsetMax = 0.0f; rngBcn.posDownOffsetMax = 0.0f;
bcnPosOffsetMaxVar = 0.0f; rngBcn.posOffsetMaxVar = 0.0f;
maxOffsetStateChangeFilt = 0.0f; rngBcn.maxOffsetStateChangeFilt = 0.0f;
bcnPosDownOffsetMin = 0.0f; rngBcn.posDownOffsetMin = 0.0f;
bcnPosOffsetMinVar = 0.0f; rngBcn.posOffsetMinVar = 0.0f;
minOffsetStateChangeFilt = 0.0f; rngBcn.minOffsetStateChangeFilt = 0.0f;
rngBcnFuseDataReportIndex = 0; rngBcn.fuseDataReportIndex = 0;
if (dal.beacon()) { if (dal.beacon()) {
if (rngBcnFusionReport == nullptr) { if (rngBcn.fusionReport == nullptr) {
rngBcnFusionReport = new rngBcnFusionReport_t[dal.beacon()->count()]; rngBcn.fusionReport = new BeaconFusion::FusionReport[dal.beacon()->count()];
} }
} }
bcnPosOffsetNED.zero(); rngBcn.posOffsetNED.zero();
bcnOriginEstInit = false; rngBcn.originEstInit = false;
#endif // EK3_FEATURE_BEACON_FUSION #endif // EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_BODY_ODOM #if EK3_FEATURE_BODY_ODOM
@ -425,7 +425,7 @@ void NavEKF3_core::InitialiseVariables()
storedRange.reset(); storedRange.reset();
storedOutput.reset(); storedOutput.reset();
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
storedRangeBeacon.reset(); rngBcn.storedRange.reset();
#endif #endif
#if EK3_FEATURE_BODY_ODOM #if EK3_FEATURE_BODY_ODOM
storedBodyOdm.reset(); storedBodyOdm.reset();
@ -451,7 +451,6 @@ void NavEKF3_core::InitialiseVariables()
effectiveMagCal = effective_magCal(); 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. // 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() void NavEKF3_core::InitialiseVariablesMag()
{ {
@ -822,7 +821,7 @@ void NavEKF3_core::UpdateStrapdownEquationsNED()
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
// If main filter velocity states are valid, update the range beacon receiver position states // If main filter velocity states are valid, update the range beacon receiver position states
if (filterStatus.flags.horiz_vel) { if (filterStatus.flags.horiz_vel) {
receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f); rngBcn.receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
} }
#endif #endif
} }

89
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1316,53 +1316,56 @@ private:
// Range Beacon Sensor Fusion // Range Beacon Sensor Fusion
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer class BeaconFusion {
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon public:
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec) EKF_obs_buffer_t<rng_bcn_elements> storedRange; // Beacon range buffer
ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements rng_bcn_elements dataDelayed; // Range beacon data at the fusion time horizon
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check uint32_t lastPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
ftype varInnovRngBcn; // range beacon observation innovation variance (m^2) ftype testRatio; // Innovation test ratio for range beacon measurements
ftype innovRngBcn; // range beacon observation innovation (m) bool health; // boolean true if range beacon measurements have passed innovation consistency check
uint32_t lastTimeRngBcn_ms[4]; // last time we received a range beacon measurement (msec) ftype varInnov; // range beacon observation innovation variance (m^2)
bool rngBcnDataToFuse; // true when there is new range beacon data to fuse ftype innov; // range beacon observation innovation (m)
Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED) uint32_t lastTime_ms[4]; // last time we received a range beacon measurement (msec)
ftype beaconVehiclePosErr; // estimated position error from the beacon system (m) bool dataToFuse; // true when there is new range beacon data to fuse
uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec) Vector3F vehiclePosNED; // NED position estimate from the beacon system (NED)
bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter ftype vehiclePosErr; // estimated position error from the beacon system (m)
uint8_t lastRngBcnChecked; // index of the last range beacon checked for data uint32_t last3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter bool goodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter ( uint8_t lastChecked; // index of the last range beacon checked for data
bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter bool alignmentStarted; // True when the initial position alignment using range measurements has started
Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter bool alignmentCompleted; // True when the initial position alignment using range measurements has finished
uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter uint8_t lastIndex; // Range beacon index last read - used during initialisation of the 3-state filter
ftype rngSum; // Sum of range measurements (m) - 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 N_beacons; // Number of range beacons in use uint8_t numMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
ftype maxBcnPosD; // maximum position of all beacons in the down direction (m) ftype sum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
ftype minBcnPosD; // minimum position of all beacons in the down direction (m) uint8_t N; // Number of range beacons in use
bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used 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 posDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m) ftype posOffsetMaxVar; // Variance of the PosDownOffsetMax state (m)
ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh 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 posDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m) ftype posOffsetMinVar; // Variance of the PosDownOffsetMin state (m)
ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in PosOffsetLow
Vector3F bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m) Vector3F posOffsetNED; // NED position of the beacon origin in earth frame (m)
bool bcnOriginEstInit; // True when the beacon origin has been initialised bool originEstInit; // True when the beacon origin has been initialised
// Range Beacon Fusion Debug Reporting // Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported uint8_t fuseDataReportIndex;// index of range beacon fusion data last reported
struct rngBcnFusionReport_t { struct FusionReport {
ftype rng; // measured range to beacon (m) ftype rng; // measured range to beacon (m)
ftype innov; // range innovation (m) ftype innov; // range innovation (m)
ftype innovVar; // innovation variance (m^2) ftype innovVar; // innovation variance (m^2)
ftype testRatio; // innovation consistency test ratio ftype testRatio; // innovation consistency test ratio
Vector3F beaconPosNED; // beacon NED position Vector3F beaconPosNED; // beacon NED position
} *rngBcnFusionReport; } *fusionReport;
} rngBcn;
#endif // if EK3_FEATURE_BEACON_FUSION #endif // if EK3_FEATURE_BEACON_FUSION
#if EK3_FEATURE_DRAG_FUSION #if EK3_FEATURE_DRAG_FUSION