mirror of https://github.com/ArduPilot/ardupilot
1116 lines
51 KiB
C++
1116 lines
51 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include "AP_NavEKF2.h"
|
|
#include "AP_NavEKF2_core.h"
|
|
#include <AP_DAL/AP_DAL.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/********************************************************
|
|
* RESET FUNCTIONS *
|
|
********************************************************/
|
|
|
|
// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
|
|
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
|
|
void NavEKF2_core::ResetVelocity(void)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
velResetNE.x = stateStruct.velocity.x;
|
|
velResetNE.y = stateStruct.velocity.y;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,3,4);
|
|
zeroCols(P,3,4);
|
|
|
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
|
stateStruct.velocity.zero();
|
|
// set the variances using the measurement noise parameter
|
|
P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);
|
|
} else {
|
|
// reset horizontal velocity states to the GPS velocity if available
|
|
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
|
// correct for antenna position
|
|
gps_elements gps_corrected = gpsDataNew;
|
|
CorrectGPSForAntennaOffset(gps_corrected);
|
|
stateStruct.velocity.x = gps_corrected.vel.x;
|
|
stateStruct.velocity.y = gps_corrected.vel.y;
|
|
// set the variances using the reported GPS speed accuracy
|
|
P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
|
} else if (imuSampleTime_ms - extNavVelMeasTime_ms < 250) {
|
|
// use external nav data as the 2nd preference
|
|
stateStruct.velocity = extNavVelDelayed.vel;
|
|
P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
|
|
} else {
|
|
stateStruct.velocity.x = 0.0f;
|
|
stateStruct.velocity.y = 0.0f;
|
|
// set the variances using the likely speed range
|
|
P[4][4] = P[3][3] = sq(25.0f);
|
|
}
|
|
// clear the timeout flags and counters
|
|
velTimeout = false;
|
|
lastVelPassTime_ms = imuSampleTime_ms;
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].velocity.x = stateStruct.velocity.x;
|
|
storedOutput[i].velocity.y = stateStruct.velocity.y;
|
|
}
|
|
outputDataNew.velocity.x = stateStruct.velocity.x;
|
|
outputDataNew.velocity.y = stateStruct.velocity.y;
|
|
outputDataDelayed.velocity.x = stateStruct.velocity.x;
|
|
outputDataDelayed.velocity.y = stateStruct.velocity.y;
|
|
|
|
// Calculate the position jump due to the reset
|
|
velResetNE.x = stateStruct.velocity.x - velResetNE.x;
|
|
velResetNE.y = stateStruct.velocity.y - velResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastVelReset_ms = imuSampleTime_ms;
|
|
|
|
|
|
}
|
|
|
|
// resets position states to last GPS measurement or to zero if in constant position mode
|
|
void NavEKF2_core::ResetPosition(void)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetNE.x = stateStruct.position.x;
|
|
posResetNE.y = stateStruct.position.y;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,6,7);
|
|
zeroCols(P,6,7);
|
|
|
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
|
// reset all position state history to the last known position
|
|
stateStruct.position.x = lastKnownPositionNE.x;
|
|
stateStruct.position.y = lastKnownPositionNE.y;
|
|
// set the variances using the position measurement noise parameter
|
|
P[6][6] = P[7][7] = sq(frontend->_gpsHorizPosNoise);
|
|
} else {
|
|
// Use GPS data as first preference if fresh data is available
|
|
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
|
// correct for antenna position
|
|
gps_elements gps_corrected = gpsDataNew;
|
|
CorrectGPSForAntennaOffset(gps_corrected);
|
|
// record the ID of the GPS for the data we are using for the reset
|
|
last_gps_idx = gps_corrected.sensor_idx;
|
|
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
|
|
stateStruct.position.x = gps_corrected.pos.x + 0.001f*gps_corrected.vel.x*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
|
|
stateStruct.position.y = gps_corrected.pos.y + 0.001f*gps_corrected.vel.y*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
|
|
// set the variances using the position measurement noise parameter
|
|
P[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
|
|
// clear the timeout flags and counters
|
|
posTimeout = false;
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
} else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) {
|
|
// use the range beacon data as a second preference
|
|
stateStruct.position.x = receiverPos.x;
|
|
stateStruct.position.y = receiverPos.y;
|
|
// set the variances from the beacon alignment filter
|
|
P[6][6] = receiverPosCov[0][0];
|
|
P[7][7] = receiverPosCov[1][1];
|
|
// clear the timeout flags and counters
|
|
rngBcnTimeout = false;
|
|
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
|
} else if (imuSampleTime_ms - extNavDataDelayed.time_ms < 250) {
|
|
// use external nav data as the third preference
|
|
ext_nav_elements extNavCorrected = extNavDataDelayed;
|
|
CorrectExtNavForSensorOffset(extNavCorrected.pos);
|
|
stateStruct.position.x = extNavCorrected.pos.x;
|
|
stateStruct.position.y = extNavCorrected.pos.y;
|
|
// set the variances from the external nav filter
|
|
P[7][7] = P[6][6] = sq(extNavCorrected.posErr);
|
|
}
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.x = stateStruct.position.x;
|
|
storedOutput[i].position.y = stateStruct.position.y;
|
|
}
|
|
outputDataNew.position.x = stateStruct.position.x;
|
|
outputDataNew.position.y = stateStruct.position.y;
|
|
outputDataDelayed.position.x = stateStruct.position.x;
|
|
outputDataDelayed.position.y = stateStruct.position.y;
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
|
posResetNE.y = stateStruct.position.y - posResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastPosReset_ms = imuSampleTime_ms;
|
|
|
|
}
|
|
|
|
// reset the stateStruct's NE position to the specified position
|
|
// posResetNE is updated to hold the change in position
|
|
// storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
|
|
// lastPosReset_ms is updated with the time of the reset
|
|
void NavEKF2_core::ResetPositionNE(ftype posN, ftype posE)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
const Vector3F posOrig = stateStruct.position;
|
|
|
|
// Set the position states to the new position
|
|
stateStruct.position.x = posN;
|
|
stateStruct.position.y = posE;
|
|
|
|
// Calculate the position offset due to the reset
|
|
posResetNE.x = stateStruct.position.x - posOrig.x;
|
|
posResetNE.y = stateStruct.position.y - posOrig.y;
|
|
|
|
// Add the offset to the output observer states
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.x += posResetNE.x;
|
|
storedOutput[i].position.y += posResetNE.y;
|
|
}
|
|
outputDataNew.position.x += posResetNE.x;
|
|
outputDataNew.position.y += posResetNE.y;
|
|
outputDataDelayed.position.x += posResetNE.x;
|
|
outputDataDelayed.position.y += posResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastPosReset_ms = imuSampleTime_ms;
|
|
}
|
|
|
|
// reset the vertical position state using the last height measurement
|
|
void NavEKF2_core::ResetHeight(void)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetD = stateStruct.position.z;
|
|
|
|
// write to the state vector
|
|
stateStruct.position.z = -hgtMea;
|
|
outputDataNew.position.z = stateStruct.position.z;
|
|
outputDataDelayed.position.z = stateStruct.position.z;
|
|
|
|
// reset the terrain state height
|
|
if (onGround) {
|
|
// assume vehicle is sitting on the ground
|
|
terrainState = stateStruct.position.z + rngOnGnd;
|
|
} else {
|
|
// can make no assumption other than vehicle is not below ground level
|
|
terrainState = MAX(stateStruct.position.z + rngOnGnd , terrainState);
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.z = stateStruct.position.z;
|
|
}
|
|
vertCompFiltState.pos = stateStruct.position.z;
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetD = stateStruct.position.z - posResetD;
|
|
|
|
// store the time of the reset
|
|
lastPosResetD_ms = imuSampleTime_ms;
|
|
|
|
// clear the timeout flags and counters
|
|
hgtTimeout = false;
|
|
lastHgtPassTime_ms = imuSampleTime_ms;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,8,8);
|
|
zeroCols(P,8,8);
|
|
|
|
// set the variances to the measurement variance
|
|
P[8][8] = posDownObsNoise;
|
|
|
|
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
|
|
// Check that GPS vertical velocity data is available and can be used
|
|
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 &&
|
|
dal.gps().have_vertical_velocity()) {
|
|
stateStruct.velocity.z = gpsDataNew.vel.z;
|
|
} else if (inFlight && useExtNavVel) {
|
|
stateStruct.velocity.z = extNavVelNew.vel.z;
|
|
} else if (onGround) {
|
|
stateStruct.velocity.z = 0.0f;
|
|
}
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].velocity.z = stateStruct.velocity.z;
|
|
}
|
|
outputDataNew.velocity.z = stateStruct.velocity.z;
|
|
outputDataDelayed.velocity.z = stateStruct.velocity.z;
|
|
vertCompFiltState.vel = outputDataNew.velocity.z;
|
|
|
|
// reset the corresponding covariances
|
|
zeroRows(P,5,5);
|
|
zeroCols(P,5,5);
|
|
|
|
// set the variances to the measurement variance
|
|
if (useExtNavVel) {
|
|
P[5][5] = sq(extNavVelNew.err);
|
|
} else {
|
|
P[5][5] = sq(frontend->_gpsVertVelNoise);
|
|
}
|
|
}
|
|
|
|
// reset the stateStruct's D position
|
|
// posResetD is updated to hold the change in position
|
|
// storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
|
|
// lastPosResetD_ms is updated with the time of the reset
|
|
void NavEKF2_core::ResetPositionD(ftype posD)
|
|
{
|
|
// Store the position before the reset so that we can record the reset delta
|
|
const ftype posDOrig = stateStruct.position.z;
|
|
|
|
// write to the state vector
|
|
stateStruct.position.z = posD;
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetD = stateStruct.position.z - posDOrig;
|
|
|
|
// Add the offset to the output observer states
|
|
outputDataNew.position.z += posResetD;
|
|
vertCompFiltState.pos = outputDataNew.position.z;
|
|
outputDataDelayed.position.z += posResetD;
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.z += posResetD;
|
|
}
|
|
|
|
// store the time of the reset
|
|
lastPosResetD_ms = imuSampleTime_ms;
|
|
}
|
|
|
|
// Zero the EKF height datum
|
|
// Return true if the height datum reset has been performed
|
|
bool NavEKF2_core::resetHeightDatum(void)
|
|
{
|
|
if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
|
|
// only allow resets when on the ground.
|
|
// If using using rangefinder for height then never perform a
|
|
// reset of the height datum
|
|
return false;
|
|
}
|
|
// record the old height estimate
|
|
ftype oldHgt = -stateStruct.position.z;
|
|
// reset the barometer so that it reads zero at the current height
|
|
dal.baro().update_calibration();
|
|
// reset the height state
|
|
stateStruct.position.z = 0.0f;
|
|
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
|
|
|
|
if (validOrigin) {
|
|
if (!gpsGoodToAlign) {
|
|
// if we don't have GPS lock then we shouldn't be doing a
|
|
// resetHeightDatum, but if we do then the best option is
|
|
// to maintain the old error
|
|
EKF_origin.alt += (int32_t)(100.0f * oldHgt);
|
|
} else {
|
|
// if we have a good GPS lock then reset to the GPS
|
|
// altitude. This ensures the reported AMSL alt from
|
|
// getLLH() is equal to GPS altitude, while also ensuring
|
|
// that the relative alt is zero
|
|
EKF_origin.alt = dal.gps().location().alt;
|
|
}
|
|
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
|
|
}
|
|
|
|
// set the terrain state to zero (on ground). The adjustment for
|
|
// frame height will get added in the later constraints
|
|
terrainState = 0;
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
correct GPS data for position offset of antenna phase centre relative to the IMU
|
|
*/
|
|
void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
|
|
{
|
|
const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx).toftype() - accelPosOffset;
|
|
if (posOffsetBody.is_zero()) {
|
|
return;
|
|
}
|
|
|
|
// Don't fuse velocity data if GPS doesn't support it
|
|
if (fuseVelData) {
|
|
// TODO use a filtered angular rate with a group delay that matches the GPS delay
|
|
Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
|
|
Vector3F velOffsetBody = angRate % posOffsetBody;
|
|
Vector3F velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
|
|
gps_data.vel.x -= velOffsetEarth.x;
|
|
gps_data.vel.y -= velOffsetEarth.y;
|
|
gps_data.vel.z -= velOffsetEarth.z;
|
|
}
|
|
|
|
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
|
gps_data.pos.x -= posOffsetEarth.x;
|
|
gps_data.pos.y -= posOffsetEarth.y;
|
|
gps_data.hgt += posOffsetEarth.z;
|
|
}
|
|
|
|
// correct external navigation earth-frame position using sensor body-frame offset
|
|
void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3F &ext_position) const
|
|
{
|
|
#if HAL_VISUALODOM_ENABLED
|
|
const auto *visual_odom = dal.visualodom();
|
|
if (visual_odom == nullptr) {
|
|
return;
|
|
}
|
|
const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
|
|
if (posOffsetBody.is_zero()) {
|
|
return;
|
|
}
|
|
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
|
ext_position.x -= posOffsetEarth.x;
|
|
ext_position.y -= posOffsetEarth.y;
|
|
ext_position.z -= posOffsetEarth.z;
|
|
#endif
|
|
}
|
|
|
|
// correct external navigation earth-frame velocity using sensor body-frame offset
|
|
void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const
|
|
{
|
|
#if HAL_VISUALODOM_ENABLED
|
|
const auto *visual_odom = dal.visualodom();
|
|
if (visual_odom == nullptr) {
|
|
return;
|
|
}
|
|
const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
|
|
if (posOffsetBody.is_zero()) {
|
|
return;
|
|
}
|
|
// TODO use a filtered angular rate with a group delay that matches the sensor delay
|
|
const Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
|
|
ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
|
|
#endif
|
|
}
|
|
|
|
/********************************************************
|
|
* FUSE MEASURED_DATA *
|
|
********************************************************/
|
|
// select fusion of velocity, position and height measurements
|
|
void NavEKF2_core::SelectVelPosFusion()
|
|
{
|
|
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
|
|
// If so, don't fuse measurements on this time step to reduce frame over-runs
|
|
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
|
|
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
|
|
posVelFusionDelayed = true;
|
|
return;
|
|
} else {
|
|
posVelFusionDelayed = false;
|
|
}
|
|
|
|
// Check for data at the fusion time horizon
|
|
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
|
|
extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
|
|
if (extNavVelToFuse) {
|
|
CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
|
|
}
|
|
|
|
// read GPS data from the sensor and check for new data in the buffer
|
|
readGpsData();
|
|
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
|
|
|
|
// Determine if we need to fuse position and velocity data on this time step
|
|
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
|
// set fusion request flags
|
|
if (frontend->_fusionModeGPS <= 1) {
|
|
fuseVelData = true;
|
|
} else {
|
|
fuseVelData = false;
|
|
}
|
|
fusePosData = true;
|
|
extNavUsedForPos = false;
|
|
|
|
// correct for antenna position
|
|
CorrectGPSForAntennaOffset(gpsDataDelayed);
|
|
|
|
// copy corrected GPS data to observation vector
|
|
if (fuseVelData) {
|
|
velPosObs[0] = gpsDataDelayed.vel.x;
|
|
velPosObs[1] = gpsDataDelayed.vel.y;
|
|
velPosObs[2] = gpsDataDelayed.vel.z;
|
|
}
|
|
velPosObs[3] = gpsDataDelayed.pos.x;
|
|
velPosObs[4] = gpsDataDelayed.pos.y;
|
|
|
|
} else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
|
// This is a special case that uses and external nav system for position
|
|
extNavUsedForPos = true;
|
|
activeHgtSource = HGT_SOURCE_EXTNAV;
|
|
fuseVelData = false;
|
|
fuseHgtData = true;
|
|
fusePosData = true;
|
|
|
|
// correct for external navigation sensor position
|
|
CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
|
|
|
|
velPosObs[3] = extNavDataDelayed.pos.x;
|
|
velPosObs[4] = extNavDataDelayed.pos.y;
|
|
velPosObs[5] = extNavDataDelayed.pos.z;
|
|
|
|
// if compass is disabled, also use it for yaw
|
|
if (!use_compass()) {
|
|
extNavUsedForYaw = true;
|
|
if (!yawAlignComplete) {
|
|
extNavYawResetRequest = true;
|
|
magYawResetRequest = false;
|
|
gpsYawResetRequest = false;
|
|
controlMagYawReset();
|
|
finalInflightYawInit = true;
|
|
} else {
|
|
fuseEulerYaw();
|
|
}
|
|
} else {
|
|
extNavUsedForYaw = false;
|
|
}
|
|
|
|
} else {
|
|
fuseVelData = false;
|
|
fusePosData = false;
|
|
}
|
|
|
|
if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) {
|
|
fuseVelData = true;
|
|
velPosObs[0] = extNavVelDelayed.vel.x;
|
|
velPosObs[1] = extNavVelDelayed.vel.y;
|
|
velPosObs[2] = extNavVelDelayed.vel.z;
|
|
}
|
|
|
|
// we have GPS data to fuse and a request to align the yaw using the GPS course
|
|
if (gpsYawResetRequest) {
|
|
realignYawGPS();
|
|
}
|
|
|
|
// Select height data to be fused from the available baro, range finder and GPS sources
|
|
|
|
selectHeightForFusion();
|
|
|
|
// if we are using GPS, check for a change in receiver and reset position and height
|
|
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) {
|
|
// record the ID of the GPS that we are using for the reset
|
|
last_gps_idx = gpsDataDelayed.sensor_idx;
|
|
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetNE.x = stateStruct.position.x;
|
|
posResetNE.y = stateStruct.position.y;
|
|
|
|
// Set the position states to the position from the new GPS
|
|
stateStruct.position.x = gpsDataDelayed.pos.x;
|
|
stateStruct.position.y = gpsDataDelayed.pos.y;
|
|
|
|
// Calculate the position offset due to the reset
|
|
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
|
posResetNE.y = stateStruct.position.y - posResetNE.y;
|
|
|
|
// Add the offset to the output observer states
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.x += posResetNE.x;
|
|
storedOutput[i].position.y += posResetNE.y;
|
|
}
|
|
outputDataNew.position.x += posResetNE.x;
|
|
outputDataNew.position.y += posResetNE.y;
|
|
outputDataDelayed.position.x += posResetNE.x;
|
|
outputDataDelayed.position.y += posResetNE.y;
|
|
|
|
// store the time of the reset
|
|
lastPosReset_ms = imuSampleTime_ms;
|
|
|
|
// If we are also using GPS as the height reference, reset the height
|
|
if (activeHgtSource == HGT_SOURCE_GPS) {
|
|
// Store the position before the reset so that we can record the reset delta
|
|
posResetD = stateStruct.position.z;
|
|
|
|
// write to the state vector
|
|
stateStruct.position.z = -hgtMea;
|
|
|
|
// Calculate the position jump due to the reset
|
|
posResetD = stateStruct.position.z - posResetD;
|
|
|
|
// Add the offset to the output observer states
|
|
outputDataNew.position.z += posResetD;
|
|
vertCompFiltState.pos = outputDataNew.position.z;
|
|
outputDataDelayed.position.z += posResetD;
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.z += posResetD;
|
|
}
|
|
|
|
// store the time of the reset
|
|
lastPosResetD_ms = imuSampleTime_ms;
|
|
}
|
|
}
|
|
|
|
// check for external nav position reset
|
|
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3) && extNavDataDelayed.posReset) {
|
|
ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y);
|
|
if (activeHgtSource == HGT_SOURCE_EXTNAV) {
|
|
ResetPositionD(-hgtMea);
|
|
}
|
|
}
|
|
|
|
// If we are operating without any aiding, fuse in the last known position
|
|
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
|
|
// Do this to coincide with the height fusion
|
|
if (fuseHgtData && PV_AidingMode == AID_NONE) {
|
|
velPosObs[3] = lastKnownPositionNE.x;
|
|
velPosObs[4] = lastKnownPositionNE.y;
|
|
fusePosData = true;
|
|
fuseVelData = false;
|
|
}
|
|
|
|
// perform fusion
|
|
if (fuseVelData || fusePosData || fuseHgtData) {
|
|
FuseVelPosNED();
|
|
// clear the flags to prevent repeated fusion of the same data
|
|
fuseVelData = false;
|
|
fuseHgtData = false;
|
|
fusePosData = false;
|
|
}
|
|
}
|
|
|
|
// fuse selected position, velocity and height measurements
|
|
void NavEKF2_core::FuseVelPosNED()
|
|
{
|
|
// health is set bad until test passed
|
|
bool velHealth = false; // boolean true if velocity measurements have passed innovation consistency check
|
|
bool posHealth = false; // boolean true if position measurements have passed innovation consistency check
|
|
bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
|
|
|
|
// declare variables used to check measurement errors
|
|
Vector3F velInnov;
|
|
|
|
// declare variables used to control access to arrays
|
|
bool fuseData[6] = {false,false,false,false,false,false};
|
|
uint8_t stateIndex;
|
|
uint8_t obsIndex;
|
|
|
|
// declare variables used by state and covariance update calculations
|
|
Vector6 R_OBS; // Measurement variances used for fusion
|
|
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
|
|
ftype SK;
|
|
|
|
// perform sequential fusion of GPS measurements. This assumes that the
|
|
// errors in the different velocity and position components are
|
|
// uncorrelated which is not true, however in the absence of covariance
|
|
// data from the GPS receiver it is the only assumption we can make
|
|
// so we might as well take advantage of the computational efficiencies
|
|
// associated with sequential fusion
|
|
if (fuseVelData || fusePosData || fuseHgtData) {
|
|
|
|
// calculate additional error in GPS position caused by manoeuvring
|
|
ftype posErr = frontend->gpsPosVarAccScale * accNavMag;
|
|
|
|
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
|
// Use different errors if operating without external aiding using an assumed position or velocity of zero
|
|
if (PV_AidingMode == AID_NONE) {
|
|
if (tiltAlignComplete && motorsArmed) {
|
|
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
|
|
R_OBS[0] = sq(constrain_ftype(frontend->_noaidHorizNoise, 0.5f, 50.0f));
|
|
} else {
|
|
// Use a smaller value to give faster initial alignment
|
|
R_OBS[0] = sq(0.5f);
|
|
}
|
|
R_OBS[1] = R_OBS[0];
|
|
R_OBS[2] = R_OBS[0];
|
|
R_OBS[3] = R_OBS[0];
|
|
R_OBS[4] = R_OBS[0];
|
|
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
|
} else {
|
|
if (gpsSpdAccuracy > 0.0f) {
|
|
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
|
|
R_OBS[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
|
R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
|
} else if (extNavVelToFuse) {
|
|
R_OBS[2] = R_OBS[0] = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f));
|
|
} else {
|
|
// calculate additional error in GPS velocity caused by manoeuvring
|
|
R_OBS[0] = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
|
R_OBS[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
|
|
}
|
|
R_OBS[1] = R_OBS[0];
|
|
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
|
|
if (gpsPosAccuracy > 0.0f) {
|
|
R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
|
|
} else if (extNavUsedForPos) {
|
|
R_OBS[3] = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
|
|
} else {
|
|
R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
|
}
|
|
R_OBS[4] = R_OBS[3];
|
|
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
|
|
// For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS perfomrance
|
|
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
|
ftype obs_data_chk;
|
|
if (extNavVelToFuse) {
|
|
obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
|
|
} else {
|
|
obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
|
}
|
|
R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
|
|
}
|
|
R_OBS[5] = posDownObsNoise;
|
|
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
|
|
|
// if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
|
|
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
|
|
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
|
|
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
|
|
// calculate innovations for height and vertical GPS vel measurements
|
|
ftype hgtErr = stateStruct.position.z - velPosObs[5];
|
|
ftype velDErr = stateStruct.velocity.z - velPosObs[2];
|
|
// check if they are the same sign and both more than 3-sigma out of bounds
|
|
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
|
|
badIMUdata = true;
|
|
} else {
|
|
badIMUdata = false;
|
|
}
|
|
}
|
|
|
|
// calculate innovations and check GPS data validity using an innovation consistency check
|
|
// test position measurements
|
|
if (fusePosData) {
|
|
// test horizontal position measurements
|
|
innovVelPos[3] = stateStruct.position.x - velPosObs[3];
|
|
innovVelPos[4] = stateStruct.position.y - velPosObs[4];
|
|
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
|
|
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
|
|
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
|
ftype maxPosInnov2 = sq(MAX(0.01f * (ftype)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
|
|
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
|
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
|
// use position data if healthy or timed out
|
|
if (PV_AidingMode == AID_NONE) {
|
|
posHealth = true;
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
} else if (posHealth || posTimeout) {
|
|
posHealth = true;
|
|
lastPosPassTime_ms = imuSampleTime_ms;
|
|
// if timed out or outside the specified uncertainty radius, reset to the GPS
|
|
if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
|
|
// reset the position to the current GPS position
|
|
ResetPosition();
|
|
// reset the velocity to the GPS velocity
|
|
ResetVelocity();
|
|
// don't fuse GPS data on this time step
|
|
fusePosData = false;
|
|
fuseVelData = false;
|
|
// Reset the position variances and corresponding covariances to a value that will pass the checks
|
|
zeroRows(P,6,7);
|
|
zeroCols(P,6,7);
|
|
P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
|
|
P[7][7] = P[6][6];
|
|
// Reset the normalised innovation to avoid failing the bad fusion tests
|
|
posTestRatio = 0.0f;
|
|
velTestRatio = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
// test velocity measurements
|
|
if (fuseVelData) {
|
|
// test velocity measurements
|
|
uint8_t imax = 2;
|
|
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
|
|
if (!useExtNavVel && (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE ||
|
|
!dal.gps().have_vertical_velocity())) {
|
|
imax = 1;
|
|
}
|
|
ftype innovVelSumSq = 0; // sum of squares of velocity innovations
|
|
ftype varVelSum = 0; // sum of velocity innovation variances
|
|
for (uint8_t i = 0; i<=imax; i++) {
|
|
// velocity states start at index 3
|
|
stateIndex = i + 3;
|
|
// calculate innovations using blended and single IMU predicted states
|
|
velInnov[i] = stateStruct.velocity[i] - velPosObs[i]; // blended
|
|
// calculate innovation variance
|
|
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
|
|
// sum the innovation and innovation variances
|
|
innovVelSumSq += sq(velInnov[i]);
|
|
varVelSum += varInnovVelPos[i];
|
|
}
|
|
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
|
// calculate the test ratio
|
|
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (ftype)frontend->_gpsVelInnovGate, 1.0f)));
|
|
// fail if the ratio is greater than 1
|
|
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
|
// use velocity data if healthy, timed out, or in constant position mode
|
|
if (velHealth || velTimeout) {
|
|
velHealth = true;
|
|
// restart the timeout count
|
|
lastVelPassTime_ms = imuSampleTime_ms;
|
|
// If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
|
|
if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
|
|
// reset the velocity to the GPS velocity
|
|
ResetVelocity();
|
|
// don't fuse GPS velocity data on this time step
|
|
fuseVelData = false;
|
|
// Reset the normalised innovation to avoid failing the bad fusion tests
|
|
velTestRatio = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
// test height measurements
|
|
if (fuseHgtData) {
|
|
// calculate height innovations
|
|
innovVelPos[5] = stateStruct.position.z - velPosObs[5];
|
|
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
|
|
// calculate the innovation consistency test ratio
|
|
hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (ftype)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
|
|
|
|
// when on ground we accept a larger test ratio to allow
|
|
// the filter to handle large switch on IMU bias errors
|
|
// without rejecting the height sensor
|
|
const ftype maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0;
|
|
|
|
// fail if the ratio is > maxTestRatio, but don't fail if bad IMU data
|
|
hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata;
|
|
|
|
// Fuse height data if healthy or timed out or in constant position mode
|
|
if (hgtHealth || hgtTimeout) {
|
|
// Calculate a filtered value to be used by pre-flight health checks
|
|
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
|
|
if (onGround) {
|
|
ftype dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
|
|
const ftype hgtInnovFiltTC = 2.0f;
|
|
ftype alpha = constrain_ftype(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
|
|
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
|
|
} else {
|
|
hgtInnovFiltState = 0.0f;
|
|
}
|
|
|
|
// if timed out, reset the height
|
|
if (hgtTimeout) {
|
|
ResetHeight();
|
|
}
|
|
|
|
// If we have got this far then declare the height data as healthy and reset the timeout counter
|
|
hgtHealth = true;
|
|
lastHgtPassTime_ms = imuSampleTime_ms;
|
|
}
|
|
}
|
|
|
|
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
|
|
if (fuseVelData && velHealth) {
|
|
fuseData[0] = true;
|
|
fuseData[1] = true;
|
|
if (useGpsVertVel || useExtNavVel) {
|
|
fuseData[2] = true;
|
|
}
|
|
tiltErrVec.zero();
|
|
}
|
|
if (fusePosData && posHealth) {
|
|
fuseData[3] = true;
|
|
fuseData[4] = true;
|
|
tiltErrVec.zero();
|
|
}
|
|
if (fuseHgtData && hgtHealth) {
|
|
fuseData[5] = true;
|
|
}
|
|
|
|
// fuse measurements sequentially
|
|
for (obsIndex=0; obsIndex<=5; obsIndex++) {
|
|
if (fuseData[obsIndex]) {
|
|
stateIndex = 3 + obsIndex;
|
|
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
|
|
// adjust scaling on GPS measurement noise variances if not enough satellites
|
|
if (obsIndex <= 2)
|
|
{
|
|
innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex];
|
|
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
|
}
|
|
else if (obsIndex == 3 || obsIndex == 4) {
|
|
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
|
|
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
|
} else if (obsIndex == 5) {
|
|
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
|
|
const ftype gndMaxBaroErr = 4.0f;
|
|
const ftype gndBaroInnovFloor = -0.5f;
|
|
|
|
if(dal.get_touchdown_expected() && activeHgtSource == HGT_SOURCE_BARO) {
|
|
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
|
|
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
|
|
// this function looks like this:
|
|
// |/
|
|
//---------|---------
|
|
// ____/|
|
|
// / |
|
|
// / |
|
|
innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
|
|
}
|
|
}
|
|
|
|
// calculate the Kalman gain and calculate innovation variances
|
|
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
|
|
SK = 1.0f/varInnovVelPos[obsIndex];
|
|
for (uint8_t i= 0; i<=15; i++) {
|
|
Kfusion[i] = P[i][stateIndex]*SK;
|
|
}
|
|
|
|
// inhibit magnetic field state estimation by setting Kalman gains to zero
|
|
if (!inhibitMagStates) {
|
|
for (uint8_t i = 16; i<=21; i++) {
|
|
Kfusion[i] = P[i][stateIndex]*SK;
|
|
}
|
|
} else {
|
|
for (uint8_t i = 16; i<=21; i++) {
|
|
Kfusion[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
// inhibit wind state estimation by setting Kalman gains to zero
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = P[22][stateIndex]*SK;
|
|
Kfusion[23] = P[23][stateIndex]*SK;
|
|
} else {
|
|
Kfusion[22] = 0.0f;
|
|
Kfusion[23] = 0.0f;
|
|
}
|
|
|
|
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
|
|
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++)
|
|
{
|
|
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
|
|
}
|
|
}
|
|
// Check that we are not going to drive any variances negative and skip the update if so
|
|
bool healthyFusion = true;
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
if (KHP[i][i] > P[i][i]) {
|
|
healthyFusion = false;
|
|
}
|
|
}
|
|
if (healthyFusion) {
|
|
// update the covariance matrix
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
|
P[i][j] = P[i][j] - KHP[i][j];
|
|
}
|
|
}
|
|
|
|
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
|
|
ForceSymmetry();
|
|
ConstrainVariances();
|
|
|
|
// update the states
|
|
// zero the attitude error state - by definition it is assumed to be zero before each observation fusion
|
|
stateStruct.angErr.zero();
|
|
|
|
// calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
|
|
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
|
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
|
|
}
|
|
|
|
// the first 3 states represent the angular misalignment vector.
|
|
// This is used to correct the estimated quaternion
|
|
stateStruct.quat.rotate(stateStruct.angErr);
|
|
|
|
// sum the attitude error from velocity and position fusion only
|
|
// used as a metric for convergence monitoring
|
|
if (obsIndex != 5) {
|
|
tiltErrVec += stateStruct.angErr;
|
|
}
|
|
// record good fusion status
|
|
if (obsIndex == 0) {
|
|
faultStatus.bad_nvel = false;
|
|
} else if (obsIndex == 1) {
|
|
faultStatus.bad_evel = false;
|
|
} else if (obsIndex == 2) {
|
|
faultStatus.bad_dvel = false;
|
|
} else if (obsIndex == 3) {
|
|
faultStatus.bad_npos = false;
|
|
} else if (obsIndex == 4) {
|
|
faultStatus.bad_epos = false;
|
|
} else if (obsIndex == 5) {
|
|
faultStatus.bad_dpos = false;
|
|
}
|
|
} else {
|
|
// record bad fusion status
|
|
if (obsIndex == 0) {
|
|
faultStatus.bad_nvel = true;
|
|
} else if (obsIndex == 1) {
|
|
faultStatus.bad_evel = true;
|
|
} else if (obsIndex == 2) {
|
|
faultStatus.bad_dvel = true;
|
|
} else if (obsIndex == 3) {
|
|
faultStatus.bad_npos = true;
|
|
} else if (obsIndex == 4) {
|
|
faultStatus.bad_epos = true;
|
|
} else if (obsIndex == 5) {
|
|
faultStatus.bad_dpos = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/********************************************************
|
|
* MISC FUNCTIONS *
|
|
********************************************************/
|
|
|
|
// select the height measurement to be fused from the available baro, range finder and GPS sources
|
|
void NavEKF2_core::selectHeightForFusion()
|
|
{
|
|
// Read range finder data and check for new data in the buffer
|
|
// This data is used by both height and optical flow fusion processing
|
|
readRangeFinder();
|
|
rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
|
|
|
|
// correct range data for the body frame position offset relative to the IMU
|
|
// the corrected reading is the reading that would have been taken if the sensor was
|
|
// co-located with the IMU
|
|
const auto *_rng = dal.rangefinder();
|
|
if (_rng && rangeDataToFuse) {
|
|
const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
|
|
if (sensor != nullptr) {
|
|
Vector3F posOffsetBody = sensor->get_pos_offset().toftype() - accelPosOffset;
|
|
if (!posOffsetBody.is_zero()) {
|
|
Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
|
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
|
|
}
|
|
}
|
|
}
|
|
|
|
// read baro height data from the sensor and check for new data in the buffer
|
|
readBaroData();
|
|
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
|
|
|
bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
|
|
// select height source
|
|
if (extNavUsedForPos) {
|
|
// always use external navigation as the height source if using for position.
|
|
activeHgtSource = HGT_SOURCE_EXTNAV;
|
|
} else if ((frontend->_altSource == 1) && _rng && rangeFinderDataIsFresh) {
|
|
// user has specified the range finder as a primary height source
|
|
activeHgtSource = HGT_SOURCE_RNG;
|
|
} else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) {
|
|
// determine if we are above or below the height switch region
|
|
ftype rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
|
|
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
|
|
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
|
|
|
|
// If the terrain height is consistent and we are moving slowly, then it can be
|
|
// used as a height reference in combination with a range finder
|
|
// apply a hysteresis to the speed check to prevent rapid switching
|
|
ftype horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
|
|
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
|
|
ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
|
|
bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
|
|
|
|
/*
|
|
* Switch between range finder and primary height source using height above ground and speed thresholds with
|
|
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
|
|
* which cannot be assumed if the vehicle is moving horizontally.
|
|
*/
|
|
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
|
|
// cannot trust terrain or range finder so stop using range finder height
|
|
if (frontend->_altSource == 0) {
|
|
activeHgtSource = HGT_SOURCE_BARO;
|
|
} else if (frontend->_altSource == 2) {
|
|
activeHgtSource = HGT_SOURCE_GPS;
|
|
}
|
|
} else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) {
|
|
// reliable terrain and range finder so start using range finder height
|
|
activeHgtSource = HGT_SOURCE_RNG;
|
|
}
|
|
} else if (frontend->_altSource == 0) {
|
|
activeHgtSource = HGT_SOURCE_BARO;
|
|
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
|
|
activeHgtSource = HGT_SOURCE_GPS;
|
|
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
|
|
activeHgtSource = HGT_SOURCE_BCN;
|
|
}
|
|
|
|
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
|
|
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && (!rangeFinderDataIsFresh));
|
|
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
|
|
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000));
|
|
bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
|
|
if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) {
|
|
activeHgtSource = HGT_SOURCE_BARO;
|
|
}
|
|
|
|
// if there is new baro data to fuse, calculate filtered baro data required by other processes
|
|
if (baroDataToFuse) {
|
|
// calculate offset to baro data that enables us to switch to Baro height use during operation
|
|
if (activeHgtSource != HGT_SOURCE_BARO) {
|
|
calcFiltBaroOffset();
|
|
}
|
|
// filtered baro data used to provide a reference for takeoff
|
|
// it is is reset to last height measurement on disarming in performArmingChecks()
|
|
if (!dal.get_takeoff_expected()) {
|
|
const ftype gndHgtFiltTC = 0.5f;
|
|
const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3;
|
|
ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
|
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
|
}
|
|
}
|
|
|
|
// If we are not using GPS as the primary height sensor, correct EKF origin height so that
|
|
// combined local NED position height and origin height remains consistent with the GPS altitude
|
|
// This also enables the GPS height to be used as a backup height source
|
|
if (gpsDataToFuse &&
|
|
(((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
|
|
((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
|
|
) {
|
|
correctEkfOriginHeight();
|
|
}
|
|
|
|
// Select the height measurement source
|
|
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
|
|
hgtMea = -extNavDataDelayed.pos.z;
|
|
posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
|
|
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
|
|
// using range finder data
|
|
// correct for tilt using a flat earth model
|
|
if (prevTnb.c.z >= 0.7) {
|
|
// calculate height above ground
|
|
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
|
|
// correct for terrain position relative to datum
|
|
hgtMea -= terrainState;
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
velPosObs[5] = -hgtMea;
|
|
// set the observation noise
|
|
posDownObsNoise = sq(constrain_ftype(frontend->_rngNoise, 0.1f, 10.0f));
|
|
// add uncertainty created by terrain gradient and vehicle tilt
|
|
posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
|
|
} else {
|
|
// disable fusion if tilted too far
|
|
fuseHgtData = false;
|
|
}
|
|
} else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) {
|
|
// using GPS data
|
|
hgtMea = gpsDataDelayed.hgt;
|
|
// enable fusion
|
|
velPosObs[5] = -hgtMea;
|
|
fuseHgtData = true;
|
|
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
|
|
if (gpsHgtAccuracy > 0.0f) {
|
|
posDownObsNoise = sq(constrain_ftype(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
|
|
} else {
|
|
posDownObsNoise = sq(constrain_ftype(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
|
|
}
|
|
} else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
|
|
// using Baro data
|
|
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
|
|
// enable fusion
|
|
velPosObs[5] = -hgtMea;
|
|
fuseHgtData = true;
|
|
// set the observation noise
|
|
posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 0.1f, 10.0f));
|
|
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
|
if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) {
|
|
posDownObsNoise *= frontend->gndEffectBaroScaler;
|
|
}
|
|
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
|
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
|
if (motorsArmed && dal.get_takeoff_expected() && !assume_zero_sideslip()) {
|
|
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
|
|
}
|
|
} else {
|
|
fuseHgtData = false;
|
|
}
|
|
|
|
// If we haven't fused height data for a while, then declare the height data as being timed out
|
|
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
|
hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
|
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
|
|
hgtTimeout = true;
|
|
} else {
|
|
hgtTimeout = false;
|
|
}
|
|
}
|
|
|