2016-07-14 02:08:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF3.h"
# include "AP_NavEKF3_core.h"
# include <AP_AHRS/AP_AHRS.h>
# include <AP_Vehicle/AP_Vehicle.h>
2017-03-16 02:59:19 -03:00
# include <GCS_MAVLink/GCS.h>
2019-11-26 02:45:14 -04:00
# include <AP_RangeFinder/AP_RangeFinder.h>
2019-11-10 22:48:14 -04:00
# include <AP_RangeFinder/AP_RangeFinder_Backend.h>
2019-06-13 23:44:39 -03:00
# include <AP_GPS/AP_GPS.h>
2019-06-26 23:34:12 -03:00
# include <AP_Baro/AP_Baro.h>
2016-07-14 02:08:43 -03:00
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 NavEKF3_core : : ResetVelocity ( void )
{
2020-05-12 04:22:36 -03:00
// Store the velocity before the reset so that we can record the reset delta
2016-07-14 02:08:43 -03:00
velResetNE . x = stateStruct . velocity . x ;
velResetNE . y = stateStruct . velocity . y ;
// reset the corresponding covariances
zeroRows ( P , 4 , 5 ) ;
zeroCols ( P , 4 , 5 ) ;
if ( PV_AidingMode ! = AID_ABSOLUTE ) {
stateStruct . velocity . zero ( ) ;
// set the variances using the measurement noise parameter
P [ 5 ] [ 5 ] = P [ 4 ] [ 4 ] = sq ( frontend - > _gpsHorizVelNoise ) ;
} else {
// reset horizontal velocity states to the GPS velocity if available
2016-12-16 21:22:07 -04:00
if ( ( imuSampleTime_ms - lastTimeGpsReceived_ms < 250 & & velResetSource = = DEFAULT ) | | velResetSource = = GPS ) {
2020-04-16 03:25:16 -03:00
// correct for antenna position
gps_elements gps_corrected = gpsDataNew ;
CorrectGPSForAntennaOffset ( gps_corrected ) ;
2020-01-26 02:06:58 -04:00
stateStruct . velocity . x = gps_corrected . vel . x ;
stateStruct . velocity . y = gps_corrected . vel . y ;
2016-07-14 02:08:43 -03:00
// set the variances using the reported GPS speed accuracy
P [ 5 ] [ 5 ] = P [ 4 ] [ 4 ] = sq ( MAX ( frontend - > _gpsHorizVelNoise , gpsSpdAccuracy ) ) ;
// clear the timeout flags and counters
velTimeout = false ;
lastVelPassTime_ms = imuSampleTime_ms ;
2020-05-18 02:22:20 -03:00
} else if ( ( imuSampleTime_ms - extNavVelMeasTime_ms < 250 & & posResetSource = = DEFAULT ) | | velResetSource = = EXTNAV ) {
// use external nav data as the 2nd preference
// correct for sensor position
ext_nav_vel_elements extNavVelCorrected = extNavVelDelayed ;
CorrectExtNavVelForSensorOffset ( extNavVelCorrected . vel ) ;
stateStruct . velocity = extNavVelCorrected . vel ;
P [ 5 ] [ 5 ] = P [ 4 ] [ 4 ] = P [ 3 ] [ 3 ] = sq ( extNavVelDelayed . err ) ;
velTimeout = false ;
lastVelPassTime_ms = imuSampleTime_ms ;
2016-07-14 02:08:43 -03:00
} else {
stateStruct . velocity . x = 0.0f ;
stateStruct . velocity . y = 0.0f ;
// set the variances using the likely speed range
P [ 5 ] [ 5 ] = P [ 4 ] [ 4 ] = 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 ;
2020-05-12 04:22:36 -03:00
// Calculate the velocity jump due to the reset
2016-07-14 02:08:43 -03:00
velResetNE . x = stateStruct . velocity . x - velResetNE . x ;
velResetNE . y = stateStruct . velocity . y - velResetNE . y ;
// store the time of the reset
lastVelReset_ms = imuSampleTime_ms ;
2016-12-16 21:22:07 -04:00
// clear reset data source preference
velResetSource = DEFAULT ;
2016-07-14 02:08:43 -03:00
}
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF3_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 , 7 , 8 ) ;
zeroCols ( P , 7 , 8 ) ;
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 [ 7 ] [ 7 ] = P [ 8 ] [ 8 ] = sq ( frontend - > _gpsHorizPosNoise ) ;
} else {
// Use GPS data as first preference if fresh data is available
2016-12-16 21:22:07 -04:00
if ( ( imuSampleTime_ms - lastTimeGpsReceived_ms < 250 & & posResetSource = = DEFAULT ) | | posResetSource = = GPS ) {
2020-04-16 03:25:16 -03:00
// correct for antenna position
gps_elements gps_corrected = gpsDataNew ;
CorrectGPSForAntennaOffset ( gps_corrected ) ;
2017-01-28 07:01:35 -04:00
// record the ID of the GPS for the data we are using for the reset
2020-01-26 02:06:58 -04:00
last_gps_idx = gps_corrected . sensor_idx ;
2016-07-14 02:08:43 -03:00
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
2020-01-26 02:06:58 -04:00
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 ) ) ;
2016-07-14 02:08:43 -03:00
// set the variances using the position measurement noise parameter
P [ 7 ] [ 7 ] = P [ 8 ] [ 8 ] = sq ( MAX ( gpsPosAccuracy , frontend - > _gpsHorizPosNoise ) ) ;
// clear the timeout flags and counters
posTimeout = false ;
lastPosPassTime_ms = imuSampleTime_ms ;
2016-12-16 21:22:07 -04:00
} else if ( ( imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 & & posResetSource = = DEFAULT ) | | posResetSource = = RNGBCN ) {
2016-07-14 02:08:43 -03:00
// 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 [ 7 ] [ 7 ] = receiverPosCov [ 0 ] [ 0 ] ;
P [ 8 ] [ 8 ] = receiverPosCov [ 1 ] [ 1 ] ;
// clear the timeout flags and counters
rngBcnTimeout = false ;
lastRngBcnPassTime_ms = imuSampleTime_ms ;
2020-04-14 21:56:28 -03:00
} else if ( ( imuSampleTime_ms - extNavDataDelayed . time_ms < 250 & & posResetSource = = DEFAULT ) | | posResetSource = = EXTNAV ) {
2020-04-16 00:09:49 -03:00
// 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 ;
2020-04-14 21:56:28 -03:00
// set the variances as received from external nav system data
P [ 7 ] [ 7 ] = P [ 8 ] [ 8 ] = sq ( extNavDataDelayed . posErr ) ;
// clear the timeout flags and counters
2020-05-12 02:13:48 -03:00
posTimeout = false ;
lastPosPassTime_ms = imuSampleTime_ms ;
2016-07-14 02:08:43 -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 ;
// 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 ;
2016-12-16 21:22:07 -04:00
// clear reset source preference
posResetSource = DEFAULT ;
2016-07-14 02:08:43 -03:00
}
2020-04-16 08:15:29 -03:00
// 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 NavEKF3_core : : ResetPositionNE ( float posN , float 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 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 NavEKF3_core : : ResetPositionD ( float posD )
{
// Store the position before the reset so that we can record the reset delta
const float 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 ;
}
2016-07-14 02:08:43 -03:00
// reset the vertical position state using the last height measurement
void NavEKF3_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 ;
}
2019-10-12 19:26:47 -03:00
vertCompFiltState . pos = stateStruct . position . z ;
2016-07-14 02:08:43 -03:00
// 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 , 9 , 9 ) ;
zeroCols ( P , 9 , 9 ) ;
// set the variances to the measurement variance
P [ 9 ] [ 9 ] = 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
2017-07-24 22:41:51 -03:00
if ( inFlight & & ! gpsNotAvailable & & frontend - > _fusionModeGPS = = 0 & & ! frontend - > inhibitGpsVertVelUse ) {
2016-07-14 02:08:43 -03:00
stateStruct . velocity . z = gpsDataNew . vel . z ;
2020-05-18 02:22:20 -03:00
} else if ( inFlight & & useExtNavVel & & ( activeHgtSource = = HGT_SOURCE_EXTNAV ) ) {
stateStruct . velocity . z = extNavVelNew . vel . z ;
2016-07-14 02:08:43 -03:00
} 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 ;
2019-10-12 19:26:47 -03:00
vertCompFiltState . vel = outputDataNew . velocity . z ;
2016-07-14 02:08:43 -03:00
// reset the corresponding covariances
zeroRows ( P , 6 , 6 ) ;
zeroCols ( P , 6 , 6 ) ;
// set the variances to the measurement variance
P [ 6 ] [ 6 ] = sq ( frontend - > _gpsVertVelNoise ) ;
}
// Zero the EKF height datum
// Return true if the height datum reset has been performed
bool NavEKF3_core : : resetHeightDatum ( void )
{
2019-07-02 23:36:07 -03:00
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
2016-07-14 02:08:43 -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
2018-03-05 16:35:51 -04:00
AP : : baro ( ) . update_calibration ( ) ;
2016-07-14 02:08:43 -03:00
// reset the height state
stateStruct . position . z = 0.0f ;
2017-01-06 12:15:35 -04:00
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
2019-07-02 23:36:07 -03:00
if ( validOrigin ) {
2019-07-02 00:42:52 -03:00
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
2019-07-09 03:27:48 -03:00
EKF_origin . alt + = ( int32_t ) ( 100.0f * oldHgt ) ;
2019-07-02 00:42:52 -03:00
} else {
2019-07-01 16:56:40 -03:00
// 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
2019-07-09 03:27:48 -03:00
EKF_origin . alt = AP : : gps ( ) . location ( ) . alt ;
2019-07-01 16:56:40 -03:00
}
2019-07-09 03:27:48 -03:00
ekfGpsRefHgt = ( double ) 0.01 * ( double ) EKF_origin . alt ;
2016-07-14 02:08:43 -03:00
}
2019-07-02 00:42:52 -03:00
2019-07-02 03:17:46 -03:00
// set the terrain state to zero (on ground). The adjustment for
// frame height will get added in the later constraints
terrainState = 0 ;
2016-07-14 02:08:43 -03:00
return true ;
}
2020-01-26 02:06:58 -04:00
/*
correct GPS data for position offset of antenna phase centre relative to the IMU
*/
2020-04-16 03:25:16 -03:00
void NavEKF3_core : : CorrectGPSForAntennaOffset ( gps_elements & gps_data ) const
2020-01-26 02:06:58 -04:00
{
const Vector3f & posOffsetBody = AP : : gps ( ) . get_antenna_offset ( gps_data . sensor_idx ) - accelPosOffset ;
if ( posOffsetBody . is_zero ( ) ) {
return ;
}
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 - = velOffsetEarth ;
}
Vector3f posOffsetEarth = prevTnb . mul_transpose ( posOffsetBody ) ;
gps_data . pos . x - = posOffsetEarth . x ;
gps_data . pos . y - = posOffsetEarth . y ;
gps_data . hgt + = posOffsetEarth . z ;
}
2020-04-16 00:09:49 -03:00
// correct external navigation earth-frame position using sensor body-frame offset
void NavEKF3_core : : CorrectExtNavForSensorOffset ( Vector3f & ext_position )
{
# if HAL_VISUALODOM_ENABLED
AP_VisualOdom * visual_odom = AP : : visualodom ( ) ;
if ( visual_odom = = nullptr ) {
return ;
}
const Vector3f & posOffsetBody = visual_odom - > get_pos_offset ( ) - 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
}
2020-05-18 02:22:20 -03:00
// correct external navigation earth-frame velocity using sensor body-frame offset
void NavEKF3_core : : CorrectExtNavVelForSensorOffset ( Vector3f & ext_velocity ) const
{
# if HAL_VISUALODOM_ENABLED
AP_VisualOdom * visual_odom = AP : : visualodom ( ) ;
if ( visual_odom = = nullptr ) {
return ;
}
const Vector3f & posOffsetBody = visual_odom - > get_pos_offset ( ) - 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
}
2016-07-14 02:08:43 -03:00
/********************************************************
* FUSE MEASURED_DATA *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// select fusion of velocity, position and height measurements
void NavEKF3_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 ;
}
2020-04-14 21:56:28 -03:00
// Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav . recall ( extNavDataDelayed , imuDataDelayed . time_ms ) ;
2020-05-18 02:22:20 -03:00
extNavVelToFuse = storedExtNavVel . recall ( extNavVelDelayed , imuDataDelayed . time_ms ) ;
2020-04-14 21:56:28 -03:00
2020-04-23 02:42:24 -03:00
// Read GPS data from the sensor
readGpsData ( ) ;
// get data that has now fallen behind the fusion time horizon
gpsDataToFuse = storedGPS . recall ( gpsDataDelayed , imuDataDelayed . time_ms ) ;
2016-07-14 02:08:43 -03:00
// Determine if we need to fuse position and velocity data on this time step
2020-04-14 21:56:28 -03:00
if ( gpsDataToFuse & & ( PV_AidingMode = = AID_ABSOLUTE ) & & ( frontend - > _fusionModeGPS ! = 3 ) ) {
2017-01-28 07:01:35 -04:00
// Don't fuse velocity data if GPS doesn't support it
if ( frontend - > _fusionModeGPS < = 1 ) {
fuseVelData = true ;
} else {
fuseVelData = false ;
}
fusePosData = true ;
2020-04-14 21:56:28 -03:00
extNavUsedForPos = false ;
2017-01-28 07:01:35 -04:00
2020-01-26 02:06:58 -04:00
CorrectGPSForAntennaOffset ( gpsDataDelayed ) ;
2020-04-14 21:56:28 -03:00
// 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 ) & & ( frontend - > _fusionModeGPS = = 3 ) ) {
// use external nav system for position
extNavUsedForPos = true ;
activeHgtSource = HGT_SOURCE_EXTNAV ;
fuseVelData = false ;
fuseHgtData = true ;
fusePosData = true ;
2020-04-16 00:09:49 -03:00
// correct for external navigation sensor position
CorrectExtNavForSensorOffset ( extNavDataDelayed . pos ) ;
2020-04-14 21:56:28 -03:00
velPosObs [ 3 ] = extNavDataDelayed . pos . x ;
velPosObs [ 4 ] = extNavDataDelayed . pos . y ;
velPosObs [ 5 ] = extNavDataDelayed . pos . z ;
2016-07-14 02:08:43 -03:00
} else {
fuseVelData = false ;
fusePosData = false ;
}
2020-05-18 02:22:20 -03:00
// fuse external navigation velocity data if available
if ( extNavVelToFuse & & ( frontend - > _fusionModeGPS = = 3 ) ) {
fuseVelData = true ;
// correct for external navigation sensor position
Vector3f vel_corrected = extNavVelDelayed . vel ;
CorrectExtNavVelForSensorOffset ( vel_corrected ) ;
velPosObs [ 0 ] = vel_corrected . x ;
velPosObs [ 1 ] = vel_corrected . y ;
velPosObs [ 2 ] = vel_corrected . z ;
}
2016-07-14 02:08:43 -03:00
// 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 ( ) ;
2017-01-28 07:01:35 -04:00
// if we are using GPS, check for a change in receiver and reset position and height
2020-04-29 20:46:55 -03:00
if ( gpsDataToFuse & & ( PV_AidingMode = = AID_ABSOLUTE ) & & ( frontend - > _fusionModeGPS ! = 3 ) & & ( gpsDataDelayed . sensor_idx ! = last_gps_idx ) ) {
2017-01-28 07:01:35 -04:00
// 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
2020-01-26 02:06:58 -04:00
stateStruct . position . x = gpsDataDelayed . pos . x ;
stateStruct . position . y = gpsDataDelayed . pos . y ;
2017-01-28 07:01:35 -04:00
// 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 ;
2020-04-15 02:32:52 -03:00
// If we are also using GPS as the height reference, reset the height
2017-01-28 07:01:35 -04:00
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 ;
2019-10-12 19:26:47 -03:00
vertCompFiltState . pos = outputDataNew . position . z ;
2017-01-28 07:01:35 -04:00
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 ;
}
}
2020-04-16 08:15:29 -03:00
// 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 ) ;
}
}
2020-04-14 21:56:28 -03:00
2016-07-14 02:08:43 -03:00
// 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 ) {
2020-04-14 21:56:28 -03:00
velPosObs [ 3 ] = lastKnownPositionNE . x ;
velPosObs [ 4 ] = lastKnownPositionNE . y ;
2016-07-14 02:08:43 -03:00
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 NavEKF3_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
Vector6 R_OBS ; // Measurement variances used for fusion
Vector6 R_OBS_DATA_CHECKS ; // Measurement variances used for data checks only
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 ) {
// calculate additional error in GPS position caused by manoeuvring
float posErr = frontend - > gpsPosVarAccScale * accNavMag ;
2020-04-14 21:56:28 -03:00
// To-Do: this posErr should come from external nav when fusing external nav position
2016-07-14 02:08:43 -03:00
// 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 ) {
2020-04-15 02:32:52 -03:00
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
2016-07-14 02:08:43 -03:00
R_OBS [ 0 ] = sq ( constrain_float ( 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_float ( gpsSpdAccuracy , frontend - > _gpsHorizVelNoise , 50.0f ) ) ;
R_OBS [ 2 ] = sq ( constrain_float ( gpsSpdAccuracy , frontend - > _gpsVertVelNoise , 50.0f ) ) ;
2020-05-18 02:22:20 -03:00
} else if ( extNavVelToFuse ) {
R_OBS [ 2 ] = R_OBS [ 0 ] = sq ( constrain_float ( extNavVelDelayed . err , 0.05f , 5.0f ) ) ;
2016-07-14 02:08:43 -03:00
} 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 ] ;
// 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_float ( gpsPosAccuracy , frontend - > _gpsHorizPosNoise , 100.0f ) ) ;
2020-04-14 21:56:28 -03:00
} else if ( extNavUsedForPos ) {
R_OBS [ 3 ] = sq ( constrain_float ( extNavDataDelayed . posErr , 0.01f , 10.0f ) ) ;
2016-07-14 02:08:43 -03:00
} else {
R_OBS [ 3 ] = sq ( constrain_float ( 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
2019-02-22 19:35:24 -04:00
// 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 performance
2016-07-14 02:08:43 -03:00
// 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 [ 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
2020-04-14 21:56:28 -03:00
const float hgtErr = stateStruct . position . z - velPosObs [ 5 ] ;
const float velDErr = stateStruct . velocity . z - velPosObs [ 2 ] ;
2016-07-14 02:08:43 -03:00
// 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 [ 9 ] [ 9 ] + R_OBS_DATA_CHECKS [ 5 ] ) ) & & ( sq ( velDErr ) > 9.0f * ( P [ 6 ] [ 6 ] + 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
2020-04-14 21:56:28 -03:00
innovVelPos [ 3 ] = stateStruct . position . x - velPosObs [ 3 ] ;
innovVelPos [ 4 ] = stateStruct . position . y - velPosObs [ 4 ] ;
2016-07-14 02:08:43 -03:00
varInnovVelPos [ 3 ] = P [ 7 ] [ 7 ] + R_OBS_DATA_CHECKS [ 3 ] ;
varInnovVelPos [ 4 ] = P [ 8 ] [ 8 ] + 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 ) ;
// 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 [ 8 ] [ 8 ] + 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 , 7 , 8 ) ;
zeroCols ( P , 7 , 8 ) ;
P [ 7 ] [ 7 ] = sq ( float ( 0.5f * frontend - > _gpsGlitchRadiusMax ) ) ;
P [ 8 ] [ 8 ] = P [ 7 ] [ 7 ] ;
// 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
2020-05-18 02:22:20 -03:00
if ( ( frontend - > _fusionModeGPS > 0 | | PV_AidingMode ! = AID_ABSOLUTE | | frontend - > inhibitGpsVertVelUse ) & & ! useExtNavVel ) {
2016-07-14 02:08:43 -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 4
stateIndex = i + 4 ;
// calculate innovations using blended and single IMU predicted states
2020-04-14 21:56:28 -03:00
velInnov [ i ] = stateStruct . velocity [ i ] - velPosObs [ i ] ; // blended
2016-07-14 02:08:43 -03:00
// 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 ) ;
// 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
2020-04-14 21:56:28 -03:00
innovVelPos [ 5 ] = stateStruct . position . z - velPosObs [ 5 ] ;
2016-07-14 02:08:43 -03:00
varInnovVelPos [ 5 ] = P [ 9 ] [ 9 ] + 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 ] ) ;
2019-07-29 18:50:39 -03:00
// 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 float maxTestRatio = ( PV_AidingMode = = AID_NONE & & onGround ) ? 3.0 : 1.0 ;
2016-07-14 02:08:43 -03:00
// fail if the ratio is > 1, but don't fail if bad IMU data
2019-07-29 18:50:39 -03:00
hgtHealth = ( ( hgtTestRatio < maxTestRatio ) | | badIMUdata ) ;
2016-07-14 02:08:43 -03:00
// Fuse height data if healthy or timed out or in constant position mode
2019-07-29 18:50:39 -03:00
if ( hgtHealth | | hgtTimeout ) {
2016-07-14 02:08:43 -03: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 ;
}
// 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 ;
2020-05-18 02:22:20 -03:00
if ( useGpsVertVel | | useExtNavVel ) {
2016-07-14 02:08:43 -03:00
fuseData [ 2 ] = true ;
}
}
if ( fusePosData & & posHealth ) {
fuseData [ 3 ] = true ;
fuseData [ 4 ] = true ;
}
if ( fuseHgtData & & hgtHealth ) {
fuseData [ 5 ] = true ;
}
// fuse measurements sequentially
for ( obsIndex = 0 ; obsIndex < = 5 ; obsIndex + + ) {
if ( fuseData [ obsIndex ] ) {
stateIndex = 4 + 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
2020-04-14 21:56:28 -03:00
if ( obsIndex < = 2 ) {
innovVelPos [ obsIndex ] = stateStruct . velocity [ obsIndex ] - velPosObs [ obsIndex ] ;
2016-07-14 02:08:43 -03:00
R_OBS [ obsIndex ] * = sq ( gpsNoiseScaler ) ;
2020-04-14 21:56:28 -03:00
} else if ( obsIndex = = 3 | | obsIndex = = 4 ) {
innovVelPos [ obsIndex ] = stateStruct . position [ obsIndex - 3 ] - velPosObs [ obsIndex ] ;
2016-07-14 02:08:43 -03:00
R_OBS [ obsIndex ] * = sq ( gpsNoiseScaler ) ;
} else if ( obsIndex = = 5 ) {
2020-04-14 21:56:28 -03:00
innovVelPos [ obsIndex ] = stateStruct . position [ obsIndex - 3 ] - velPosObs [ obsIndex ] ;
2016-07-14 02:08:43 -03:00
const float gndMaxBaroErr = 4.0f ;
const float gndBaroInnovFloor = - 0.5f ;
2017-03-24 14:35:16 -03:00
if ( getTouchdownExpected ( ) & & activeHgtSource = = HGT_SOURCE_BARO ) {
2016-07-14 02:08:43 -03:00
// 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 ] ;
2017-05-09 19:31:55 -03:00
for ( uint8_t i = 0 ; i < = 9 ; i + + ) {
2016-07-14 02:08:43 -03:00
Kfusion [ i ] = P [ i ] [ stateIndex ] * SK ;
}
2017-05-09 19:31:55 -03:00
// inhibit delta angle bias state estmation by setting Kalman gains to zero
if ( ! inhibitDelAngBiasStates ) {
for ( uint8_t i = 10 ; i < = 12 ; i + + ) {
Kfusion [ i ] = P [ i ] [ stateIndex ] * SK ;
}
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset ( & Kfusion [ 10 ] , 0 , 12 ) ;
}
2019-02-22 19:35:24 -04:00
// inhibit delta velocity bias state estimation by setting Kalman gains to zero
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelVelBiasStates ) {
for ( uint8_t i = 13 ; i < = 15 ; i + + ) {
Kfusion [ i ] = P [ i ] [ stateIndex ] * SK ;
}
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset ( & Kfusion [ 13 ] , 0 , 12 ) ;
}
2016-07-14 02:08:43 -03:00
// 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 {
2017-05-09 19:31:55 -03:00
// zero indexes 16 to 21 = 6*4 bytes
memset ( & Kfusion [ 16 ] , 0 , 24 ) ;
2016-07-14 02:08:43 -03:00
}
// 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 {
2017-05-09 19:31:55 -03:00
// zero indexes 22 to 23 = 2*4 bytes
memset ( & Kfusion [ 22 ] , 0 , 8 ) ;
2016-07-14 02:08:43 -03:00
}
// 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 ] ;
}
}
2019-02-22 19:35:24 -04:00
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
2016-07-14 02:08:43 -03:00
ForceSymmetry ( ) ;
ConstrainVariances ( ) ;
// update states and renormalise the quaternions
for ( uint8_t i = 0 ; i < = stateIndexLim ; i + + ) {
statesArray [ i ] = statesArray [ i ] - Kfusion [ i ] * innovVelPos [ obsIndex ] ;
}
stateStruct . quat . normalize ( ) ;
// 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 ;
}
}
}
}
}
// 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 NavEKF3_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
2019-11-26 02:45:14 -04:00
const RangeFinder * _rng = AP : : rangefinder ( ) ;
if ( _rng & & rangeDataToFuse ) {
AP_RangeFinder_Backend * sensor = _rng - > get_backend ( rangeDataDelayed . sensor_idx ) ;
2017-08-08 03:14:53 -03:00
if ( sensor ! = nullptr ) {
Vector3f posOffsetBody = sensor - > get_pos_offset ( ) - accelPosOffset ;
if ( ! posOffsetBody . is_zero ( ) ) {
Vector3f posOffsetEarth = prevTnb . mul_transpose ( posOffsetBody ) ;
rangeDataDelayed . rng + = posOffsetEarth . z / prevTnb . c . z ;
}
2016-07-14 02:08:43 -03:00
}
}
// read baro height data from the sensor and check for new data in the buffer
readBaroData ( ) ;
baroDataToFuse = storedBaro . recall ( baroDataDelayed , imuDataDelayed . time_ms ) ;
// select height source
2020-04-14 21:56:28 -03:00
if ( extNavUsedForPos & & ( frontend - > _altSource = = 4 ) ) {
// always use external navigation as the height source if using for position.
activeHgtSource = HGT_SOURCE_EXTNAV ;
} else if ( _rng & & ( ( frontend - > _useRngSwHgt > 0 ) & & ( frontend - > _altSource = = 1 ) ) & & ( imuSampleTime_ms - rngValidMeaTime_ms < 500 ) ) {
2016-07-14 02:08:43 -03:00
if ( frontend - > _altSource = = 1 ) {
// always use range finder
activeHgtSource = HGT_SOURCE_RNG ;
} else {
// determine if we are above or below the height switch region
2019-11-26 02:45:14 -04:00
float rangeMaxUse = 1e-4 f * ( float ) _rng - > max_distance_cm_orient ( ROTATION_PITCH_270 ) * ( float ) frontend - > _useRngSwHgt ;
2016-07-14 02:08:43 -03:00
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
2016-12-12 18:08:39 -04:00
// apply a hysteresis to the speed check to prevent rapid switching
2016-12-20 08:39:24 -04:00
bool dontTrustTerrain , trustTerrain ;
if ( filterStatus . flags . horiz_vel ) {
// We can use the velocity estimate
float horizSpeed = norm ( stateStruct . velocity . x , stateStruct . velocity . y ) ;
dontTrustTerrain = ( horizSpeed > frontend - > _useRngSwSpd ) | | ! terrainHgtStable ;
float trust_spd_trigger = MAX ( ( frontend - > _useRngSwSpd - 1.0f ) , ( frontend - > _useRngSwSpd * 0.5f ) ) ;
trustTerrain = ( horizSpeed < trust_spd_trigger ) & & terrainHgtStable ;
} else {
// We can't use the velocity estimate
dontTrustTerrain = ! terrainHgtStable ;
trustTerrain = terrainHgtStable ;
}
2016-07-14 02:08:43 -03:00
/*
* 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 & & ( activeHgtSource ! = HGT_SOURCE_RNG ) ) {
// reliable terrain and range finder so start using range finder height
activeHgtSource = HGT_SOURCE_RNG ;
}
}
} 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 ;
} else {
activeHgtSource = HGT_SOURCE_BARO ;
}
2020-04-14 21:56:28 -03:00
// Use Baro alt as a fallback if we lose range finder, GPS or external nav
2016-07-14 02:08:43 -03:00
bool lostRngHgt = ( ( activeHgtSource = = HGT_SOURCE_RNG ) & & ( ( imuSampleTime_ms - rngValidMeaTime_ms ) > 500 ) ) ;
bool lostGpsHgt = ( ( activeHgtSource = = HGT_SOURCE_GPS ) & & ( ( imuSampleTime_ms - lastTimeGpsReceived_ms ) > 2000 ) ) ;
2020-04-14 21:56:28 -03:00
bool lostExtNavHgt = ( ( activeHgtSource = = HGT_SOURCE_EXTNAV ) & & ( ( imuSampleTime_ms - extNavMeasTime_ms ) > 2000 ) ) ;
if ( lostRngHgt | | lostGpsHgt | | lostExtNavHgt ) {
2016-07-14 02:08:43 -03:00
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 ( ! 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 ;
}
}
2017-05-09 03:23:58 -03:00
// 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 ( ) ;
2016-07-14 02:08:43 -03:00
}
// Select the height measurement source
2020-04-14 21:56:28 -03:00
if ( extNavDataToFuse & & ( activeHgtSource = = HGT_SOURCE_EXTNAV ) ) {
hgtMea = - extNavDataDelayed . pos . z ;
posDownObsNoise = sq ( constrain_float ( extNavDataDelayed . posErr , 0.1f , 10.0f ) ) ;
} else if ( rangeDataToFuse & & ( activeHgtSource = = HGT_SOURCE_RNG ) ) {
2016-07-14 02:08:43 -03:00
// 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 ;
2020-04-14 21:56:28 -03:00
velPosObs [ 5 ] = - hgtMea ;
2016-07-14 02:08:43 -03:00
// enable fusion
fuseHgtData = true ;
// set the observation noise
posDownObsNoise = sq ( constrain_float ( 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 ;
2020-04-14 21:56:28 -03:00
velPosObs [ 5 ] = - hgtMea ;
2016-07-14 02:08:43 -03:00
// enable fusion
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_float ( gpsHgtAccuracy , 1.5f * frontend - > _gpsHorizPosNoise , 100.0f ) ) ;
} else {
posDownObsNoise = sq ( constrain_float ( 1.5f * frontend - > _gpsHorizPosNoise , 0.1f , 10.0f ) ) ;
}
} else if ( baroDataToFuse & & ( activeHgtSource = = HGT_SOURCE_BARO ) ) {
// 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 ;
}
// 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 & & getTakeoffExpected ( ) ) {
hgtMea = MAX ( hgtMea , meaHgtAtTakeOff ) ;
}
2020-04-14 21:56:28 -03:00
velPosObs [ 5 ] = - hgtMea ;
2016-07-14 02:08:43 -03:00
} 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
2020-05-18 02:22:20 -03:00
hgtRetryTime_ms = ( ( useGpsVertVel | | useExtNavVel ) & & ! velTimeout ) ? frontend - > hgtRetryTimeMode0_ms : frontend - > hgtRetryTimeMode12_ms ;
2016-07-14 02:08:43 -03:00
if ( imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms ) {
hgtTimeout = true ;
} else {
hgtTimeout = false ;
}
}
2017-03-16 02:59:19 -03:00
/*
* Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox .
* The script file used to generate these and other equations in this filter can be found here :
* https : //github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
*/
void NavEKF3_core : : FuseBodyVel ( )
{
Vector24 H_VEL ;
Vector3f bodyVelPred ;
// Copy required states to local variable names
float q0 = stateStruct . quat [ 0 ] ;
float q1 = stateStruct . quat [ 1 ] ;
float q2 = stateStruct . quat [ 2 ] ;
float q3 = stateStruct . quat [ 3 ] ;
float vn = stateStruct . velocity . x ;
float ve = stateStruct . velocity . y ;
float vd = stateStruct . velocity . z ;
// Fuse X, Y and Z axis measurements sequentially assuming observation errors are uncorrelated
for ( uint8_t obsIndex = 0 ; obsIndex < = 2 ; obsIndex + + ) {
// calculate relative velocity in sensor frame including the relative motion due to rotation
bodyVelPred = ( prevTnb * stateStruct . velocity ) ;
// correct sensor offset body frame position offset relative to IMU
Vector3f posOffsetBody = ( * bodyOdmDataDelayed . body_offset ) - accelPosOffset ;
// correct prediction for relative motion due to rotation
// note - % operator overloaded for cross product
if ( imuDataDelayed . delAngDT > 0.001f ) {
bodyVelPred + = ( imuDataDelayed . delAng * ( 1.0f / imuDataDelayed . delAngDT ) ) % posOffsetBody ;
}
// calculate observation jacobians and Kalman gains
if ( obsIndex = = 0 ) {
// calculate X axis observation Jacobian
H_VEL [ 0 ] = q2 * vd * - 2.0f + q3 * ve * 2.0f + q0 * vn * 2.0f ;
H_VEL [ 1 ] = q3 * vd * 2.0f + q2 * ve * 2.0f + q1 * vn * 2.0f ;
H_VEL [ 2 ] = q0 * vd * - 2.0f + q1 * ve * 2.0f - q2 * vn * 2.0f ;
H_VEL [ 3 ] = q1 * vd * 2.0f + q0 * ve * 2.0f - q3 * vn * 2.0f ;
H_VEL [ 4 ] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3 ;
H_VEL [ 5 ] = q0 * q3 * 2.0f + q1 * q2 * 2.0f ;
H_VEL [ 6 ] = q0 * q2 * - 2.0f + q1 * q3 * 2.0f ;
for ( uint8_t index = 7 ; index < 24 ; index + + ) {
H_VEL [ index ] = 0.0f ;
}
// calculate intermediate expressions for X axis Kalman gains
2017-07-27 01:55:02 -03:00
float R_VEL = sq ( bodyOdmDataDelayed . velErr ) ;
2017-03-16 02:59:19 -03:00
float t2 = q0 * q3 * 2.0f ;
float t3 = q1 * q2 * 2.0f ;
float t4 = t2 + t3 ;
float t5 = q0 * q0 ;
float t6 = q1 * q1 ;
float t7 = q2 * q2 ;
float t8 = q3 * q3 ;
float t9 = t5 + t6 - t7 - t8 ;
float t10 = q0 * q2 * 2.0f ;
float t25 = q1 * q3 * 2.0f ;
float t11 = t10 - t25 ;
float t12 = q3 * ve * 2.0f ;
float t13 = q0 * vn * 2.0f ;
float t26 = q2 * vd * 2.0f ;
float t14 = t12 + t13 - t26 ;
float t15 = q3 * vd * 2.0f ;
float t16 = q2 * ve * 2.0f ;
float t17 = q1 * vn * 2.0f ;
float t18 = t15 + t16 + t17 ;
float t19 = q0 * vd * 2.0f ;
float t20 = q2 * vn * 2.0f ;
float t27 = q1 * ve * 2.0f ;
float t21 = t19 + t20 - t27 ;
float t22 = q1 * vd * 2.0f ;
float t23 = q0 * ve * 2.0f ;
float t28 = q3 * vn * 2.0f ;
float t24 = t22 + t23 - t28 ;
float t29 = P [ 0 ] [ 0 ] * t14 ;
float t30 = P [ 1 ] [ 1 ] * t18 ;
float t31 = P [ 4 ] [ 5 ] * t9 ;
float t32 = P [ 5 ] [ 5 ] * t4 ;
float t33 = P [ 0 ] [ 5 ] * t14 ;
float t34 = P [ 1 ] [ 5 ] * t18 ;
float t35 = P [ 3 ] [ 5 ] * t24 ;
float t79 = P [ 6 ] [ 5 ] * t11 ;
float t80 = P [ 2 ] [ 5 ] * t21 ;
float t36 = t31 + t32 + t33 + t34 + t35 - t79 - t80 ;
float t37 = t4 * t36 ;
float t38 = P [ 4 ] [ 6 ] * t9 ;
float t39 = P [ 5 ] [ 6 ] * t4 ;
float t40 = P [ 0 ] [ 6 ] * t14 ;
float t41 = P [ 1 ] [ 6 ] * t18 ;
float t42 = P [ 3 ] [ 6 ] * t24 ;
float t81 = P [ 6 ] [ 6 ] * t11 ;
float t82 = P [ 2 ] [ 6 ] * t21 ;
float t43 = t38 + t39 + t40 + t41 + t42 - t81 - t82 ;
float t44 = P [ 4 ] [ 0 ] * t9 ;
float t45 = P [ 5 ] [ 0 ] * t4 ;
float t46 = P [ 1 ] [ 0 ] * t18 ;
float t47 = P [ 3 ] [ 0 ] * t24 ;
float t84 = P [ 6 ] [ 0 ] * t11 ;
float t85 = P [ 2 ] [ 0 ] * t21 ;
float t48 = t29 + t44 + t45 + t46 + t47 - t84 - t85 ;
float t49 = t14 * t48 ;
float t50 = P [ 4 ] [ 1 ] * t9 ;
float t51 = P [ 5 ] [ 1 ] * t4 ;
float t52 = P [ 0 ] [ 1 ] * t14 ;
float t53 = P [ 3 ] [ 1 ] * t24 ;
float t86 = P [ 6 ] [ 1 ] * t11 ;
float t87 = P [ 2 ] [ 1 ] * t21 ;
float t54 = t30 + t50 + t51 + t52 + t53 - t86 - t87 ;
float t55 = t18 * t54 ;
float t56 = P [ 4 ] [ 2 ] * t9 ;
float t57 = P [ 5 ] [ 2 ] * t4 ;
float t58 = P [ 0 ] [ 2 ] * t14 ;
float t59 = P [ 1 ] [ 2 ] * t18 ;
float t60 = P [ 3 ] [ 2 ] * t24 ;
float t78 = P [ 2 ] [ 2 ] * t21 ;
float t88 = P [ 6 ] [ 2 ] * t11 ;
float t61 = t56 + t57 + t58 + t59 + t60 - t78 - t88 ;
float t62 = P [ 4 ] [ 3 ] * t9 ;
float t63 = P [ 5 ] [ 3 ] * t4 ;
float t64 = P [ 0 ] [ 3 ] * t14 ;
float t65 = P [ 1 ] [ 3 ] * t18 ;
float t66 = P [ 3 ] [ 3 ] * t24 ;
float t90 = P [ 6 ] [ 3 ] * t11 ;
float t91 = P [ 2 ] [ 3 ] * t21 ;
float t67 = t62 + t63 + t64 + t65 + t66 - t90 - t91 ;
float t68 = t24 * t67 ;
float t69 = P [ 4 ] [ 4 ] * t9 ;
float t70 = P [ 5 ] [ 4 ] * t4 ;
float t71 = P [ 0 ] [ 4 ] * t14 ;
float t72 = P [ 1 ] [ 4 ] * t18 ;
float t73 = P [ 3 ] [ 4 ] * t24 ;
float t92 = P [ 6 ] [ 4 ] * t11 ;
float t93 = P [ 2 ] [ 4 ] * t21 ;
float t74 = t69 + t70 + t71 + t72 + t73 - t92 - t93 ;
float t75 = t9 * t74 ;
float t83 = t11 * t43 ;
float t89 = t21 * t61 ;
float t76 = R_VEL + t37 + t49 + t55 + t68 + t75 - t83 - t89 ;
float t77 ;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if ( t76 > R_VEL ) {
t77 = 1.0f / t76 ;
faultStatus . bad_xvel = false ;
} else {
t76 = R_VEL ;
t77 = 1.0f / R_VEL ;
faultStatus . bad_xvel = true ;
return ;
}
2019-10-29 11:59:28 -03:00
varInnovBodyVel [ 0 ] = t76 ;
2017-03-16 02:59:19 -03:00
// calculate innovation for X axis observation
innovBodyVel [ 0 ] = bodyVelPred . x - bodyOdmDataDelayed . vel . x ;
// calculate Kalman gains for X-axis observation
Kfusion [ 0 ] = t77 * ( t29 + P [ 0 ] [ 5 ] * t4 + P [ 0 ] [ 4 ] * t9 - P [ 0 ] [ 6 ] * t11 + P [ 0 ] [ 1 ] * t18 - P [ 0 ] [ 2 ] * t21 + P [ 0 ] [ 3 ] * t24 ) ;
Kfusion [ 1 ] = t77 * ( t30 + P [ 1 ] [ 5 ] * t4 + P [ 1 ] [ 4 ] * t9 + P [ 1 ] [ 0 ] * t14 - P [ 1 ] [ 6 ] * t11 - P [ 1 ] [ 2 ] * t21 + P [ 1 ] [ 3 ] * t24 ) ;
Kfusion [ 2 ] = t77 * ( - t78 + P [ 2 ] [ 5 ] * t4 + P [ 2 ] [ 4 ] * t9 + P [ 2 ] [ 0 ] * t14 - P [ 2 ] [ 6 ] * t11 + P [ 2 ] [ 1 ] * t18 + P [ 2 ] [ 3 ] * t24 ) ;
Kfusion [ 3 ] = t77 * ( t66 + P [ 3 ] [ 5 ] * t4 + P [ 3 ] [ 4 ] * t9 + P [ 3 ] [ 0 ] * t14 - P [ 3 ] [ 6 ] * t11 + P [ 3 ] [ 1 ] * t18 - P [ 3 ] [ 2 ] * t21 ) ;
Kfusion [ 4 ] = t77 * ( t69 + P [ 4 ] [ 5 ] * t4 + P [ 4 ] [ 0 ] * t14 - P [ 4 ] [ 6 ] * t11 + P [ 4 ] [ 1 ] * t18 - P [ 4 ] [ 2 ] * t21 + P [ 4 ] [ 3 ] * t24 ) ;
Kfusion [ 5 ] = t77 * ( t32 + P [ 5 ] [ 4 ] * t9 + P [ 5 ] [ 0 ] * t14 - P [ 5 ] [ 6 ] * t11 + P [ 5 ] [ 1 ] * t18 - P [ 5 ] [ 2 ] * t21 + P [ 5 ] [ 3 ] * t24 ) ;
Kfusion [ 6 ] = t77 * ( - t81 + P [ 6 ] [ 5 ] * t4 + P [ 6 ] [ 4 ] * t9 + P [ 6 ] [ 0 ] * t14 + P [ 6 ] [ 1 ] * t18 - P [ 6 ] [ 2 ] * t21 + P [ 6 ] [ 3 ] * t24 ) ;
Kfusion [ 7 ] = t77 * ( P [ 7 ] [ 5 ] * t4 + P [ 7 ] [ 4 ] * t9 + P [ 7 ] [ 0 ] * t14 - P [ 7 ] [ 6 ] * t11 + P [ 7 ] [ 1 ] * t18 - P [ 7 ] [ 2 ] * t21 + P [ 7 ] [ 3 ] * t24 ) ;
Kfusion [ 8 ] = t77 * ( P [ 8 ] [ 5 ] * t4 + P [ 8 ] [ 4 ] * t9 + P [ 8 ] [ 0 ] * t14 - P [ 8 ] [ 6 ] * t11 + P [ 8 ] [ 1 ] * t18 - P [ 8 ] [ 2 ] * t21 + P [ 8 ] [ 3 ] * t24 ) ;
Kfusion [ 9 ] = t77 * ( P [ 9 ] [ 5 ] * t4 + P [ 9 ] [ 4 ] * t9 + P [ 9 ] [ 0 ] * t14 - P [ 9 ] [ 6 ] * t11 + P [ 9 ] [ 1 ] * t18 - P [ 9 ] [ 2 ] * t21 + P [ 9 ] [ 3 ] * t24 ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = t77 * ( P [ 10 ] [ 5 ] * t4 + P [ 10 ] [ 4 ] * t9 + P [ 10 ] [ 0 ] * t14 - P [ 10 ] [ 6 ] * t11 + P [ 10 ] [ 1 ] * t18 - P [ 10 ] [ 2 ] * t21 + P [ 10 ] [ 3 ] * t24 ) ;
Kfusion [ 11 ] = t77 * ( P [ 11 ] [ 5 ] * t4 + P [ 11 ] [ 4 ] * t9 + P [ 11 ] [ 0 ] * t14 - P [ 11 ] [ 6 ] * t11 + P [ 11 ] [ 1 ] * t18 - P [ 11 ] [ 2 ] * t21 + P [ 11 ] [ 3 ] * t24 ) ;
Kfusion [ 12 ] = t77 * ( P [ 12 ] [ 5 ] * t4 + P [ 12 ] [ 4 ] * t9 + P [ 12 ] [ 0 ] * t14 - P [ 12 ] [ 6 ] * t11 + P [ 12 ] [ 1 ] * t18 - P [ 12 ] [ 2 ] * t21 + P [ 12 ] [ 3 ] * t24 ) ;
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset ( & Kfusion [ 10 ] , 0 , 12 ) ;
}
if ( ! inhibitDelVelBiasStates ) {
Kfusion [ 13 ] = t77 * ( P [ 13 ] [ 5 ] * t4 + P [ 13 ] [ 4 ] * t9 + P [ 13 ] [ 0 ] * t14 - P [ 13 ] [ 6 ] * t11 + P [ 13 ] [ 1 ] * t18 - P [ 13 ] [ 2 ] * t21 + P [ 13 ] [ 3 ] * t24 ) ;
Kfusion [ 14 ] = t77 * ( P [ 14 ] [ 5 ] * t4 + P [ 14 ] [ 4 ] * t9 + P [ 14 ] [ 0 ] * t14 - P [ 14 ] [ 6 ] * t11 + P [ 14 ] [ 1 ] * t18 - P [ 14 ] [ 2 ] * t21 + P [ 14 ] [ 3 ] * t24 ) ;
Kfusion [ 15 ] = t77 * ( P [ 15 ] [ 5 ] * t4 + P [ 15 ] [ 4 ] * t9 + P [ 15 ] [ 0 ] * t14 - P [ 15 ] [ 6 ] * t11 + P [ 15 ] [ 1 ] * t18 - P [ 15 ] [ 2 ] * t21 + P [ 15 ] [ 3 ] * t24 ) ;
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset ( & Kfusion [ 13 ] , 0 , 12 ) ;
}
2017-03-16 02:59:19 -03:00
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = t77 * ( P [ 16 ] [ 5 ] * t4 + P [ 16 ] [ 4 ] * t9 + P [ 16 ] [ 0 ] * t14 - P [ 16 ] [ 6 ] * t11 + P [ 16 ] [ 1 ] * t18 - P [ 16 ] [ 2 ] * t21 + P [ 16 ] [ 3 ] * t24 ) ;
Kfusion [ 17 ] = t77 * ( P [ 17 ] [ 5 ] * t4 + P [ 17 ] [ 4 ] * t9 + P [ 17 ] [ 0 ] * t14 - P [ 17 ] [ 6 ] * t11 + P [ 17 ] [ 1 ] * t18 - P [ 17 ] [ 2 ] * t21 + P [ 17 ] [ 3 ] * t24 ) ;
Kfusion [ 18 ] = t77 * ( P [ 18 ] [ 5 ] * t4 + P [ 18 ] [ 4 ] * t9 + P [ 18 ] [ 0 ] * t14 - P [ 18 ] [ 6 ] * t11 + P [ 18 ] [ 1 ] * t18 - P [ 18 ] [ 2 ] * t21 + P [ 18 ] [ 3 ] * t24 ) ;
Kfusion [ 19 ] = t77 * ( P [ 19 ] [ 5 ] * t4 + P [ 19 ] [ 4 ] * t9 + P [ 19 ] [ 0 ] * t14 - P [ 19 ] [ 6 ] * t11 + P [ 19 ] [ 1 ] * t18 - P [ 19 ] [ 2 ] * t21 + P [ 19 ] [ 3 ] * t24 ) ;
Kfusion [ 20 ] = t77 * ( P [ 20 ] [ 5 ] * t4 + P [ 20 ] [ 4 ] * t9 + P [ 20 ] [ 0 ] * t14 - P [ 20 ] [ 6 ] * t11 + P [ 20 ] [ 1 ] * t18 - P [ 20 ] [ 2 ] * t21 + P [ 20 ] [ 3 ] * t24 ) ;
Kfusion [ 21 ] = t77 * ( P [ 21 ] [ 5 ] * t4 + P [ 21 ] [ 4 ] * t9 + P [ 21 ] [ 0 ] * t14 - P [ 21 ] [ 6 ] * t11 + P [ 21 ] [ 1 ] * t18 - P [ 21 ] [ 2 ] * t21 + P [ 21 ] [ 3 ] * t24 ) ;
} else {
2017-05-09 19:31:55 -03:00
// zero indexes 16 to 21 = 6*4 bytes
memset ( & Kfusion [ 16 ] , 0 , 24 ) ;
2017-03-16 02:59:19 -03:00
}
2017-05-09 19:31:55 -03:00
2017-03-16 02:59:19 -03:00
if ( ! inhibitWindStates ) {
Kfusion [ 22 ] = t77 * ( P [ 22 ] [ 5 ] * t4 + P [ 22 ] [ 4 ] * t9 + P [ 22 ] [ 0 ] * t14 - P [ 22 ] [ 6 ] * t11 + P [ 22 ] [ 1 ] * t18 - P [ 22 ] [ 2 ] * t21 + P [ 22 ] [ 3 ] * t24 ) ;
Kfusion [ 23 ] = t77 * ( P [ 23 ] [ 5 ] * t4 + P [ 23 ] [ 4 ] * t9 + P [ 23 ] [ 0 ] * t14 - P [ 23 ] [ 6 ] * t11 + P [ 23 ] [ 1 ] * t18 - P [ 23 ] [ 2 ] * t21 + P [ 23 ] [ 3 ] * t24 ) ;
} else {
2017-05-09 19:31:55 -03:00
// zero indexes 22 to 23 = 2*4 bytes
memset ( & Kfusion [ 22 ] , 0 , 8 ) ;
2017-03-16 02:59:19 -03:00
}
} else if ( obsIndex = = 1 ) {
// calculate Y axis observation Jacobian
H_VEL [ 0 ] = q1 * vd * 2.0f + q0 * ve * 2.0f - q3 * vn * 2.0f ;
H_VEL [ 1 ] = q0 * vd * 2.0f - q1 * ve * 2.0f + q2 * vn * 2.0f ;
H_VEL [ 2 ] = q3 * vd * 2.0f + q2 * ve * 2.0f + q1 * vn * 2.0f ;
H_VEL [ 3 ] = q2 * vd * 2.0f - q3 * ve * 2.0f - q0 * vn * 2.0f ;
H_VEL [ 4 ] = q0 * q3 * - 2.0f + q1 * q2 * 2.0f ;
H_VEL [ 5 ] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3 ;
H_VEL [ 6 ] = q0 * q1 * 2.0f + q2 * q3 * 2.0f ;
for ( uint8_t index = 7 ; index < 24 ; index + + ) {
H_VEL [ index ] = 0.0f ;
}
// calculate intermediate expressions for Y axis Kalman gains
2017-07-27 01:55:02 -03:00
float R_VEL = sq ( bodyOdmDataDelayed . velErr ) ;
2017-03-16 02:59:19 -03:00
float t2 = q0 * q3 * 2.0f ;
float t9 = q1 * q2 * 2.0f ;
float t3 = t2 - t9 ;
float t4 = q0 * q0 ;
float t5 = q1 * q1 ;
float t6 = q2 * q2 ;
float t7 = q3 * q3 ;
float t8 = t4 - t5 + t6 - t7 ;
float t10 = q0 * q1 * 2.0f ;
float t11 = q2 * q3 * 2.0f ;
float t12 = t10 + t11 ;
float t13 = q1 * vd * 2.0f ;
float t14 = q0 * ve * 2.0f ;
float t26 = q3 * vn * 2.0f ;
float t15 = t13 + t14 - t26 ;
float t16 = q0 * vd * 2.0f ;
float t17 = q2 * vn * 2.0f ;
float t27 = q1 * ve * 2.0f ;
float t18 = t16 + t17 - t27 ;
float t19 = q3 * vd * 2.0f ;
float t20 = q2 * ve * 2.0f ;
float t21 = q1 * vn * 2.0f ;
float t22 = t19 + t20 + t21 ;
float t23 = q3 * ve * 2.0f ;
float t24 = q0 * vn * 2.0f ;
float t28 = q2 * vd * 2.0f ;
float t25 = t23 + t24 - t28 ;
float t29 = P [ 0 ] [ 0 ] * t15 ;
float t30 = P [ 1 ] [ 1 ] * t18 ;
float t31 = P [ 5 ] [ 4 ] * t8 ;
float t32 = P [ 6 ] [ 4 ] * t12 ;
float t33 = P [ 0 ] [ 4 ] * t15 ;
float t34 = P [ 1 ] [ 4 ] * t18 ;
float t35 = P [ 2 ] [ 4 ] * t22 ;
float t78 = P [ 4 ] [ 4 ] * t3 ;
float t79 = P [ 3 ] [ 4 ] * t25 ;
float t36 = t31 + t32 + t33 + t34 + t35 - t78 - t79 ;
float t37 = P [ 5 ] [ 6 ] * t8 ;
float t38 = P [ 6 ] [ 6 ] * t12 ;
float t39 = P [ 0 ] [ 6 ] * t15 ;
float t40 = P [ 1 ] [ 6 ] * t18 ;
float t41 = P [ 2 ] [ 6 ] * t22 ;
float t81 = P [ 4 ] [ 6 ] * t3 ;
float t82 = P [ 3 ] [ 6 ] * t25 ;
float t42 = t37 + t38 + t39 + t40 + t41 - t81 - t82 ;
float t43 = t12 * t42 ;
float t44 = P [ 5 ] [ 0 ] * t8 ;
float t45 = P [ 6 ] [ 0 ] * t12 ;
float t46 = P [ 1 ] [ 0 ] * t18 ;
float t47 = P [ 2 ] [ 0 ] * t22 ;
float t83 = P [ 4 ] [ 0 ] * t3 ;
float t84 = P [ 3 ] [ 0 ] * t25 ;
float t48 = t29 + t44 + t45 + t46 + t47 - t83 - t84 ;
float t49 = t15 * t48 ;
float t50 = P [ 5 ] [ 1 ] * t8 ;
float t51 = P [ 6 ] [ 1 ] * t12 ;
float t52 = P [ 0 ] [ 1 ] * t15 ;
float t53 = P [ 2 ] [ 1 ] * t22 ;
float t85 = P [ 4 ] [ 1 ] * t3 ;
float t86 = P [ 3 ] [ 1 ] * t25 ;
float t54 = t30 + t50 + t51 + t52 + t53 - t85 - t86 ;
float t55 = t18 * t54 ;
float t56 = P [ 5 ] [ 2 ] * t8 ;
float t57 = P [ 6 ] [ 2 ] * t12 ;
float t58 = P [ 0 ] [ 2 ] * t15 ;
float t59 = P [ 1 ] [ 2 ] * t18 ;
float t60 = P [ 2 ] [ 2 ] * t22 ;
float t87 = P [ 4 ] [ 2 ] * t3 ;
float t88 = P [ 3 ] [ 2 ] * t25 ;
float t61 = t56 + t57 + t58 + t59 + t60 - t87 - t88 ;
float t62 = t22 * t61 ;
float t63 = P [ 5 ] [ 3 ] * t8 ;
float t64 = P [ 6 ] [ 3 ] * t12 ;
float t65 = P [ 0 ] [ 3 ] * t15 ;
float t66 = P [ 1 ] [ 3 ] * t18 ;
float t67 = P [ 2 ] [ 3 ] * t22 ;
float t89 = P [ 4 ] [ 3 ] * t3 ;
float t90 = P [ 3 ] [ 3 ] * t25 ;
float t68 = t63 + t64 + t65 + t66 + t67 - t89 - t90 ;
float t69 = P [ 5 ] [ 5 ] * t8 ;
float t70 = P [ 6 ] [ 5 ] * t12 ;
float t71 = P [ 0 ] [ 5 ] * t15 ;
float t72 = P [ 1 ] [ 5 ] * t18 ;
float t73 = P [ 2 ] [ 5 ] * t22 ;
float t92 = P [ 4 ] [ 5 ] * t3 ;
float t93 = P [ 3 ] [ 5 ] * t25 ;
float t74 = t69 + t70 + t71 + t72 + t73 - t92 - t93 ;
float t75 = t8 * t74 ;
float t80 = t3 * t36 ;
float t91 = t25 * t68 ;
float t76 = R_VEL + t43 + t49 + t55 + t62 + t75 - t80 - t91 ;
float t77 ;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if ( t76 > R_VEL ) {
t77 = 1.0f / t76 ;
faultStatus . bad_yvel = false ;
} else {
t76 = R_VEL ;
t77 = 1.0f / R_VEL ;
faultStatus . bad_yvel = true ;
return ;
}
2019-10-29 11:59:28 -03:00
varInnovBodyVel [ 1 ] = t76 ;
2017-03-16 02:59:19 -03:00
// calculate innovation for Y axis observation
innovBodyVel [ 1 ] = bodyVelPred . y - bodyOdmDataDelayed . vel . y ;
// calculate Kalman gains for Y-axis observation
Kfusion [ 0 ] = t77 * ( t29 - P [ 0 ] [ 4 ] * t3 + P [ 0 ] [ 5 ] * t8 + P [ 0 ] [ 6 ] * t12 + P [ 0 ] [ 1 ] * t18 + P [ 0 ] [ 2 ] * t22 - P [ 0 ] [ 3 ] * t25 ) ;
Kfusion [ 1 ] = t77 * ( t30 - P [ 1 ] [ 4 ] * t3 + P [ 1 ] [ 5 ] * t8 + P [ 1 ] [ 0 ] * t15 + P [ 1 ] [ 6 ] * t12 + P [ 1 ] [ 2 ] * t22 - P [ 1 ] [ 3 ] * t25 ) ;
Kfusion [ 2 ] = t77 * ( t60 - P [ 2 ] [ 4 ] * t3 + P [ 2 ] [ 5 ] * t8 + P [ 2 ] [ 0 ] * t15 + P [ 2 ] [ 6 ] * t12 + P [ 2 ] [ 1 ] * t18 - P [ 2 ] [ 3 ] * t25 ) ;
Kfusion [ 3 ] = t77 * ( - t90 - P [ 3 ] [ 4 ] * t3 + P [ 3 ] [ 5 ] * t8 + P [ 3 ] [ 0 ] * t15 + P [ 3 ] [ 6 ] * t12 + P [ 3 ] [ 1 ] * t18 + P [ 3 ] [ 2 ] * t22 ) ;
Kfusion [ 4 ] = t77 * ( - t78 + P [ 4 ] [ 5 ] * t8 + P [ 4 ] [ 0 ] * t15 + P [ 4 ] [ 6 ] * t12 + P [ 4 ] [ 1 ] * t18 + P [ 4 ] [ 2 ] * t22 - P [ 4 ] [ 3 ] * t25 ) ;
Kfusion [ 5 ] = t77 * ( t69 - P [ 5 ] [ 4 ] * t3 + P [ 5 ] [ 0 ] * t15 + P [ 5 ] [ 6 ] * t12 + P [ 5 ] [ 1 ] * t18 + P [ 5 ] [ 2 ] * t22 - P [ 5 ] [ 3 ] * t25 ) ;
Kfusion [ 6 ] = t77 * ( t38 - P [ 6 ] [ 4 ] * t3 + P [ 6 ] [ 5 ] * t8 + P [ 6 ] [ 0 ] * t15 + P [ 6 ] [ 1 ] * t18 + P [ 6 ] [ 2 ] * t22 - P [ 6 ] [ 3 ] * t25 ) ;
Kfusion [ 7 ] = t77 * ( - P [ 7 ] [ 4 ] * t3 + P [ 7 ] [ 5 ] * t8 + P [ 7 ] [ 0 ] * t15 + P [ 7 ] [ 6 ] * t12 + P [ 7 ] [ 1 ] * t18 + P [ 7 ] [ 2 ] * t22 - P [ 7 ] [ 3 ] * t25 ) ;
Kfusion [ 8 ] = t77 * ( - P [ 8 ] [ 4 ] * t3 + P [ 8 ] [ 5 ] * t8 + P [ 8 ] [ 0 ] * t15 + P [ 8 ] [ 6 ] * t12 + P [ 8 ] [ 1 ] * t18 + P [ 8 ] [ 2 ] * t22 - P [ 8 ] [ 3 ] * t25 ) ;
Kfusion [ 9 ] = t77 * ( - P [ 9 ] [ 4 ] * t3 + P [ 9 ] [ 5 ] * t8 + P [ 9 ] [ 0 ] * t15 + P [ 9 ] [ 6 ] * t12 + P [ 9 ] [ 1 ] * t18 + P [ 9 ] [ 2 ] * t22 - P [ 9 ] [ 3 ] * t25 ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = t77 * ( - P [ 10 ] [ 4 ] * t3 + P [ 10 ] [ 5 ] * t8 + P [ 10 ] [ 0 ] * t15 + P [ 10 ] [ 6 ] * t12 + P [ 10 ] [ 1 ] * t18 + P [ 10 ] [ 2 ] * t22 - P [ 10 ] [ 3 ] * t25 ) ;
Kfusion [ 11 ] = t77 * ( - P [ 11 ] [ 4 ] * t3 + P [ 11 ] [ 5 ] * t8 + P [ 11 ] [ 0 ] * t15 + P [ 11 ] [ 6 ] * t12 + P [ 11 ] [ 1 ] * t18 + P [ 11 ] [ 2 ] * t22 - P [ 11 ] [ 3 ] * t25 ) ;
Kfusion [ 12 ] = t77 * ( - P [ 12 ] [ 4 ] * t3 + P [ 12 ] [ 5 ] * t8 + P [ 12 ] [ 0 ] * t15 + P [ 12 ] [ 6 ] * t12 + P [ 12 ] [ 1 ] * t18 + P [ 12 ] [ 2 ] * t22 - P [ 12 ] [ 3 ] * t25 ) ;
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset ( & Kfusion [ 10 ] , 0 , 12 ) ;
}
if ( ! inhibitDelVelBiasStates ) {
Kfusion [ 13 ] = t77 * ( - P [ 13 ] [ 4 ] * t3 + P [ 13 ] [ 5 ] * t8 + P [ 13 ] [ 0 ] * t15 + P [ 13 ] [ 6 ] * t12 + P [ 13 ] [ 1 ] * t18 + P [ 13 ] [ 2 ] * t22 - P [ 13 ] [ 3 ] * t25 ) ;
Kfusion [ 14 ] = t77 * ( - P [ 14 ] [ 4 ] * t3 + P [ 14 ] [ 5 ] * t8 + P [ 14 ] [ 0 ] * t15 + P [ 14 ] [ 6 ] * t12 + P [ 14 ] [ 1 ] * t18 + P [ 14 ] [ 2 ] * t22 - P [ 14 ] [ 3 ] * t25 ) ;
Kfusion [ 15 ] = t77 * ( - P [ 15 ] [ 4 ] * t3 + P [ 15 ] [ 5 ] * t8 + P [ 15 ] [ 0 ] * t15 + P [ 15 ] [ 6 ] * t12 + P [ 15 ] [ 1 ] * t18 + P [ 15 ] [ 2 ] * t22 - P [ 15 ] [ 3 ] * t25 ) ;
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset ( & Kfusion [ 13 ] , 0 , 12 ) ;
}
2017-03-16 02:59:19 -03:00
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = t77 * ( - P [ 16 ] [ 4 ] * t3 + P [ 16 ] [ 5 ] * t8 + P [ 16 ] [ 0 ] * t15 + P [ 16 ] [ 6 ] * t12 + P [ 16 ] [ 1 ] * t18 + P [ 16 ] [ 2 ] * t22 - P [ 16 ] [ 3 ] * t25 ) ;
Kfusion [ 17 ] = t77 * ( - P [ 17 ] [ 4 ] * t3 + P [ 17 ] [ 5 ] * t8 + P [ 17 ] [ 0 ] * t15 + P [ 17 ] [ 6 ] * t12 + P [ 17 ] [ 1 ] * t18 + P [ 17 ] [ 2 ] * t22 - P [ 17 ] [ 3 ] * t25 ) ;
Kfusion [ 18 ] = t77 * ( - P [ 18 ] [ 4 ] * t3 + P [ 18 ] [ 5 ] * t8 + P [ 18 ] [ 0 ] * t15 + P [ 18 ] [ 6 ] * t12 + P [ 18 ] [ 1 ] * t18 + P [ 18 ] [ 2 ] * t22 - P [ 18 ] [ 3 ] * t25 ) ;
Kfusion [ 19 ] = t77 * ( - P [ 19 ] [ 4 ] * t3 + P [ 19 ] [ 5 ] * t8 + P [ 19 ] [ 0 ] * t15 + P [ 19 ] [ 6 ] * t12 + P [ 19 ] [ 1 ] * t18 + P [ 19 ] [ 2 ] * t22 - P [ 19 ] [ 3 ] * t25 ) ;
Kfusion [ 20 ] = t77 * ( - P [ 20 ] [ 4 ] * t3 + P [ 20 ] [ 5 ] * t8 + P [ 20 ] [ 0 ] * t15 + P [ 20 ] [ 6 ] * t12 + P [ 20 ] [ 1 ] * t18 + P [ 20 ] [ 2 ] * t22 - P [ 20 ] [ 3 ] * t25 ) ;
Kfusion [ 21 ] = t77 * ( - P [ 21 ] [ 4 ] * t3 + P [ 21 ] [ 5 ] * t8 + P [ 21 ] [ 0 ] * t15 + P [ 21 ] [ 6 ] * t12 + P [ 21 ] [ 1 ] * t18 + P [ 21 ] [ 2 ] * t22 - P [ 21 ] [ 3 ] * t25 ) ;
} else {
2017-05-09 19:31:55 -03:00
// zero indexes 16 to 21 = 6*4 bytes
memset ( & Kfusion [ 16 ] , 0 , 24 ) ;
2017-03-16 02:59:19 -03:00
}
2017-05-09 19:31:55 -03:00
2017-03-16 02:59:19 -03:00
if ( ! inhibitWindStates ) {
Kfusion [ 22 ] = t77 * ( - P [ 22 ] [ 4 ] * t3 + P [ 22 ] [ 5 ] * t8 + P [ 22 ] [ 0 ] * t15 + P [ 22 ] [ 6 ] * t12 + P [ 22 ] [ 1 ] * t18 + P [ 22 ] [ 2 ] * t22 - P [ 22 ] [ 3 ] * t25 ) ;
Kfusion [ 23 ] = t77 * ( - P [ 23 ] [ 4 ] * t3 + P [ 23 ] [ 5 ] * t8 + P [ 23 ] [ 0 ] * t15 + P [ 23 ] [ 6 ] * t12 + P [ 23 ] [ 1 ] * t18 + P [ 23 ] [ 2 ] * t22 - P [ 23 ] [ 3 ] * t25 ) ;
} else {
2017-05-09 19:31:55 -03:00
// zero indexes 22 to 23 = 2*4 bytes
memset ( & Kfusion [ 22 ] , 0 , 8 ) ;
2017-03-16 02:59:19 -03:00
}
} else if ( obsIndex = = 2 ) {
// calculate Z axis observation Jacobian
H_VEL [ 0 ] = q0 * vd * 2.0f - q1 * ve * 2.0f + q2 * vn * 2.0f ;
H_VEL [ 1 ] = q1 * vd * - 2.0f - q0 * ve * 2.0f + q3 * vn * 2.0f ;
H_VEL [ 2 ] = q2 * vd * - 2.0f + q3 * ve * 2.0f + q0 * vn * 2.0f ;
H_VEL [ 3 ] = q3 * vd * 2.0f + q2 * ve * 2.0f + q1 * vn * 2.0f ;
H_VEL [ 4 ] = q0 * q2 * 2.0f + q1 * q3 * 2.0f ;
H_VEL [ 5 ] = q0 * q1 * - 2.0f + q2 * q3 * 2.0f ;
H_VEL [ 6 ] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3 ;
for ( uint8_t index = 7 ; index < 24 ; index + + ) {
H_VEL [ index ] = 0.0f ;
}
// calculate intermediate expressions for Z axis Kalman gains
2017-07-27 01:55:02 -03:00
float R_VEL = sq ( bodyOdmDataDelayed . velErr ) ;
2017-03-16 02:59:19 -03:00
float t2 = q0 * q2 * 2.0f ;
float t3 = q1 * q3 * 2.0f ;
float t4 = t2 + t3 ;
float t5 = q0 * q0 ;
float t6 = q1 * q1 ;
float t7 = q2 * q2 ;
float t8 = q3 * q3 ;
float t9 = t5 - t6 - t7 + t8 ;
float t10 = q0 * q1 * 2.0f ;
float t25 = q2 * q3 * 2.0f ;
float t11 = t10 - t25 ;
float t12 = q0 * vd * 2.0f ;
float t13 = q2 * vn * 2.0f ;
float t26 = q1 * ve * 2.0f ;
float t14 = t12 + t13 - t26 ;
float t15 = q1 * vd * 2.0f ;
float t16 = q0 * ve * 2.0f ;
float t27 = q3 * vn * 2.0f ;
float t17 = t15 + t16 - t27 ;
float t18 = q3 * ve * 2.0f ;
float t19 = q0 * vn * 2.0f ;
float t28 = q2 * vd * 2.0f ;
float t20 = t18 + t19 - t28 ;
float t21 = q3 * vd * 2.0f ;
float t22 = q2 * ve * 2.0f ;
float t23 = q1 * vn * 2.0f ;
float t24 = t21 + t22 + t23 ;
float t29 = P [ 0 ] [ 0 ] * t14 ;
float t30 = P [ 6 ] [ 4 ] * t9 ;
float t31 = P [ 4 ] [ 4 ] * t4 ;
float t32 = P [ 0 ] [ 4 ] * t14 ;
float t33 = P [ 2 ] [ 4 ] * t20 ;
float t34 = P [ 3 ] [ 4 ] * t24 ;
float t78 = P [ 5 ] [ 4 ] * t11 ;
float t79 = P [ 1 ] [ 4 ] * t17 ;
float t35 = t30 + t31 + t32 + t33 + t34 - t78 - t79 ;
float t36 = t4 * t35 ;
float t37 = P [ 6 ] [ 5 ] * t9 ;
float t38 = P [ 4 ] [ 5 ] * t4 ;
float t39 = P [ 0 ] [ 5 ] * t14 ;
float t40 = P [ 2 ] [ 5 ] * t20 ;
float t41 = P [ 3 ] [ 5 ] * t24 ;
float t80 = P [ 5 ] [ 5 ] * t11 ;
float t81 = P [ 1 ] [ 5 ] * t17 ;
float t42 = t37 + t38 + t39 + t40 + t41 - t80 - t81 ;
float t43 = P [ 6 ] [ 0 ] * t9 ;
float t44 = P [ 4 ] [ 0 ] * t4 ;
float t45 = P [ 2 ] [ 0 ] * t20 ;
float t46 = P [ 3 ] [ 0 ] * t24 ;
float t83 = P [ 5 ] [ 0 ] * t11 ;
float t84 = P [ 1 ] [ 0 ] * t17 ;
float t47 = t29 + t43 + t44 + t45 + t46 - t83 - t84 ;
float t48 = t14 * t47 ;
float t49 = P [ 6 ] [ 1 ] * t9 ;
float t50 = P [ 4 ] [ 1 ] * t4 ;
float t51 = P [ 0 ] [ 1 ] * t14 ;
float t52 = P [ 2 ] [ 1 ] * t20 ;
float t53 = P [ 3 ] [ 1 ] * t24 ;
float t85 = P [ 5 ] [ 1 ] * t11 ;
float t86 = P [ 1 ] [ 1 ] * t17 ;
float t54 = t49 + t50 + t51 + t52 + t53 - t85 - t86 ;
float t55 = P [ 6 ] [ 2 ] * t9 ;
float t56 = P [ 4 ] [ 2 ] * t4 ;
float t57 = P [ 0 ] [ 2 ] * t14 ;
float t58 = P [ 2 ] [ 2 ] * t20 ;
float t59 = P [ 3 ] [ 2 ] * t24 ;
float t88 = P [ 5 ] [ 2 ] * t11 ;
float t89 = P [ 1 ] [ 2 ] * t17 ;
float t60 = t55 + t56 + t57 + t58 + t59 - t88 - t89 ;
float t61 = t20 * t60 ;
float t62 = P [ 6 ] [ 3 ] * t9 ;
float t63 = P [ 4 ] [ 3 ] * t4 ;
float t64 = P [ 0 ] [ 3 ] * t14 ;
float t65 = P [ 2 ] [ 3 ] * t20 ;
float t66 = P [ 3 ] [ 3 ] * t24 ;
float t90 = P [ 5 ] [ 3 ] * t11 ;
float t91 = P [ 1 ] [ 3 ] * t17 ;
float t67 = t62 + t63 + t64 + t65 + t66 - t90 - t91 ;
float t68 = t24 * t67 ;
float t69 = P [ 6 ] [ 6 ] * t9 ;
float t70 = P [ 4 ] [ 6 ] * t4 ;
float t71 = P [ 0 ] [ 6 ] * t14 ;
float t72 = P [ 2 ] [ 6 ] * t20 ;
float t73 = P [ 3 ] [ 6 ] * t24 ;
float t92 = P [ 5 ] [ 6 ] * t11 ;
float t93 = P [ 1 ] [ 6 ] * t17 ;
float t74 = t69 + t70 + t71 + t72 + t73 - t92 - t93 ;
float t75 = t9 * t74 ;
float t82 = t11 * t42 ;
float t87 = t17 * t54 ;
float t76 = R_VEL + t36 + t48 + t61 + t68 + t75 - t82 - t87 ;
float t77 ;
// calculate innovation variance for Z axis observation and protect against a badly conditioned calculation
if ( t76 > R_VEL ) {
t77 = 1.0f / t76 ;
faultStatus . bad_zvel = false ;
} else {
t76 = R_VEL ;
t77 = 1.0f / R_VEL ;
faultStatus . bad_zvel = true ;
return ;
}
2019-10-29 11:59:28 -03:00
varInnovBodyVel [ 2 ] = t76 ;
2017-03-16 02:59:19 -03:00
// calculate innovation for Z axis observation
innovBodyVel [ 2 ] = bodyVelPred . z - bodyOdmDataDelayed . vel . z ;
// calculate Kalman gains for X-axis observation
Kfusion [ 0 ] = t77 * ( t29 + P [ 0 ] [ 4 ] * t4 + P [ 0 ] [ 6 ] * t9 - P [ 0 ] [ 5 ] * t11 - P [ 0 ] [ 1 ] * t17 + P [ 0 ] [ 2 ] * t20 + P [ 0 ] [ 3 ] * t24 ) ;
Kfusion [ 1 ] = t77 * ( P [ 1 ] [ 4 ] * t4 + P [ 1 ] [ 0 ] * t14 + P [ 1 ] [ 6 ] * t9 - P [ 1 ] [ 5 ] * t11 - P [ 1 ] [ 1 ] * t17 + P [ 1 ] [ 2 ] * t20 + P [ 1 ] [ 3 ] * t24 ) ;
Kfusion [ 2 ] = t77 * ( t58 + P [ 2 ] [ 4 ] * t4 + P [ 2 ] [ 0 ] * t14 + P [ 2 ] [ 6 ] * t9 - P [ 2 ] [ 5 ] * t11 - P [ 2 ] [ 1 ] * t17 + P [ 2 ] [ 3 ] * t24 ) ;
Kfusion [ 3 ] = t77 * ( t66 + P [ 3 ] [ 4 ] * t4 + P [ 3 ] [ 0 ] * t14 + P [ 3 ] [ 6 ] * t9 - P [ 3 ] [ 5 ] * t11 - P [ 3 ] [ 1 ] * t17 + P [ 3 ] [ 2 ] * t20 ) ;
Kfusion [ 4 ] = t77 * ( t31 + P [ 4 ] [ 0 ] * t14 + P [ 4 ] [ 6 ] * t9 - P [ 4 ] [ 5 ] * t11 - P [ 4 ] [ 1 ] * t17 + P [ 4 ] [ 2 ] * t20 + P [ 4 ] [ 3 ] * t24 ) ;
Kfusion [ 5 ] = t77 * ( - t80 + P [ 5 ] [ 4 ] * t4 + P [ 5 ] [ 0 ] * t14 + P [ 5 ] [ 6 ] * t9 - P [ 5 ] [ 1 ] * t17 + P [ 5 ] [ 2 ] * t20 + P [ 5 ] [ 3 ] * t24 ) ;
Kfusion [ 6 ] = t77 * ( t69 + P [ 6 ] [ 4 ] * t4 + P [ 6 ] [ 0 ] * t14 - P [ 6 ] [ 5 ] * t11 - P [ 6 ] [ 1 ] * t17 + P [ 6 ] [ 2 ] * t20 + P [ 6 ] [ 3 ] * t24 ) ;
Kfusion [ 7 ] = t77 * ( P [ 7 ] [ 4 ] * t4 + P [ 7 ] [ 0 ] * t14 + P [ 7 ] [ 6 ] * t9 - P [ 7 ] [ 5 ] * t11 - P [ 7 ] [ 1 ] * t17 + P [ 7 ] [ 2 ] * t20 + P [ 7 ] [ 3 ] * t24 ) ;
Kfusion [ 8 ] = t77 * ( P [ 8 ] [ 4 ] * t4 + P [ 8 ] [ 0 ] * t14 + P [ 8 ] [ 6 ] * t9 - P [ 8 ] [ 5 ] * t11 - P [ 8 ] [ 1 ] * t17 + P [ 8 ] [ 2 ] * t20 + P [ 8 ] [ 3 ] * t24 ) ;
Kfusion [ 9 ] = t77 * ( P [ 9 ] [ 4 ] * t4 + P [ 9 ] [ 0 ] * t14 + P [ 9 ] [ 6 ] * t9 - P [ 9 ] [ 5 ] * t11 - P [ 9 ] [ 1 ] * t17 + P [ 9 ] [ 2 ] * t20 + P [ 9 ] [ 3 ] * t24 ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = t77 * ( P [ 10 ] [ 4 ] * t4 + P [ 10 ] [ 0 ] * t14 + P [ 10 ] [ 6 ] * t9 - P [ 10 ] [ 5 ] * t11 - P [ 10 ] [ 1 ] * t17 + P [ 10 ] [ 2 ] * t20 + P [ 10 ] [ 3 ] * t24 ) ;
Kfusion [ 11 ] = t77 * ( P [ 11 ] [ 4 ] * t4 + P [ 11 ] [ 0 ] * t14 + P [ 11 ] [ 6 ] * t9 - P [ 11 ] [ 5 ] * t11 - P [ 11 ] [ 1 ] * t17 + P [ 11 ] [ 2 ] * t20 + P [ 11 ] [ 3 ] * t24 ) ;
Kfusion [ 12 ] = t77 * ( P [ 12 ] [ 4 ] * t4 + P [ 12 ] [ 0 ] * t14 + P [ 12 ] [ 6 ] * t9 - P [ 12 ] [ 5 ] * t11 - P [ 12 ] [ 1 ] * t17 + P [ 12 ] [ 2 ] * t20 + P [ 12 ] [ 3 ] * t24 ) ;
} else {
// zero indexes 10 to 12 = 3*4 bytes
memset ( & Kfusion [ 10 ] , 0 , 12 ) ;
}
if ( ! inhibitDelVelBiasStates ) {
Kfusion [ 13 ] = t77 * ( P [ 13 ] [ 4 ] * t4 + P [ 13 ] [ 0 ] * t14 + P [ 13 ] [ 6 ] * t9 - P [ 13 ] [ 5 ] * t11 - P [ 13 ] [ 1 ] * t17 + P [ 13 ] [ 2 ] * t20 + P [ 13 ] [ 3 ] * t24 ) ;
Kfusion [ 14 ] = t77 * ( P [ 14 ] [ 4 ] * t4 + P [ 14 ] [ 0 ] * t14 + P [ 14 ] [ 6 ] * t9 - P [ 14 ] [ 5 ] * t11 - P [ 14 ] [ 1 ] * t17 + P [ 14 ] [ 2 ] * t20 + P [ 14 ] [ 3 ] * t24 ) ;
Kfusion [ 15 ] = t77 * ( P [ 15 ] [ 4 ] * t4 + P [ 15 ] [ 0 ] * t14 + P [ 15 ] [ 6 ] * t9 - P [ 15 ] [ 5 ] * t11 - P [ 15 ] [ 1 ] * t17 + P [ 15 ] [ 2 ] * t20 + P [ 15 ] [ 3 ] * t24 ) ;
} else {
// zero indexes 13 to 15 = 3*4 bytes
memset ( & Kfusion [ 13 ] , 0 , 12 ) ;
}
2017-03-16 02:59:19 -03:00
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = t77 * ( P [ 16 ] [ 4 ] * t4 + P [ 16 ] [ 0 ] * t14 + P [ 16 ] [ 6 ] * t9 - P [ 16 ] [ 5 ] * t11 - P [ 16 ] [ 1 ] * t17 + P [ 16 ] [ 2 ] * t20 + P [ 16 ] [ 3 ] * t24 ) ;
Kfusion [ 17 ] = t77 * ( P [ 17 ] [ 4 ] * t4 + P [ 17 ] [ 0 ] * t14 + P [ 17 ] [ 6 ] * t9 - P [ 17 ] [ 5 ] * t11 - P [ 17 ] [ 1 ] * t17 + P [ 17 ] [ 2 ] * t20 + P [ 17 ] [ 3 ] * t24 ) ;
Kfusion [ 18 ] = t77 * ( P [ 18 ] [ 4 ] * t4 + P [ 18 ] [ 0 ] * t14 + P [ 18 ] [ 6 ] * t9 - P [ 18 ] [ 5 ] * t11 - P [ 18 ] [ 1 ] * t17 + P [ 18 ] [ 2 ] * t20 + P [ 18 ] [ 3 ] * t24 ) ;
Kfusion [ 19 ] = t77 * ( P [ 19 ] [ 4 ] * t4 + P [ 19 ] [ 0 ] * t14 + P [ 19 ] [ 6 ] * t9 - P [ 19 ] [ 5 ] * t11 - P [ 19 ] [ 1 ] * t17 + P [ 19 ] [ 2 ] * t20 + P [ 19 ] [ 3 ] * t24 ) ;
Kfusion [ 20 ] = t77 * ( P [ 20 ] [ 4 ] * t4 + P [ 20 ] [ 0 ] * t14 + P [ 20 ] [ 6 ] * t9 - P [ 20 ] [ 5 ] * t11 - P [ 20 ] [ 1 ] * t17 + P [ 20 ] [ 2 ] * t20 + P [ 20 ] [ 3 ] * t24 ) ;
Kfusion [ 21 ] = t77 * ( P [ 21 ] [ 4 ] * t4 + P [ 21 ] [ 0 ] * t14 + P [ 21 ] [ 6 ] * t9 - P [ 21 ] [ 5 ] * t11 - P [ 21 ] [ 1 ] * t17 + P [ 21 ] [ 2 ] * t20 + P [ 21 ] [ 3 ] * t24 ) ;
} else {
2017-05-09 19:31:55 -03:00
// zero indexes 16 to 21 = 6*4 bytes
memset ( & Kfusion [ 16 ] , 0 , 24 ) ;
2017-03-16 02:59:19 -03:00
}
2017-05-09 19:31:55 -03:00
2017-03-16 02:59:19 -03:00
if ( ! inhibitWindStates ) {
Kfusion [ 22 ] = t77 * ( P [ 22 ] [ 4 ] * t4 + P [ 22 ] [ 0 ] * t14 + P [ 22 ] [ 6 ] * t9 - P [ 22 ] [ 5 ] * t11 - P [ 22 ] [ 1 ] * t17 + P [ 22 ] [ 2 ] * t20 + P [ 22 ] [ 3 ] * t24 ) ;
Kfusion [ 23 ] = t77 * ( P [ 23 ] [ 4 ] * t4 + P [ 23 ] [ 0 ] * t14 + P [ 23 ] [ 6 ] * t9 - P [ 23 ] [ 5 ] * t11 - P [ 23 ] [ 1 ] * t17 + P [ 23 ] [ 2 ] * t20 + P [ 23 ] [ 3 ] * t24 ) ;
} else {
2017-05-09 19:31:55 -03:00
// zero indexes 22 to 23 = 2*4 bytes
memset ( & Kfusion [ 22 ] , 0 , 8 ) ;
2017-03-16 02:59:19 -03:00
}
} else {
return ;
}
// calculate the innovation consistency test ratio
// TODO add tuning parameter for gate
bodyVelTestRatio [ obsIndex ] = sq ( innovBodyVel [ obsIndex ] ) / ( sq ( 5.0f ) * varInnovBodyVel [ obsIndex ] ) ;
// Check the innovation for consistency and don't fuse if out of bounds
// TODO also apply angular velocity magnitude check
if ( ( bodyVelTestRatio [ obsIndex ] ) < 1.0f ) {
// record the last time observations were accepted for fusion
prevBodyVelFuseTime_ms = imuSampleTime_ms ;
// notify first time only
if ( ! bodyVelFusionActive ) {
bodyVelFusionActive = true ;
2017-07-09 01:17:14 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF3 IMU%u fusing odometry " , ( unsigned ) imu_index ) ;
2017-03-16 02:59:19 -03:00
}
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for ( unsigned i = 0 ; i < = stateIndexLim ; i + + ) {
for ( unsigned j = 0 ; j < = 6 ; j + + ) {
KH [ i ] [ j ] = Kfusion [ i ] * H_VEL [ j ] ;
}
for ( unsigned j = 7 ; j < = stateIndexLim ; j + + ) {
KH [ i ] [ j ] = 0.0f ;
}
}
for ( unsigned j = 0 ; j < = stateIndexLim ; j + + ) {
for ( unsigned i = 0 ; i < = stateIndexLim ; i + + ) {
ftype res = 0 ;
res + = KH [ i ] [ 0 ] * P [ 0 ] [ j ] ;
res + = KH [ i ] [ 1 ] * P [ 1 ] [ j ] ;
res + = KH [ i ] [ 2 ] * P [ 2 ] [ j ] ;
res + = KH [ i ] [ 3 ] * P [ 3 ] [ j ] ;
res + = KH [ i ] [ 4 ] * P [ 4 ] [ j ] ;
res + = KH [ i ] [ 5 ] * P [ 5 ] [ j ] ;
res + = KH [ i ] [ 6 ] * P [ 6 ] [ j ] ;
KHP [ i ] [ j ] = res ;
}
}
// 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 ] ;
}
}
2019-02-22 19:35:24 -04:00
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
2017-03-16 02:59:19 -03:00
ForceSymmetry ( ) ;
ConstrainVariances ( ) ;
// correct the state vector
for ( uint8_t j = 0 ; j < = stateIndexLim ; j + + ) {
statesArray [ j ] = statesArray [ j ] - Kfusion [ j ] * innovBodyVel [ obsIndex ] ;
}
stateStruct . quat . normalize ( ) ;
} else {
// record bad axis
if ( obsIndex = = 0 ) {
faultStatus . bad_xvel = true ;
} else if ( obsIndex = = 1 ) {
faultStatus . bad_yvel = true ;
} else if ( obsIndex = = 2 ) {
faultStatus . bad_zvel = true ;
}
}
}
}
}
// select fusion of body odometry measurements
void NavEKF3_core : : SelectBodyOdomFusion ( )
{
// 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 ) & & ! bodyVelFusionDelayed ) {
bodyVelFusionDelayed = true ;
return ;
} else {
bodyVelFusionDelayed = false ;
}
// Check for data at the fusion time horizon
if ( storedBodyOdm . recall ( bodyOdmDataDelayed , imuDataDelayed . time_ms ) ) {
// start performance timer
hal . util - > perf_begin ( _perf_FuseBodyOdom ) ;
2017-07-27 02:01:48 -03:00
usingWheelSensors = false ;
2017-03-16 02:59:19 -03:00
// Fuse data into the main filter
FuseBodyVel ( ) ;
// stop the performance timer
hal . util - > perf_end ( _perf_FuseBodyOdom ) ;
2017-07-27 02:01:48 -03:00
} else if ( storedWheelOdm . recall ( wheelOdmDataDelayed , imuDataDelayed . time_ms ) ) {
// check if the delta time is too small to calculate a velocity
2017-10-27 17:49:28 -03:00
if ( wheelOdmDataDelayed . delTime > EKF_TARGET_DT ) {
2017-07-27 02:01:48 -03:00
// get the forward velocity
2017-10-27 17:49:28 -03:00
float fwdSpd = wheelOdmDataDelayed . delAng * wheelOdmDataDelayed . radius * ( 1.0f / wheelOdmDataDelayed . delTime ) ;
2017-07-27 02:01:48 -03:00
// get the unit vector from the projection of the X axis onto the horizontal
Vector3f unitVec ;
unitVec . x = prevTnb . a . x ;
unitVec . y = prevTnb . a . y ;
unitVec . z = 0.0f ;
2017-10-30 17:18:30 -03:00
unitVec . normalize ( ) ;
2017-07-27 02:01:48 -03:00
// multiply by forward speed to get velocity vector measured by wheel encoders
Vector3f velNED = unitVec * fwdSpd ;
// This is a hack to enable use of the existing body frame velocity fusion method
// TODO write a dedicated observation model for wheel encoders
usingWheelSensors = true ;
bodyOdmDataDelayed . vel = prevTnb * velNED ;
2017-10-27 17:49:28 -03:00
bodyOdmDataDelayed . body_offset = wheelOdmDataDelayed . hub_offset ;
2017-07-27 02:01:48 -03:00
bodyOdmDataDelayed . velErr = frontend - > _wencOdmVelErr ;
// Fuse data into the main filter
FuseBodyVel ( ) ;
}
2017-03-16 02:59:19 -03:00
}
}