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
}
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 ;
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-10-29 02:06:24 -03:00
stateStruct . position . x = gpsDataNew . pos . x + 0.001f * gpsDataNew . vel . x * ( float ( imuDataDelayed . time_ms ) - float ( lastTimeGpsReceived_ms ) ) ;
stateStruct . position . y = gpsDataNew . pos . y + 0.001f * gpsDataNew . vel . y * ( float ( imuDataDelayed . time_ms ) - float ( lastTimeGpsReceived_ms ) ) ;
2015-10-05 23:30:07 -03:00
}
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 ;
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 )
{
// read the altimeter
readHgtData ( ) ;
// write to the state vector
stateStruct . position . z = - baroDataNew . hgt ; // down position from blended accel data
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 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-10-05 23:30:07 -03:00
// check for and read new GPS data
readGpsData ( ) ;
// Determine if we need to fuse position and velocity data on this time step
2015-10-21 02:20:52 -03:00
if ( RecallGPS ( ) & & 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 ;
}
// check for and read new height data
readHgtData ( ) ;
// If we haven't received 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
2015-11-04 21:00:57 -04:00
hgtRetryTime_ms = ( useGpsVertVel & & ! velTimeout ) ? frontend - > hgtRetryTimeMode0_ms : frontend - > hgtRetryTimeMode12_ms ;
2015-10-05 23:30:07 -03:00
if ( imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime_ms ) {
hgtTimeout = true ;
}
// command fusion of height data
// wait until the EKF time horizon catches up with the measurement
if ( RecallBaro ( ) ) {
// enable fusion
fuseHgtData = true ;
}
2015-10-21 02:20:52 -03:00
// 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 ( ) ;
gpsDataDelayed . pos . x = lastKnownPositionNE . x ;
gpsDataDelayed . pos . y = lastKnownPositionNE . y ;
2015-10-22 23:41:30 -03:00
// 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 ) {
fusePosData = true ;
fuseVelData = true ;
} else {
fusePosData = false ;
fuseVelData = false ;
}
2015-10-21 02:20:52 -03:00
}
2015-10-05 23:30:07 -03:00
// perform fusion
if ( fuseVelData | | fusePosData | | fuseHgtData ) {
// ensure that the covariance prediction is up to date before fusing data
if ( ! covPredStep ) CovariancePrediction ( ) ;
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-10-05 23:30:07 -03:00
observation [ 5 ] = - baroDataDelayed . hgt ;
// 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.
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
// otherwise we scale it using manoeuvre acceleration
2015-10-22 23:41:30 -03:00
// Use different errors if flying without GPS using synthetic position and velocity data
if ( PV_AidingMode = = AID_NONE & & inFlight ) {
// Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
// This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
R_OBS [ 0 ] = sq ( 10.0f ) ;
R_OBS [ 1 ] = R_OBS [ 0 ] ;
R_OBS [ 2 ] = R_OBS [ 0 ] ;
// Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
R_OBS [ 3 ] = sq ( 25.0f ) ;
R_OBS [ 4 ] = R_OBS [ 3 ] ;
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 ] ;
2015-11-04 21:00:57 -04:00
R_OBS [ 3 ] = sq ( constrain_float ( frontend - > _gpsHorizPosNoise , 0.1f , 10.0f ) ) + sq ( posErr ) ;
2015-10-22 23:41:30 -03:00
R_OBS [ 4 ] = R_OBS [ 3 ] ;
2015-10-05 23:30:07 -03:00
}
2015-11-04 21:00:57 -04:00
R_OBS [ 5 ] = sq ( constrain_float ( frontend - > _baroAltNoise , 0.1f , 10.0f ) ) ;
2015-10-05 23:30:07 -03:00
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
if ( getTakeoffExpected ( ) | | getTouchdownExpected ( ) ) {
2015-11-04 21:00:57 -04:00
R_OBS [ 5 ] * = frontend - > gndEffectBaroScaler ;
2015-10-05 23:30:07 -03: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
2015-11-04 21:00:57 -04:00
for ( uint8_t i = 0 ; i < = 1 ; 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
for ( uint8_t i = 2 ; i < = 5 ; i + + ) R_OBS_DATA_CHECKS [ i ] = R_OBS [ i ] ;
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
// 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-04 21:00:57 -04:00
if ( useGpsVertVel & & fuseVelData & & ( imuSampleTime_ms - lastHgtReceived_ms ) < ( 2 * frontend - > hgtAvg_ms ) ) {
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-04 21:00:57 -04:00
float maxPosInnov2 = sq ( frontend - > _gpsPosInnovGate ) * ( 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 ) ;
// use position data if healthy, timed out, or in constant position mode
if ( posHealth | | posTimeout | | ( PV_AidingMode = = AID_NONE ) ) {
posHealth = true ;
// only reset the failed time and do glitch timeout checks if we are doing full aiding
if ( PV_AidingMode = = AID_ABSOLUTE ) {
lastPosPassTime_ms = imuSampleTime_ms ;
2015-10-29 02:06:24 -03:00
// if timed out or outside the specified uncertainty radius, reset to the GPS
2015-11-04 21:00:57 -04:00
if ( posTimeout | | ( ( P [ 6 ] [ 6 ] + P [ 7 ] [ 7 ] ) > sq ( float ( frontend - > _gpsGlitchRadiusMax ) ) ) ) {
2015-10-29 02:06:24 -03:00
// reset the position to the current GPS position
2015-10-05 23:30:07 -03:00
ResetPosition ( ) ;
// reset the velocity to the GPS velocity
ResetVelocity ( ) ;
2015-10-29 07:46:26 -03:00
// don't fuse GPS data on this time step
2015-10-05 23:30:07 -03:00
fusePosData = false ;
2015-10-29 07:46:26 -03:00
fuseVelData = false ;
2015-10-23 09:44:43 -03:00
// Reset the position variances and corresponding covariances to a value that will pass the checks
zeroRows ( P , 6 , 7 ) ;
zeroCols ( P , 6 , 7 ) ;
2015-11-04 21:00:57 -04:00
P [ 6 ] [ 6 ] = sq ( float ( 0.5f * frontend - > _gpsGlitchRadiusMax ) ) ;
2015-10-23 09:44:43 -03:00
P [ 7 ] [ 7 ] = P [ 6 ] [ 6 ] ;
2015-10-29 07:46:26 -03:00
// Reset the normalised innovation to avoid failing the bad fusion tests
2015-10-05 23:30:07 -03:00
posTestRatio = 0.0f ;
velTestRatio = 0.0f ;
}
}
} 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-04 21:00:57 -04:00
velTestRatio = innovVelSumSq / ( varVelSum * sq ( frontend - > _gpsVelInnovGate ) ) ;
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-04 21:00:57 -04:00
hgtTestRatio = sq ( innovVelPos [ 5 ] ) / ( sq ( frontend - > _hgtInnovGate ) * 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 ) ;
hgtTimeout = ( imuSampleTime_ms - lastHgtPassTime_ms ) > hgtRetryTime_ms ;
// Fuse height data if healthy or timed out or in constant position mode
if ( hgtHealth | | hgtTimeout | | ( PV_AidingMode = = AID_NONE ) ) {
hgtHealth = true ;
lastHgtPassTime_ms = imuSampleTime_ms ;
// if timed out, reset the height, but do not fuse data on this time step
if ( hgtTimeout ) {
ResetHeight ( ) ;
fuseHgtData = false ;
}
}
else {
hgtHealth = false ;
}
}
// 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 {
innovVelPos [ obsIndex ] = stateStruct . position [ obsIndex - 3 ] - observation [ obsIndex ] ;
if ( obsIndex = = 5 ) {
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 ;
}
// 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
// Don't apply corrections to Z bias state as this has been done already as part of the single IMU calculations
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-10-07 15:27:09 -03:00
# endif // HAL_CPU_CLASS