2015-10-05 23:30:07 -03:00
/// -*- 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 )
{
2015-10-29 02:06:24 -03:00
// Store the position before the reset so that we can record the reset delta
velResetNE . x = stateStruct . velocity . x ;
velResetNE . y = stateStruct . velocity . y ;
2015-10-05 23:30:07 -03:00
if ( PV_AidingMode ! = AID_ABSOLUTE ) {
stateStruct . velocity . zero ( ) ;
} else if ( ! gpsNotAvailable ) {
2015-10-29 02:06:24 -03:00
// 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
2015-10-05 23:30:07 -03:00
}
2015-11-17 19:15:34 -04:00
for ( uint8_t i = 0 ; i < imu_buffer_length ; i + + ) {
2015-10-05 23:30:07 -03:00
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 ;
2015-10-29 02:06:24 -03:00
// 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 ;
2015-10-05 23:30:07 -03:00
}
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF2_core : : ResetPosition ( void )
{
2015-10-29 02:06:24 -03:00
// Store the position before the reset so that we can record the reset delta
posResetNE . x = stateStruct . position . x ;
posResetNE . y = stateStruct . position . y ;
2015-10-05 23:30:07 -03:00
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
2015-11-05 18:41:42 -04:00
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 ) ) ;
2015-10-05 23:30:07 -03:00
}
2015-11-17 19:15:34 -04:00
for ( uint8_t i = 0 ; i < imu_buffer_length ; i + + ) {
2015-10-05 23:30:07 -03:00
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 ;
2015-10-29 02:06:24 -03:00
// 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 ;
2015-10-05 23:30:07 -03:00
}
// reset the vertical position state using the last height measurement
void NavEKF2_core : : ResetHeight ( void )
{
// write to the state vector
2015-11-12 04:07:52 -04:00
stateStruct . position . z = - hgtMea ;
2015-10-05 23:30:07 -03:00
terrainState = stateStruct . position . z + rngOnGnd ;
2015-11-17 19:15:34 -04:00
for ( uint8_t i = 0 ; i < imu_buffer_length ; i + + ) {
2015-10-05 23:30:07 -03:00
storedOutput [ i ] . position . z = stateStruct . position . z ;
}
outputDataNew . position . z = stateStruct . position . z ;
outputDataDelayed . position . z = stateStruct . position . z ;
2015-11-12 23:54:02 -04:00
// 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 ;
}
2015-11-17 20:33:51 -04:00
for ( uint8_t i = 0 ; i < imu_buffer_length ; i + + ) {
2015-11-12 23:54:02 -04:00
storedOutput [ i ] . velocity . z = stateStruct . velocity . z ;
}
outputDataNew . velocity . z = stateStruct . velocity . z ;
outputDataDelayed . velocity . z = stateStruct . velocity . z ;
2015-10-05 23:30:07 -03:00
}
// 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
2015-11-04 21:00:57 -04:00
if ( frontend - > _altSource = = 1 ) {
2015-10-05 23:30:07 -03:00
return false ;
}
// record the old height estimate
float oldHgt = - stateStruct . position . z ;
// reset the barometer so that it reads zero at the current height
2015-11-04 21:00:57 -04:00
frontend - > _baro . update_calibration ( ) ;
2015-10-05 23:30:07 -03:00
// 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 ( )
{
2015-10-18 19:34:11 -03:00
// 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
2015-10-20 06:12:17 -03:00
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if ( magFusePerformed & & dtIMUavg < 0.005f & & ! posVelFusionDelayed ) {
posVelFusionDelayed = true ;
2015-10-18 19:34:11 -03:00
return ;
2015-10-20 06:12:17 -03:00
} else {
posVelFusionDelayed = false ;
2015-10-18 19:34:11 -03:00
}
2015-11-12 04:07:52 -04:00
// read GPS data from the sensor and check for new data in the buffer
2015-10-05 23:30:07 -03:00
readGpsData ( ) ;
2015-11-13 18:28:45 -04:00
gpsDataToFuse = storedGPS . recall ( gpsDataDelayed , imuDataDelayed . time_ms ) ;
2015-10-05 23:30:07 -03:00
// Determine if we need to fuse position and velocity data on this time step
2015-11-12 04:07:52 -04:00
if ( gpsDataToFuse & & PV_AidingMode = = AID_ABSOLUTE ) {
2015-10-05 23:30:07 -03:00
// Don't fuse velocity data if GPS doesn't support it
2015-11-04 21:00:57 -04:00
if ( frontend - > _fusionModeGPS < = 1 ) {
2015-10-05 23:30:07 -03:00
fuseVelData = true ;
} else {
fuseVelData = false ;
}
fusePosData = true ;
} else {
fuseVelData = false ;
fusePosData = false ;
}
2015-11-12 04:07:52 -04:00
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion ( ) ;
2015-10-05 23:30:07 -03:00
2016-01-19 23:07:10 -04:00
// If we are operating without any aiding, fuse in the last known position
2015-10-21 02:20:52 -03:00
// 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 ( ) ;
2016-01-19 23:07:10 -04:00
gpsDataDelayed . pos . x = lastKnownPositionNE . x ;
gpsDataDelayed . pos . y = lastKnownPositionNE . y ;
2016-01-23 17:47:32 -04:00
fusePosData = true ;
2016-01-19 23:07:10 -04:00
fuseVelData = false ;
2015-10-21 02:20:52 -03:00
}
2015-10-05 23:30:07 -03:00
// perform fusion
if ( fuseVelData | | fusePosData | | fuseHgtData ) {
FuseVelPosNED ( ) ;
2015-10-21 02:20:52 -03:00
// clear the flags to prevent repeated fusion of the same data
fuseVelData = false ;
fuseHgtData = false ;
fusePosData = false ;
2015-10-05 23:30:07 -03:00
}
}
// fuse selected position, velocity and height measurements
void NavEKF2_core : : FuseVelPosNED ( )
{
// start performance timer
2015-10-20 02:42:50 -03:00
hal . util - > perf_begin ( _perf_FuseVelPosNED ) ;
2015-10-05 23:30:07 -03:00
// 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 ;
2015-11-04 21:00:57 -04:00
if ( useAirspeed ( ) ) gpsRetryTime = frontend - > gpsRetryTimeUseTAS_ms ;
else gpsRetryTime = frontend - > gpsRetryTimeNoTAS_ms ;
2015-10-05 23:30:07 -03:00
2015-10-21 02:20:52 -03:00
// form the observation vector
2015-10-29 02:06:24 -03:00
observation [ 0 ] = gpsDataDelayed . vel . x ;
observation [ 1 ] = gpsDataDelayed . vel . y ;
2015-10-21 02:20:52 -03:00
observation [ 2 ] = gpsDataDelayed . vel . z ;
2015-10-29 02:06:24 -03:00
observation [ 3 ] = gpsDataDelayed . pos . x ;
observation [ 4 ] = gpsDataDelayed . pos . y ;
2015-11-12 04:07:52 -04:00
observation [ 5 ] = - hgtMea ;
2015-10-05 23:30:07 -03:00
// calculate additional error in GPS position caused by manoeuvring
2015-11-04 21:00:57 -04:00
posErr = frontend - > gpsPosVarAccScale * accNavMag ;
2015-10-05 23:30:07 -03:00
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
2016-01-19 23:07:10 -04:00
// Use different errors if operating without external aiding using an assumed position or velocity of zero
2016-01-09 17:20:49 -04:00
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
2016-01-19 23:07:10 -04:00
R_OBS [ 0 ] = sq ( constrain_float ( frontend - > _noaidHorizNoise , 0.5f , 50.0f ) ) ;
2016-01-09 17:20:49 -04:00
} else {
// Use a smaller value to give faster initial alignment
R_OBS [ 0 ] = sq ( 0.5f ) ;
}
2015-10-22 23:41:30 -03:00
R_OBS [ 1 ] = R_OBS [ 0 ] ;
R_OBS [ 2 ] = R_OBS [ 0 ] ;
2016-01-19 23:07:10 -04:00
R_OBS [ 3 ] = R_OBS [ 0 ] ;
R_OBS [ 4 ] = R_OBS [ 0 ] ;
2016-01-09 17:20:49 -04:00
for ( uint8_t i = 0 ; i < = 2 ; i + + ) R_OBS_DATA_CHECKS [ i ] = R_OBS [ i ] ;
2015-10-05 23:30:07 -03:00
} else {
2015-10-22 23:41:30 -03:00
if ( gpsSpdAccuracy > 0.0f ) {
// use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
2015-11-04 21:00:57 -04:00
R_OBS [ 0 ] = sq ( constrain_float ( gpsSpdAccuracy , frontend - > _gpsHorizVelNoise , 50.0f ) ) ;
R_OBS [ 2 ] = sq ( constrain_float ( gpsSpdAccuracy , frontend - > _gpsVertVelNoise , 50.0f ) ) ;
2015-10-22 23:41:30 -03:00
} else {
// calculate additional error in GPS velocity caused by manoeuvring
2015-11-04 21:00:57 -04:00
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 ) ;
2015-10-22 23:41:30 -03:00
}
R_OBS [ 1 ] = R_OBS [ 0 ] ;
2016-01-06 04:58:15 -04:00
// 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
2016-01-19 23:07:10 -04:00
R_OBS [ 3 ] = sq ( constrain_float ( frontend - > _gpsHorizPosNoise , 0.1f , 10.0f ) ) + sq ( posErr ) ;
R_OBS [ 4 ] = R_OBS [ 3 ] ;
2016-01-06 04:58:15 -04:00
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 ) ;
2015-10-05 23:30:07 -03:00
}
2016-01-09 17:20:49 -04:00
R_OBS [ 5 ] = posDownObsNoise ;
for ( uint8_t i = 3 ; i < = 5 ; i + + ) R_OBS_DATA_CHECKS [ i ] = R_OBS [ i ] ;
2015-10-05 23:30:07 -03:00
2015-11-12 04:07:52 -04:00
// if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
2015-10-05 23:30:07 -03:00
// 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.
2015-11-12 04:07:52 -04:00
if ( useGpsVertVel & & fuseVelData & & ( frontend - > _altSource ! = 2 ) ) {
2015-10-05 23:30:07 -03:00
// 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
2015-11-27 13:11:58 -04:00
float maxPosInnov2 = sq ( MAX ( 0.01f * ( float ) frontend - > _gpsPosInnovGate , 1.0f ) ) * ( varInnovVelPos [ 3 ] + varInnovVelPos [ 4 ] ) ;
2015-10-05 23:30:07 -03:00
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 ) ;
2016-01-09 17:20:49 -04:00
// use position data if healthy or timed out
2016-01-19 23:07:10 -04:00
if ( PV_AidingMode = = AID_NONE ) {
posHealth = true ;
lastPosPassTime_ms = imuSampleTime_ms ;
} else if ( posHealth | | posTimeout ) {
2015-10-05 23:30:07 -03:00
posHealth = true ;
2016-01-09 17:20:49 -04:00
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 ;
2015-10-05 23:30:07 -03:00
}
} else {
posHealth = false ;
}
}
// test velocity measurements
if ( fuseVelData ) {
// test velocity measurements
uint8_t imax = 2 ;
2015-10-22 23:41:30 -03:00
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
2015-11-04 21:00:57 -04:00
if ( frontend - > _fusionModeGPS > = 1 | | PV_AidingMode ! = AID_ABSOLUTE ) {
2015-10-05 23:30:07 -03:00
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
2015-11-27 13:11:58 -04:00
velTestRatio = innovVelSumSq / ( varVelSum * sq ( MAX ( 0.01f * ( float ) frontend - > _gpsVelInnovGate , 1.0f ) ) ) ;
2015-10-05 23:30:07 -03:00
// 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 ) ;
2015-10-29 07:46:26 -03:00
// use velocity data if healthy, timed out, or in constant position mode
2015-10-30 00:52:49 -03:00
if ( velHealth | | velTimeout ) {
2015-10-05 23:30:07 -03:00
velHealth = true ;
// restart the timeout count
lastVelPassTime_ms = imuSampleTime_ms ;
2015-10-29 07:46:26 -03:00
// 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 ;
}
2015-10-05 23:30:07 -03:00
} 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
2015-11-27 13:11:58 -04:00
hgtTestRatio = sq ( innovVelPos [ 5 ] ) / ( sq ( MAX ( 0.01f * ( float ) frontend - > _hgtInnovGate , 1.0f ) ) * varInnovVelPos [ 5 ] ) ;
2015-10-05 23:30:07 -03:00
// 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
2015-11-12 04:07:52 -04:00
if ( hgtHealth | | hgtTimeout | | ( PV_AidingMode = = AID_NONE & & onGround ) ) {
2015-11-05 21:12:08 -04:00
// 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-3 f ;
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 ;
}
2015-11-12 04:07:52 -04:00
// if timed out, reset the height
2015-10-05 23:30:07 -03:00
if ( hgtTimeout ) {
ResetHeight ( ) ;
2015-11-12 04:07:52 -04:00
hgtTimeout = false ;
2015-10-05 23:30:07 -03:00
}
2015-11-12 04:07:52 -04:00
// If we have got this far then declare the height data as healthy and reset the timeout counter
hgtHealth = true ;
lastHgtPassTime_ms = imuSampleTime_ms ;
2015-10-05 23:30:07 -03:00
}
}
// 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 ) ;
2015-11-12 04:07:52 -04:00
} else if ( obsIndex = = 5 ) {
2015-10-05 23:30:07 -03:00
innovVelPos [ obsIndex ] = stateStruct . position [ obsIndex - 3 ] - observation [ obsIndex ] ;
2015-11-12 04:07:52 -04:00
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 ) ;
2015-10-05 23:30:07 -03:00
}
}
// 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 ;
}
// 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
2015-10-20 02:42:50 -03:00
hal . util - > perf_end ( _perf_FuseVelPosNED ) ;
2015-10-05 23:30:07 -03:00
}
/********************************************************
* MISC FUNCTIONS *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2015-11-12 04:07:52 -04:00
// 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 ( ) ;
2015-11-13 18:28:45 -04:00
rangeDataToFuse = storedRange . recall ( rangeDataDelayed , imuDataDelayed . time_ms ) ;
2015-11-12 04:07:52 -04:00
// read baro height data from the sensor and check for new data in the buffer
readBaroData ( ) ;
2015-11-13 18:28:45 -04:00
baroDataToFuse = storedBaro . recall ( baroDataDelayed , imuDataDelayed . time_ms ) ;
2015-11-12 04:07:52 -04:00
// determine if we should be using a height source other than baro
2015-11-12 07:41:01 -04:00
bool usingRangeForHgt = ( frontend - > _altSource = = 1 & & imuSampleTime_ms - rngValidMeaTime_ms < 500 & & frontend - > _fusionModeGPS = = 3 ) ;
2016-02-26 19:58:38 -04:00
bool usingGpsForHgt = ( frontend - > _altSource = = 2 & & imuSampleTime_ms - lastTimeGpsReceived_ms < 500 & & validOrigin & & gpsAccuracyGood ) ;
2015-11-12 04:07:52 -04:00
// 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-3 f ;
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 ) {
2015-11-27 13:11:58 -04:00
hgtMea = MAX ( rangeDataDelayed . rng * prevTnb . c . z , rngOnGnd ) ;
2015-11-12 04:07:52 -04:00
// 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 ;
}
}
2015-10-07 15:27:09 -03:00
# endif // HAL_CPU_CLASS