mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
20923da23a
Eliminate the use of horizontal position states during non-aiding operation to make it easier to tune. Explicitly set the horizontal position associated Kalman gains to zero and the coresponding covariance entries to zero after avery fusion operation. Make the horizontal velocity observation noise used during non-aiding operation adjustable. Use a fixed value of velocity noise during initial alignment so that the flight peformance can be tuned without affecting the initial alignment.
625 lines
29 KiB
C++
625 lines
29 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
|
|
#include "AP_NavEKF2.h"
|
|
#include "AP_NavEKF2_core.h"
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
|
|
#include <stdio.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;
|
|
|
|
if (PV_AidingMode != AID_ABSOLUTE) {
|
|
stateStruct.velocity.zero();
|
|
} else if (!gpsNotAvailable) {
|
|
// reset horizontal velocity states to the GPS velocity
|
|
stateStruct.velocity.x = gpsDataNew.vel.x; // north velocity from blended accel data
|
|
stateStruct.velocity.y = gpsDataNew.vel.y; // east velocity from blended accel data
|
|
}
|
|
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;
|
|
|
|
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;
|
|
} else if (!gpsNotAvailable) {
|
|
// write to state vector and compensate for offset between last GPs measurement and the EKF time horizon
|
|
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
|
|
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
|
|
}
|
|
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 vertical position state using the last height measurement
|
|
void NavEKF2_core::ResetHeight(void)
|
|
{
|
|
// write to the state vector
|
|
stateStruct.position.z = -hgtMea;
|
|
terrainState = stateStruct.position.z + rngOnGnd;
|
|
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
|
storedOutput[i].position.z = stateStruct.position.z;
|
|
}
|
|
outputDataNew.position.z = stateStruct.position.z;
|
|
outputDataDelayed.position.z = stateStruct.position.z;
|
|
|
|
// 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) {
|
|
stateStruct.velocity.z = gpsDataNew.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;
|
|
}
|
|
|
|
// Reset the baro so that it reads zero at the current height
|
|
// Reset the EKF height to zero
|
|
// Adjust the EKf origin height so that the EKF height + origin height is the same as before
|
|
// Return true if the height datum reset has been performed
|
|
// If using a range finder for height do not reset and return false
|
|
bool NavEKF2_core::resetHeightDatum(void)
|
|
{
|
|
// if we are using a range finder for height, return false
|
|
if (frontend->_altSource == 1) {
|
|
return false;
|
|
}
|
|
// record the old height estimate
|
|
float oldHgt = -stateStruct.position.z;
|
|
// reset the barometer so that it reads zero at the current height
|
|
frontend->_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 afer the reset is the same
|
|
if (validOrigin) {
|
|
EKF_origin.alt += oldHgt*100;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/********************************************************
|
|
* 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;
|
|
}
|
|
|
|
// 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) {
|
|
// Don't fuse velocity data if GPS doesn't support it
|
|
if (frontend->_fusionModeGPS <= 1) {
|
|
fuseVelData = true;
|
|
} else {
|
|
fuseVelData = false;
|
|
}
|
|
fusePosData = true;
|
|
} else {
|
|
fuseVelData = false;
|
|
fusePosData = false;
|
|
}
|
|
|
|
// Select height data to be fused from the available baro, range finder and GPS sources
|
|
selectHeightForFusion();
|
|
|
|
// If we are operating without any aiding, fuse in the last known position and zero velocity
|
|
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
|
|
// Do this to coincide with the height fusion
|
|
if (fuseHgtData && PV_AidingMode == AID_NONE) {
|
|
gpsDataDelayed.vel.zero();
|
|
// only fuse synthetic measurements when rate of change of velocity is less than 1g to reduce attitude errors due to launch acceleration
|
|
if (accNavMag < 9.8f) {
|
|
fuseVelData = true;
|
|
} else {
|
|
fuseVelData = false;
|
|
}
|
|
fusePosData = 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()
|
|
{
|
|
// start performance timer
|
|
hal.util->perf_begin(_perf_FuseVelPosNED);
|
|
|
|
// health is set bad until test passed
|
|
velHealth = false;
|
|
posHealth = false;
|
|
hgtHealth = false;
|
|
|
|
// 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
|
|
float posErr;
|
|
Vector6 R_OBS; // Measurement variances used for fusion
|
|
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
|
|
Vector6 observation;
|
|
float SK;
|
|
|
|
// perform sequential fusion of GPS measurements. This assumes that the
|
|
// 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) {
|
|
// set the GPS data timeout depending on whether airspeed data is present
|
|
uint32_t gpsRetryTime;
|
|
if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms;
|
|
else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms;
|
|
|
|
// form the observation vector
|
|
observation[0] = gpsDataDelayed.vel.x;
|
|
observation[1] = gpsDataDelayed.vel.y;
|
|
observation[2] = gpsDataDelayed.vel.z;
|
|
observation[3] = gpsDataDelayed.pos.x;
|
|
observation[4] = gpsDataDelayed.pos.y;
|
|
observation[5] = -hgtMea;
|
|
|
|
// calculate additional error in GPS position caused by manoeuvring
|
|
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 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_float(frontend->_noaidHorizVelNoise, 0.5f, 25.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];
|
|
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 noise parameter
|
|
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
|
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
|
} else {
|
|
// calculate additional error in GPS velocity caused by manoeuvring
|
|
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
|
R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
|
|
}
|
|
R_OBS[1] = R_OBS[0];
|
|
// 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
|
|
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
|
}
|
|
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
|
R_OBS[4] = R_OBS[3];
|
|
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 independant 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
|
|
float hgtErr = stateStruct.position.z - observation[5];
|
|
float velDErr = stateStruct.velocity.z - observation[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 - observation[3];
|
|
innovVelPos[4] = stateStruct.position.y - observation[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
|
|
float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
|
|
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
|
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
|
// declare a timeout condition if we have been too long without data or not aiding
|
|
posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
|
|
// use position data if healthy or timed out
|
|
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;
|
|
}
|
|
} else {
|
|
posHealth = false;
|
|
}
|
|
}
|
|
|
|
// 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 (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
|
|
imax = 1;
|
|
}
|
|
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
|
float 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] - observation[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 * (float)frontend->_gpsVelInnovGate, 1.0f)));
|
|
// fail if the ratio is greater than 1
|
|
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
|
// declare a timeout if we have not fused velocity data for too long or not aiding
|
|
velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
|
|
// 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;
|
|
}
|
|
} else {
|
|
velHealth = false;
|
|
}
|
|
}
|
|
|
|
// test height measurements
|
|
if (fuseHgtData) {
|
|
// calculate height innovations
|
|
innovVelPos[5] = stateStruct.position.z - observation[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 * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
|
|
// fail if the ratio is > 1, but don't fail if bad IMU data
|
|
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
|
// Fuse height data if healthy or timed out or in constant position mode
|
|
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) {
|
|
// 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) {
|
|
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
|
|
const float hgtInnovFiltTC = 2.0f;
|
|
float alpha = constrain_float(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();
|
|
hgtTimeout = false;
|
|
}
|
|
|
|
// 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) {
|
|
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] - observation[obsIndex];
|
|
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
|
}
|
|
else if (obsIndex == 3 || obsIndex == 4) {
|
|
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
|
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
|
} else if (obsIndex == 5) {
|
|
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
|
const float gndMaxBaroErr = 4.0f;
|
|
const float gndBaroInnovFloor = -0.5f;
|
|
|
|
if(getTouchdownExpected()) {
|
|
// 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_float(-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;
|
|
}
|
|
|
|
// inhibit position state modification if we are not aiding
|
|
if (PV_AidingMode == AID_NONE) {
|
|
Kfusion[6] = 0.0f;
|
|
Kfusion[7] = 0.0f;
|
|
}
|
|
|
|
// zero the attitude error state - by definition it is assumed to be zero before each observaton 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
|
|
// 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;
|
|
}
|
|
|
|
// 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];
|
|
}
|
|
}
|
|
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-condiioning.
|
|
ForceSymmetry();
|
|
ConstrainVariances();
|
|
|
|
// stop performance timer
|
|
hal.util->perf_end(_perf_FuseVelPosNED);
|
|
}
|
|
|
|
/********************************************************
|
|
* 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);
|
|
|
|
// read baro height data from the sensor and check for new data in the buffer
|
|
readBaroData();
|
|
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
|
|
|
// determine if we should be using a height source other than baro
|
|
bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500 && frontend->_fusionModeGPS == 3);
|
|
bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500 && validOrigin);
|
|
|
|
// if there is new baro data to fuse, calculate filterred baro data required by other processes
|
|
if (baroDataToFuse) {
|
|
// calculate offset to baro data that enables baro to be used as a backup if we are using other height sources
|
|
if (usingRangeForHgt || usingGpsForHgt) {
|
|
calcFiltBaroOffset();
|
|
}
|
|
// filtered baro data used to provide a reference for takeoff
|
|
// it is is reset to last height measurement on disarming in performArmingChecks()
|
|
if (!getTakeoffExpected()) {
|
|
const float gndHgtFiltTC = 0.5f;
|
|
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
|
|
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
|
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
|
}
|
|
}
|
|
|
|
// Select the height measurement source
|
|
if (rangeDataToFuse && usingRangeForHgt) {
|
|
// using range finder data
|
|
// correct for tilt using a flat earth model
|
|
if (prevTnb.c.z >= 0.7) {
|
|
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
// set the observation noise
|
|
posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
|
|
} else {
|
|
// disable fusion if tilted too far
|
|
fuseHgtData = false;
|
|
}
|
|
} else if (gpsDataToFuse && usingGpsForHgt) {
|
|
// using GPS data
|
|
hgtMea = gpsDataDelayed.hgt;
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
// set the observation noise to the horizontal GPS noise plus a scaler becasue GPS vertical position is usually less accurate
|
|
// TODO use VDOP/HDOP, reported accuracy or a separate parameter
|
|
posDownObsNoise = sq(constrain_float(frontend->_gpsHorizPosNoise * 1.5f, 0.1f, 10.0f));
|
|
} else if (baroDataToFuse && !usingRangeForHgt && !usingGpsForHgt) {
|
|
// using Baro data
|
|
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
|
|
// enable fusion
|
|
fuseHgtData = true;
|
|
// set the observation noise
|
|
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
|
|
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
|
if (getTakeoffExpected() || getTouchdownExpected()) {
|
|
posDownObsNoise *= frontend->gndEffectBaroScaler;
|
|
}
|
|
} 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 && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
|
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
|
|
hgtTimeout = true;
|
|
} else {
|
|
hgtTimeout = false;
|
|
}
|
|
}
|
|
|
|
#endif // HAL_CPU_CLASS
|