2016-07-14 02:08:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF3.h"
2023-02-10 03:46:40 -04:00
2016-07-14 02:08:43 -03:00
# include "AP_NavEKF3_core.h"
2023-02-10 03:46:40 -04:00
# if EK3_FEATURE_OPTFLOW_FUSION
2017-03-16 02:59:19 -03:00
# include <GCS_MAVLink/GCS.h>
2024-08-29 23:26:19 -03:00
# include <AP_DAL/AP_DAL.h>
2016-07-14 02:08:43 -03:00
/********************************************************
* RESET FUNCTIONS *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/********************************************************
* FUSE MEASURED_DATA *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// select fusion of optical flow measurements
void NavEKF3_core : : SelectFlowFusion ( )
{
// 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 & & ! optFlowFusionDelayed ) {
optFlowFusionDelayed = true ;
return ;
} else {
optFlowFusionDelayed = false ;
}
2021-05-27 20:26:57 -03:00
of_elements ofDataDelayed ; // OF data at the fusion time horizon
2020-08-17 22:51:50 -03:00
// Check for data at the fusion time horizon
2020-08-17 23:46:57 -03:00
const bool flowDataToFuse = storedOF . recall ( ofDataDelayed , imuDataDelayed . time_ms ) ;
2020-08-17 22:51:50 -03:00
2016-07-14 02:08:43 -03:00
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ( ( imuSampleTime_ms - flowValidMeaTime_ms ) < 1000 ) ;
2017-04-27 22:04:20 -03:00
// check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0
2020-07-04 00:27:49 -03:00
gndOffsetValid = ( ( imuSampleTime_ms - gndHgtValidTime_ms ) < 5000 ) | | ( activeHgtSource = = AP_NavEKF_Source : : SourceZ : : RANGEFINDER ) ;
2016-07-14 02:08:43 -03:00
// Perform tilt check
bool tiltOK = ( prevTnb . c . z > frontend - > DCM33FlowMin ) ;
// Constrain measurements to zero if takeoff is not detected and the height above ground
2022-01-13 07:45:07 -04:00
// is insufficient to achieve acceptable focus. This allows the vehicle to be picked up
2016-07-14 02:08:43 -03:00
// and carried to test optical flow operation
if ( ! takeOffDetected & & ( ( terrainState - stateStruct . position . z ) < 0.5f ) ) {
ofDataDelayed . flowRadXYcomp . zero ( ) ;
ofDataDelayed . flowRadXY . zero ( ) ;
flowDataValid = true ;
}
2019-02-22 17:11:36 -04:00
// if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
2019-03-14 15:49:33 -03:00
if ( ( ( flowDataToFuse & & ( frontend - > _flowUse = = FLOW_USE_TERRAIN ) ) | | rangeDataToFuse ) & & tiltOK ) {
2016-07-14 02:08:43 -03:00
// Estimate the terrain offset (runs a one state EKF)
2021-05-27 20:26:57 -03:00
EstimateTerrainOffset ( ofDataDelayed ) ;
2016-07-14 02:08:43 -03:00
}
// Fuse optical flow data into the main filter
2019-02-22 17:11:36 -04:00
if ( flowDataToFuse & & tiltOK ) {
2021-08-16 09:41:54 -03:00
const bool fuse_optflow = ( frontend - > _flowUse = = FLOW_USE_NAV ) & & frontend - > sources . useVelXYSource ( AP_NavEKF_Source : : SourceXY : : OPTFLOW ) ;
// Set the flow noise used by the fusion processes
R_LOS = sq ( MAX ( frontend - > _flowNoise , 0.05f ) ) ;
// Fuse the optical flow X and Y axis data into the main filter sequentially
FuseOptFlow ( ofDataDelayed , fuse_optflow ) ;
2016-07-14 02:08:43 -03:00
}
}
/*
Estimation of terrain offset using a single state EKF
2019-02-22 19:35:24 -04:00
The filter can fuse motion compensated optical flow rates and range finder measurements
2019-02-22 17:11:36 -04:00
Equations generated using https : //github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator
2016-07-14 02:08:43 -03:00
*/
2021-05-27 20:26:57 -03:00
void NavEKF3_core : : EstimateTerrainOffset ( const of_elements & ofDataDelayed )
2016-07-14 02:08:43 -03:00
{
2019-02-22 17:11:36 -04:00
// horizontal velocity squared
2021-05-04 08:12:23 -03:00
ftype velHorizSq = sq ( stateStruct . velocity . x ) + sq ( stateStruct . velocity . y ) ;
2016-07-14 02:08:43 -03:00
2019-02-22 17:11:36 -04:00
// don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
// don't fuse flow data if it exceeds validity limits
2019-03-14 16:07:35 -03:00
// don't update terrain offset if ground is being used as the zero height datum in the main filter
2019-03-14 15:49:33 -03:00
bool cantFuseFlowData = ( ( frontend - > _flowUse ! = FLOW_USE_TERRAIN )
2021-09-20 22:47:51 -03:00
| | ! gpsIsInUse
2019-02-22 17:11:36 -04:00
| | PV_AidingMode = = AID_RELATIVE
| | velHorizSq < 25.0f
| | ( MAX ( ofDataDelayed . flowRadXY [ 0 ] , ofDataDelayed . flowRadXY [ 1 ] ) > frontend - > _maxFlowRate ) ) ;
2020-07-04 00:27:49 -03:00
if ( ( ! rangeDataToFuse & & cantFuseFlowData ) | | ( activeHgtSource = = AP_NavEKF_Source : : SourceZ : : RANGEFINDER ) ) {
2016-07-14 02:08:43 -03:00
// skip update
inhibitGndState = true ;
} else {
inhibitGndState = false ;
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
2022-01-13 07:45:07 -04:00
// limit distance to prevent intialisation after bad gps causing bad numerical conditioning
2021-05-04 08:12:23 -03:00
ftype distanceTravelledSq = sq ( stateStruct . position [ 0 ] - prevPosN ) + sq ( stateStruct . position [ 1 ] - prevPosE ) ;
2016-07-14 02:08:43 -03:00
distanceTravelledSq = MIN ( distanceTravelledSq , 100.0f ) ;
prevPosN = stateStruct . position [ 0 ] ;
prevPosE = stateStruct . position [ 1 ] ;
2022-01-13 07:45:07 -04:00
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copter's vertical velocity
2021-05-04 08:12:23 -03:00
ftype timeLapsed = MIN ( 0.001f * ( imuSampleTime_ms - timeAtLastAuxEKF_ms ) , 1.0f ) ;
ftype Pincrement = ( distanceTravelledSq * sq ( frontend - > _terrGradMax ) ) + sq ( timeLapsed ) * P [ 6 ] [ 6 ] ;
2016-07-14 02:08:43 -03:00
Popt + = Pincrement ;
timeAtLastAuxEKF_ms = imuSampleTime_ms ;
// fuse range finder data
if ( rangeDataToFuse ) {
2022-01-26 04:28:04 -04:00
// reset terrain state if rangefinder data not fused for 5 seconds
if ( imuSampleTime_ms - gndHgtValidTime_ms > 5000 ) {
terrainState = MAX ( rangeDataDelayed . rng * prevTnb . c . z , rngOnGnd ) + stateStruct . position . z ;
}
2016-07-14 02:08:43 -03:00
// predict range
2021-05-04 08:12:23 -03:00
ftype predRngMeas = MAX ( ( terrainState - stateStruct . position [ 2 ] ) , rngOnGnd ) / prevTnb . c . z ;
2016-07-14 02:08:43 -03:00
// Copy required states to local variable names
2021-05-04 08:12:23 -03:00
ftype q0 = stateStruct . quat [ 0 ] ; // quaternion at optical flow measurement time
ftype q1 = stateStruct . quat [ 1 ] ; // quaternion at optical flow measurement time
ftype q2 = stateStruct . quat [ 2 ] ; // quaternion at optical flow measurement time
ftype q3 = stateStruct . quat [ 3 ] ; // quaternion at optical flow measurement time
2016-07-14 02:08:43 -03:00
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
2021-05-04 08:12:23 -03:00
ftype R_RNG = frontend - > _rngNoise ;
2016-07-14 02:08:43 -03:00
// calculate Kalman gain
2021-05-04 08:12:23 -03:00
ftype SK_RNG = sq ( q0 ) - sq ( q1 ) - sq ( q2 ) + sq ( q3 ) ;
ftype K_RNG = Popt / ( SK_RNG * ( R_RNG + Popt / sq ( SK_RNG ) ) ) ;
2016-07-14 02:08:43 -03:00
// Calculate the innovation variance for data logging
varInnovRng = ( R_RNG + Popt / sq ( SK_RNG ) ) ;
// constrain terrain height to be below the vehicle
terrainState = MAX ( terrainState , stateStruct . position [ 2 ] + rngOnGnd ) ;
// Calculate the measurement innovation
innovRng = predRngMeas - rangeDataDelayed . rng ;
// calculate the innovation consistency test ratio
2021-05-04 08:12:23 -03:00
auxRngTestRatio = sq ( innovRng ) / ( sq ( MAX ( 0.01f * ( ftype ) frontend - > _rngInnovGate , 1.0f ) ) * varInnovRng ) ;
2016-07-14 02:08:43 -03:00
2017-06-19 20:40:25 -03:00
// Check the innovation test ratio and don't fuse if too large
if ( auxRngTestRatio < 1.0f ) {
2016-07-14 02:08:43 -03:00
// correct the state
terrainState - = K_RNG * innovRng ;
// constrain the state
terrainState = MAX ( terrainState , stateStruct . position [ 2 ] + rngOnGnd ) ;
// correct the covariance
Popt = Popt - sq ( Popt ) / ( SK_RNG * ( R_RNG + Popt / sq ( SK_RNG ) ) * ( sq ( q0 ) - sq ( q1 ) - sq ( q2 ) + sq ( q3 ) ) ) ;
// prevent the state variance from becoming negative
Popt = MAX ( Popt , 0.0f ) ;
2022-01-26 04:28:04 -04:00
// record the time we last updated the terrain offset state
gndHgtValidTime_ms = imuSampleTime_ms ;
2016-07-14 02:08:43 -03:00
}
}
2019-02-22 17:11:36 -04:00
if ( ! cantFuseFlowData ) {
2016-07-14 02:08:43 -03:00
2021-05-04 08:12:23 -03:00
Vector3F relVelSensor ; // velocity of sensor relative to ground in sensor axes
Vector2F losPred ; // predicted optical flow angular rate measurement
ftype q0 = stateStruct . quat [ 0 ] ; // quaternion at optical flow measurement time
ftype q1 = stateStruct . quat [ 1 ] ; // quaternion at optical flow measurement time
ftype q2 = stateStruct . quat [ 2 ] ; // quaternion at optical flow measurement time
ftype q3 = stateStruct . quat [ 3 ] ; // quaternion at optical flow measurement time
ftype K_OPT ;
ftype H_OPT ;
Vector2F auxFlowObsInnovVar ;
2016-07-14 02:08:43 -03:00
// predict range to centre of image
2021-05-04 08:12:23 -03:00
ftype flowRngPred = MAX ( ( terrainState - stateStruct . position . z ) , rngOnGnd ) / prevTnb . c . z ;
2016-07-14 02:08:43 -03:00
// constrain terrain height to be below the vehicle
2019-02-22 17:11:36 -04:00
terrainState = MAX ( terrainState , stateStruct . position . z + rngOnGnd ) ;
2016-07-14 02:08:43 -03:00
// calculate relative velocity in sensor frame
relVelSensor = prevTnb * stateStruct . velocity ;
// divide velocity by range, subtract body rates and apply scale factor to
// get predicted sensed angular optical rates relative to X and Y sensor axes
2019-02-22 17:11:36 -04:00
losPred . x = relVelSensor . y / flowRngPred ;
losPred . y = - relVelSensor . x / flowRngPred ;
2016-07-14 02:08:43 -03:00
// calculate innovations
2019-02-22 17:11:36 -04:00
auxFlowObsInnov = losPred - ofDataDelayed . flowRadXYcomp ;
// calculate observation jacobians
2021-05-04 08:12:23 -03:00
ftype t2 = q0 * q0 ;
ftype t3 = q1 * q1 ;
ftype t4 = q2 * q2 ;
ftype t5 = q3 * q3 ;
ftype t6 = stateStruct . position . z - terrainState ;
ftype t7 = 1.0f / ( t6 * t6 ) ;
ftype t8 = q0 * q3 * 2.0f ;
ftype t9 = t2 - t3 - t4 + t5 ;
2019-02-22 17:11:36 -04:00
// prevent the state variances from becoming badly conditioned
Popt = MAX ( Popt , 1E-6 f ) ;
// calculate observation noise variance from parameter
2021-05-04 08:12:23 -03:00
ftype flow_noise_variance = sq ( MAX ( frontend - > _flowNoise , 0.05f ) ) ;
2019-02-22 17:11:36 -04:00
// Fuse Y axis data
// Calculate observation partial derivative
H_OPT = t7 * t9 * ( - stateStruct . velocity . z * ( q0 * q2 * 2.0 - q1 * q3 * 2.0 ) + stateStruct . velocity . x * ( t2 + t3 - t4 - t5 ) + stateStruct . velocity . y * ( t8 + q1 * q2 * 2.0 ) ) ;
// calculate innovation variance
auxFlowObsInnovVar . y = H_OPT * Popt * H_OPT + flow_noise_variance ;
// calculate Kalman gain
K_OPT = Popt * H_OPT / auxFlowObsInnovVar . y ;
// calculate the innovation consistency test ratio
2021-05-04 08:12:23 -03:00
auxFlowTestRatio . y = sq ( auxFlowObsInnov . y ) / ( sq ( MAX ( 0.01f * ( ftype ) frontend - > _flowInnovGate , 1.0f ) ) * auxFlowObsInnovVar . y ) ;
2019-02-22 17:11:36 -04:00
// don't fuse if optical flow data is outside valid range
if ( auxFlowTestRatio . y < 1.0f ) {
// correct the state
terrainState - = K_OPT * auxFlowObsInnov . y ;
// constrain the state
terrainState = MAX ( terrainState , stateStruct . position . z + rngOnGnd ) ;
// update intermediate variables used when fusing the X axis
t6 = stateStruct . position . z - terrainState ;
t7 = 1.0f / ( t6 * t6 ) ;
// correct the covariance
Popt = Popt - K_OPT * H_OPT * Popt ;
// prevent the state variances from becoming badly conditioned
Popt = MAX ( Popt , 1E-6 f ) ;
2022-01-26 04:28:04 -04:00
// record the time we last updated the terrain offset state
gndHgtValidTime_ms = imuSampleTime_ms ;
2019-02-22 17:11:36 -04:00
}
// fuse X axis data
H_OPT = - t7 * t9 * ( stateStruct . velocity . z * ( q0 * q1 * 2.0 + q2 * q3 * 2.0 ) + stateStruct . velocity . y * ( t2 - t3 + t4 - t5 ) - stateStruct . velocity . x * ( t8 - q1 * q2 * 2.0 ) ) ;
2016-07-14 02:08:43 -03:00
// calculate innovation variances
2019-02-22 17:11:36 -04:00
auxFlowObsInnovVar . x = H_OPT * Popt * H_OPT + flow_noise_variance ;
2016-07-14 02:08:43 -03:00
// calculate Kalman gain
2019-02-22 17:11:36 -04:00
K_OPT = Popt * H_OPT / auxFlowObsInnovVar . x ;
2016-07-14 02:08:43 -03:00
// calculate the innovation consistency test ratio
2021-05-04 08:12:23 -03:00
auxFlowTestRatio . x = sq ( auxFlowObsInnov . x ) / ( sq ( MAX ( 0.01f * ( ftype ) frontend - > _flowInnovGate , 1.0f ) ) * auxFlowObsInnovVar . x ) ;
2016-07-14 02:08:43 -03:00
// don't fuse if optical flow data is outside valid range
2019-02-22 17:11:36 -04:00
if ( auxFlowTestRatio . x < 1.0f ) {
2016-07-14 02:08:43 -03:00
// correct the state
2019-02-22 17:11:36 -04:00
terrainState - = K_OPT * auxFlowObsInnov . x ;
2016-07-14 02:08:43 -03:00
// constrain the state
2019-02-22 17:11:36 -04:00
terrainState = MAX ( terrainState , stateStruct . position . z + rngOnGnd ) ;
2016-07-14 02:08:43 -03:00
// correct the covariance
Popt = Popt - K_OPT * H_OPT * Popt ;
2019-02-22 17:11:36 -04:00
// prevent the state variances from becoming badly conditioned
Popt = MAX ( Popt , 1E-6 f ) ;
2016-07-14 02:08:43 -03:00
}
}
}
}
/*
* Fuse angular motion compensated optical flow rates 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 :
2017-04-10 02:24:45 -03:00
* https : //github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
2016-07-14 02:08:43 -03:00
* Requires a valid terrain height estimate .
2021-08-16 09:41:54 -03:00
*
* really_fuse should be true to actually fuse into the main filter , false to only calculate variances
2016-07-14 02:08:43 -03:00
*/
2021-08-16 09:41:54 -03:00
void NavEKF3_core : : FuseOptFlow ( const of_elements & ofDataDelayed , bool really_fuse )
2016-07-14 02:08:43 -03:00
{
Vector24 H_LOS ;
Vector2 losPred ;
// Copy required states to local variable names
2021-05-04 08:12:23 -03:00
ftype q0 = stateStruct . quat [ 0 ] ;
ftype q1 = stateStruct . quat [ 1 ] ;
ftype q2 = stateStruct . quat [ 2 ] ;
ftype q3 = stateStruct . quat [ 3 ] ;
ftype vn = stateStruct . velocity . x ;
ftype ve = stateStruct . velocity . y ;
ftype vd = stateStruct . velocity . z ;
ftype pd = stateStruct . position . z ;
2016-07-14 02:08:43 -03:00
// constrain height above ground to be above range measured on ground
2021-05-04 08:12:23 -03:00
ftype heightAboveGndEst = MAX ( ( terrainState - pd ) , rngOnGnd ) ;
2016-07-14 02:08:43 -03:00
2022-01-17 02:48:50 -04:00
// calculate range from ground plain to centre of sensor fov assuming flat earth
ftype range = constrain_ftype ( ( heightAboveGndEst / prevTnb . c . z ) , rngOnGnd , 1000.0f ) ;
// correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain
Vector3F posOffsetBody = ofDataDelayed . body_offset - accelPosOffset ;
if ( ! posOffsetBody . is_zero ( ) ) {
Vector3F posOffsetEarth = prevTnb . mul_transpose ( posOffsetBody ) ;
range - = posOffsetEarth . z / prevTnb . c . z ;
}
2022-10-24 00:01:08 -03:00
# if APM_BUILD_TYPE(APM_BUILD_Rover)
// override with user specified height (if given, for rover)
if ( ofDataDelayed . heightOverride > 0 ) {
range = ofDataDelayed . heightOverride ;
}
# endif
2022-01-17 02:48:50 -04:00
2016-07-14 02:08:43 -03:00
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
for ( uint8_t obsIndex = 0 ; obsIndex < = 1 ; obsIndex + + ) { // fuse X axis data first
// calculate relative velocity in sensor frame including the relative motion due to rotation
2022-01-17 02:48:50 -04:00
const Vector3F relVelSensor = ( prevTnb * stateStruct . velocity ) + ( ofDataDelayed . bodyRadXYZ % posOffsetBody ) ;
2016-07-14 02:08:43 -03:00
2019-02-22 19:35:24 -04:00
// divide velocity by range to get predicted angular LOS rates relative to X and Y axes
2016-07-14 02:08:43 -03:00
losPred [ 0 ] = relVelSensor . y / range ;
losPred [ 1 ] = - relVelSensor . x / range ;
// calculate observation jacobians and Kalman gains
memset ( & H_LOS [ 0 ] , 0 , sizeof ( H_LOS ) ) ;
if ( obsIndex = = 0 ) {
// calculate X axis observation Jacobian
2021-05-04 08:12:23 -03:00
ftype t2 = 1.0f / range ;
2016-07-14 02:08:43 -03:00
H_LOS [ 0 ] = t2 * ( q1 * vd * 2.0f + q0 * ve * 2.0f - q3 * vn * 2.0f ) ;
H_LOS [ 1 ] = t2 * ( q0 * vd * 2.0f - q1 * ve * 2.0f + q2 * vn * 2.0f ) ;
H_LOS [ 2 ] = t2 * ( q3 * vd * 2.0f + q2 * ve * 2.0f + q1 * vn * 2.0f ) ;
H_LOS [ 3 ] = - t2 * ( q2 * vd * - 2.0f + q3 * ve * 2.0f + q0 * vn * 2.0f ) ;
H_LOS [ 4 ] = - t2 * ( q0 * q3 * 2.0f - q1 * q2 * 2.0f ) ;
H_LOS [ 5 ] = t2 * ( q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3 ) ;
H_LOS [ 6 ] = t2 * ( q0 * q1 * 2.0f + q2 * q3 * 2.0f ) ;
2019-02-22 19:35:24 -04:00
// calculate intermediate variables for the X observation innovation variance and Kalman gains
2021-05-04 08:12:23 -03:00
ftype t3 = q1 * vd * 2.0f ;
ftype t4 = q0 * ve * 2.0f ;
ftype t11 = q3 * vn * 2.0f ;
ftype t5 = t3 + t4 - t11 ;
ftype t6 = q0 * q3 * 2.0f ;
ftype t29 = q1 * q2 * 2.0f ;
ftype t7 = t6 - t29 ;
ftype t8 = q0 * q1 * 2.0f ;
ftype t9 = q2 * q3 * 2.0f ;
ftype t10 = t8 + t9 ;
ftype t12 = P [ 0 ] [ 0 ] * t2 * t5 ;
ftype t13 = q0 * vd * 2.0f ;
ftype t14 = q2 * vn * 2.0f ;
ftype t28 = q1 * ve * 2.0f ;
ftype t15 = t13 + t14 - t28 ;
ftype t16 = q3 * vd * 2.0f ;
ftype t17 = q2 * ve * 2.0f ;
ftype t18 = q1 * vn * 2.0f ;
ftype t19 = t16 + t17 + t18 ;
ftype t20 = q3 * ve * 2.0f ;
ftype t21 = q0 * vn * 2.0f ;
ftype t30 = q2 * vd * 2.0f ;
ftype t22 = t20 + t21 - t30 ;
ftype t23 = q0 * q0 ;
ftype t24 = q1 * q1 ;
ftype t25 = q2 * q2 ;
ftype t26 = q3 * q3 ;
ftype t27 = t23 - t24 + t25 - t26 ;
ftype t31 = P [ 1 ] [ 1 ] * t2 * t15 ;
ftype t32 = P [ 6 ] [ 0 ] * t2 * t10 ;
ftype t33 = P [ 1 ] [ 0 ] * t2 * t15 ;
ftype t34 = P [ 2 ] [ 0 ] * t2 * t19 ;
ftype t35 = P [ 5 ] [ 0 ] * t2 * t27 ;
ftype t79 = P [ 4 ] [ 0 ] * t2 * t7 ;
ftype t80 = P [ 3 ] [ 0 ] * t2 * t22 ;
ftype t36 = t12 + t32 + t33 + t34 + t35 - t79 - t80 ;
ftype t37 = t2 * t5 * t36 ;
ftype t38 = P [ 6 ] [ 1 ] * t2 * t10 ;
ftype t39 = P [ 0 ] [ 1 ] * t2 * t5 ;
ftype t40 = P [ 2 ] [ 1 ] * t2 * t19 ;
ftype t41 = P [ 5 ] [ 1 ] * t2 * t27 ;
ftype t81 = P [ 4 ] [ 1 ] * t2 * t7 ;
ftype t82 = P [ 3 ] [ 1 ] * t2 * t22 ;
ftype t42 = t31 + t38 + t39 + t40 + t41 - t81 - t82 ;
ftype t43 = t2 * t15 * t42 ;
ftype t44 = P [ 6 ] [ 2 ] * t2 * t10 ;
ftype t45 = P [ 0 ] [ 2 ] * t2 * t5 ;
ftype t46 = P [ 1 ] [ 2 ] * t2 * t15 ;
ftype t47 = P [ 2 ] [ 2 ] * t2 * t19 ;
ftype t48 = P [ 5 ] [ 2 ] * t2 * t27 ;
ftype t83 = P [ 4 ] [ 2 ] * t2 * t7 ;
ftype t84 = P [ 3 ] [ 2 ] * t2 * t22 ;
ftype t49 = t44 + t45 + t46 + t47 + t48 - t83 - t84 ;
ftype t50 = t2 * t19 * t49 ;
ftype t51 = P [ 6 ] [ 3 ] * t2 * t10 ;
ftype t52 = P [ 0 ] [ 3 ] * t2 * t5 ;
ftype t53 = P [ 1 ] [ 3 ] * t2 * t15 ;
ftype t54 = P [ 2 ] [ 3 ] * t2 * t19 ;
ftype t55 = P [ 5 ] [ 3 ] * t2 * t27 ;
ftype t85 = P [ 4 ] [ 3 ] * t2 * t7 ;
ftype t86 = P [ 3 ] [ 3 ] * t2 * t22 ;
ftype t56 = t51 + t52 + t53 + t54 + t55 - t85 - t86 ;
ftype t57 = P [ 6 ] [ 5 ] * t2 * t10 ;
ftype t58 = P [ 0 ] [ 5 ] * t2 * t5 ;
ftype t59 = P [ 1 ] [ 5 ] * t2 * t15 ;
ftype t60 = P [ 2 ] [ 5 ] * t2 * t19 ;
ftype t61 = P [ 5 ] [ 5 ] * t2 * t27 ;
ftype t88 = P [ 4 ] [ 5 ] * t2 * t7 ;
ftype t89 = P [ 3 ] [ 5 ] * t2 * t22 ;
ftype t62 = t57 + t58 + t59 + t60 + t61 - t88 - t89 ;
ftype t63 = t2 * t27 * t62 ;
ftype t64 = P [ 6 ] [ 4 ] * t2 * t10 ;
ftype t65 = P [ 0 ] [ 4 ] * t2 * t5 ;
ftype t66 = P [ 1 ] [ 4 ] * t2 * t15 ;
ftype t67 = P [ 2 ] [ 4 ] * t2 * t19 ;
ftype t68 = P [ 5 ] [ 4 ] * t2 * t27 ;
ftype t90 = P [ 4 ] [ 4 ] * t2 * t7 ;
ftype t91 = P [ 3 ] [ 4 ] * t2 * t22 ;
ftype t69 = t64 + t65 + t66 + t67 + t68 - t90 - t91 ;
ftype t70 = P [ 6 ] [ 6 ] * t2 * t10 ;
ftype t71 = P [ 0 ] [ 6 ] * t2 * t5 ;
ftype t72 = P [ 1 ] [ 6 ] * t2 * t15 ;
ftype t73 = P [ 2 ] [ 6 ] * t2 * t19 ;
ftype t74 = P [ 5 ] [ 6 ] * t2 * t27 ;
ftype t93 = P [ 4 ] [ 6 ] * t2 * t7 ;
ftype t94 = P [ 3 ] [ 6 ] * t2 * t22 ;
ftype t75 = t70 + t71 + t72 + t73 + t74 - t93 - t94 ;
ftype t76 = t2 * t10 * t75 ;
ftype t87 = t2 * t22 * t56 ;
ftype t92 = t2 * t7 * t69 ;
ftype t77 = R_LOS + t37 + t43 + t50 + t63 + t76 - t87 - t92 ;
ftype t78 ;
2016-07-14 02:08:43 -03:00
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if ( t77 > R_LOS ) {
t78 = 1.0f / t77 ;
faultStatus . bad_xflow = false ;
} else {
t77 = R_LOS ;
t78 = 1.0f / R_LOS ;
faultStatus . bad_xflow = true ;
return ;
}
2021-08-16 10:00:52 -03:00
flowVarInnov [ 0 ] = t77 ;
2016-07-14 02:08:43 -03:00
// calculate innovation for X axis observation
2021-08-16 09:41:54 -03:00
// flowInnovTime_ms will be updated when Y-axis innovations are calculated
2021-08-16 10:00:52 -03:00
flowInnov [ 0 ] = losPred [ 0 ] - ofDataDelayed . flowRadXYcomp . x ;
2016-07-14 02:08:43 -03:00
// calculate Kalman gains for X-axis observation
Kfusion [ 0 ] = t78 * ( t12 - P [ 0 ] [ 4 ] * t2 * t7 + P [ 0 ] [ 1 ] * t2 * t15 + P [ 0 ] [ 6 ] * t2 * t10 + P [ 0 ] [ 2 ] * t2 * t19 - P [ 0 ] [ 3 ] * t2 * t22 + P [ 0 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 1 ] = t78 * ( t31 + P [ 1 ] [ 0 ] * t2 * t5 - P [ 1 ] [ 4 ] * t2 * t7 + P [ 1 ] [ 6 ] * t2 * t10 + P [ 1 ] [ 2 ] * t2 * t19 - P [ 1 ] [ 3 ] * t2 * t22 + P [ 1 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 2 ] = t78 * ( t47 + P [ 2 ] [ 0 ] * t2 * t5 - P [ 2 ] [ 4 ] * t2 * t7 + P [ 2 ] [ 1 ] * t2 * t15 + P [ 2 ] [ 6 ] * t2 * t10 - P [ 2 ] [ 3 ] * t2 * t22 + P [ 2 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 3 ] = t78 * ( - t86 + P [ 3 ] [ 0 ] * t2 * t5 - P [ 3 ] [ 4 ] * t2 * t7 + P [ 3 ] [ 1 ] * t2 * t15 + P [ 3 ] [ 6 ] * t2 * t10 + P [ 3 ] [ 2 ] * t2 * t19 + P [ 3 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 4 ] = t78 * ( - t90 + P [ 4 ] [ 0 ] * t2 * t5 + P [ 4 ] [ 1 ] * t2 * t15 + P [ 4 ] [ 6 ] * t2 * t10 + P [ 4 ] [ 2 ] * t2 * t19 - P [ 4 ] [ 3 ] * t2 * t22 + P [ 4 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 5 ] = t78 * ( t61 + P [ 5 ] [ 0 ] * t2 * t5 - P [ 5 ] [ 4 ] * t2 * t7 + P [ 5 ] [ 1 ] * t2 * t15 + P [ 5 ] [ 6 ] * t2 * t10 + P [ 5 ] [ 2 ] * t2 * t19 - P [ 5 ] [ 3 ] * t2 * t22 ) ;
Kfusion [ 6 ] = t78 * ( t70 + P [ 6 ] [ 0 ] * t2 * t5 - P [ 6 ] [ 4 ] * t2 * t7 + P [ 6 ] [ 1 ] * t2 * t15 + P [ 6 ] [ 2 ] * t2 * t19 - P [ 6 ] [ 3 ] * t2 * t22 + P [ 6 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 7 ] = t78 * ( P [ 7 ] [ 0 ] * t2 * t5 - P [ 7 ] [ 4 ] * t2 * t7 + P [ 7 ] [ 1 ] * t2 * t15 + P [ 7 ] [ 6 ] * t2 * t10 + P [ 7 ] [ 2 ] * t2 * t19 - P [ 7 ] [ 3 ] * t2 * t22 + P [ 7 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 8 ] = t78 * ( P [ 8 ] [ 0 ] * t2 * t5 - P [ 8 ] [ 4 ] * t2 * t7 + P [ 8 ] [ 1 ] * t2 * t15 + P [ 8 ] [ 6 ] * t2 * t10 + P [ 8 ] [ 2 ] * t2 * t19 - P [ 8 ] [ 3 ] * t2 * t22 + P [ 8 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 9 ] = t78 * ( P [ 9 ] [ 0 ] * t2 * t5 - P [ 9 ] [ 4 ] * t2 * t7 + P [ 9 ] [ 1 ] * t2 * t15 + P [ 9 ] [ 6 ] * t2 * t10 + P [ 9 ] [ 2 ] * t2 * t19 - P [ 9 ] [ 3 ] * t2 * t22 + P [ 9 ] [ 5 ] * t2 * t27 ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = t78 * ( P [ 10 ] [ 0 ] * t2 * t5 - P [ 10 ] [ 4 ] * t2 * t7 + P [ 10 ] [ 1 ] * t2 * t15 + P [ 10 ] [ 6 ] * t2 * t10 + P [ 10 ] [ 2 ] * t2 * t19 - P [ 10 ] [ 3 ] * t2 * t22 + P [ 10 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 11 ] = t78 * ( P [ 11 ] [ 0 ] * t2 * t5 - P [ 11 ] [ 4 ] * t2 * t7 + P [ 11 ] [ 1 ] * t2 * t15 + P [ 11 ] [ 6 ] * t2 * t10 + P [ 11 ] [ 2 ] * t2 * t19 - P [ 11 ] [ 3 ] * t2 * t22 + P [ 11 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 12 ] = t78 * ( P [ 12 ] [ 0 ] * t2 * t5 - P [ 12 ] [ 4 ] * t2 * t7 + P [ 12 ] [ 1 ] * t2 * t15 + P [ 12 ] [ 6 ] * t2 * t10 + P [ 12 ] [ 2 ] * t2 * t19 - P [ 12 ] [ 3 ] * t2 * t22 + P [ 12 ] [ 5 ] * t2 * t27 ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 10 to 12
zero_range ( & Kfusion [ 0 ] , 10 , 12 ) ;
2017-05-09 19:31:55 -03:00
}
2021-07-15 18:53:49 -03:00
if ( ! inhibitDelVelBiasStates & & ! badIMUdata ) {
2021-03-08 02:09:02 -04:00
for ( uint8_t index = 0 ; index < 3 ; index + + ) {
2021-03-22 05:47:53 -03:00
const uint8_t stateIndex = index + 13 ;
2021-03-08 02:09:02 -04:00
if ( ! dvelBiasAxisInhibit [ index ] ) {
Kfusion [ stateIndex ] = t78 * ( P [ stateIndex ] [ 0 ] * t2 * t5 - P [ stateIndex ] [ 4 ] * t2 * t7 + P [ stateIndex ] [ 1 ] * t2 * t15 + P [ stateIndex ] [ 6 ] * t2 * t10 + P [ stateIndex ] [ 2 ] * t2 * t19 - P [ stateIndex ] [ 3 ] * t2 * t22 + P [ stateIndex ] [ 5 ] * t2 * t27 ) ;
} else {
Kfusion [ stateIndex ] = 0.0f ;
}
}
2016-07-14 02:08:43 -03:00
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 13 to 15
zero_range ( & Kfusion [ 0 ] , 13 , 15 ) ;
2016-07-14 02:08:43 -03:00
}
2017-05-09 19:31:55 -03:00
2016-07-14 02:08:43 -03:00
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = t78 * ( P [ 16 ] [ 0 ] * t2 * t5 - P [ 16 ] [ 4 ] * t2 * t7 + P [ 16 ] [ 1 ] * t2 * t15 + P [ 16 ] [ 6 ] * t2 * t10 + P [ 16 ] [ 2 ] * t2 * t19 - P [ 16 ] [ 3 ] * t2 * t22 + P [ 16 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 17 ] = t78 * ( P [ 17 ] [ 0 ] * t2 * t5 - P [ 17 ] [ 4 ] * t2 * t7 + P [ 17 ] [ 1 ] * t2 * t15 + P [ 17 ] [ 6 ] * t2 * t10 + P [ 17 ] [ 2 ] * t2 * t19 - P [ 17 ] [ 3 ] * t2 * t22 + P [ 17 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 18 ] = t78 * ( P [ 18 ] [ 0 ] * t2 * t5 - P [ 18 ] [ 4 ] * t2 * t7 + P [ 18 ] [ 1 ] * t2 * t15 + P [ 18 ] [ 6 ] * t2 * t10 + P [ 18 ] [ 2 ] * t2 * t19 - P [ 18 ] [ 3 ] * t2 * t22 + P [ 18 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 19 ] = t78 * ( P [ 19 ] [ 0 ] * t2 * t5 - P [ 19 ] [ 4 ] * t2 * t7 + P [ 19 ] [ 1 ] * t2 * t15 + P [ 19 ] [ 6 ] * t2 * t10 + P [ 19 ] [ 2 ] * t2 * t19 - P [ 19 ] [ 3 ] * t2 * t22 + P [ 19 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 20 ] = t78 * ( P [ 20 ] [ 0 ] * t2 * t5 - P [ 20 ] [ 4 ] * t2 * t7 + P [ 20 ] [ 1 ] * t2 * t15 + P [ 20 ] [ 6 ] * t2 * t10 + P [ 20 ] [ 2 ] * t2 * t19 - P [ 20 ] [ 3 ] * t2 * t22 + P [ 20 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 21 ] = t78 * ( P [ 21 ] [ 0 ] * t2 * t5 - P [ 21 ] [ 4 ] * t2 * t7 + P [ 21 ] [ 1 ] * t2 * t15 + P [ 21 ] [ 6 ] * t2 * t10 + P [ 21 ] [ 2 ] * t2 * t19 - P [ 21 ] [ 3 ] * t2 * t22 + P [ 21 ] [ 5 ] * t2 * t27 ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 16 to 21
zero_range ( & Kfusion [ 0 ] , 16 , 21 ) ;
2017-05-09 19:31:55 -03:00
}
2023-09-26 19:18:07 -03:00
if ( ! inhibitWindStates & & ! treatWindStatesAsTruth ) {
2017-05-09 19:31:55 -03:00
Kfusion [ 22 ] = t78 * ( P [ 22 ] [ 0 ] * t2 * t5 - P [ 22 ] [ 4 ] * t2 * t7 + P [ 22 ] [ 1 ] * t2 * t15 + P [ 22 ] [ 6 ] * t2 * t10 + P [ 22 ] [ 2 ] * t2 * t19 - P [ 22 ] [ 3 ] * t2 * t22 + P [ 22 ] [ 5 ] * t2 * t27 ) ;
Kfusion [ 23 ] = t78 * ( P [ 23 ] [ 0 ] * t2 * t5 - P [ 23 ] [ 4 ] * t2 * t7 + P [ 23 ] [ 1 ] * t2 * t15 + P [ 23 ] [ 6 ] * t2 * t10 + P [ 23 ] [ 2 ] * t2 * t19 - P [ 23 ] [ 3 ] * t2 * t22 + P [ 23 ] [ 5 ] * t2 * t27 ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 22 to 23
zero_range ( & Kfusion [ 0 ] , 22 , 23 ) ;
2016-07-14 02:08:43 -03:00
}
} else {
// calculate Y axis observation Jacobian
2021-05-04 08:12:23 -03:00
ftype t2 = 1.0f / range ;
2016-07-14 02:08:43 -03:00
H_LOS [ 0 ] = - t2 * ( q2 * vd * - 2.0f + q3 * ve * 2.0f + q0 * vn * 2.0f ) ;
H_LOS [ 1 ] = - t2 * ( q3 * vd * 2.0f + q2 * ve * 2.0f + q1 * vn * 2.0f ) ;
H_LOS [ 2 ] = t2 * ( q0 * vd * 2.0f - q1 * ve * 2.0f + q2 * vn * 2.0f ) ;
H_LOS [ 3 ] = - t2 * ( q1 * vd * 2.0f + q0 * ve * 2.0f - q3 * vn * 2.0f ) ;
H_LOS [ 4 ] = - t2 * ( q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3 ) ;
H_LOS [ 5 ] = - t2 * ( q0 * q3 * 2.0f + q1 * q2 * 2.0f ) ;
H_LOS [ 6 ] = t2 * ( q0 * q2 * 2.0f - q1 * q3 * 2.0f ) ;
2019-02-22 19:35:24 -04:00
// calculate intermediate variables for the Y observation innovation variance and Kalman gains
2021-05-04 08:12:23 -03:00
ftype t3 = q3 * ve * 2.0f ;
ftype t4 = q0 * vn * 2.0f ;
ftype t11 = q2 * vd * 2.0f ;
ftype t5 = t3 + t4 - t11 ;
ftype t6 = q0 * q3 * 2.0f ;
ftype t7 = q1 * q2 * 2.0f ;
ftype t8 = t6 + t7 ;
ftype t9 = q0 * q2 * 2.0f ;
ftype t28 = q1 * q3 * 2.0f ;
ftype t10 = t9 - t28 ;
ftype t12 = P [ 0 ] [ 0 ] * t2 * t5 ;
ftype t13 = q3 * vd * 2.0f ;
ftype t14 = q2 * ve * 2.0f ;
ftype t15 = q1 * vn * 2.0f ;
ftype t16 = t13 + t14 + t15 ;
ftype t17 = q0 * vd * 2.0f ;
ftype t18 = q2 * vn * 2.0f ;
ftype t29 = q1 * ve * 2.0f ;
ftype t19 = t17 + t18 - t29 ;
ftype t20 = q1 * vd * 2.0f ;
ftype t21 = q0 * ve * 2.0f ;
ftype t30 = q3 * vn * 2.0f ;
ftype t22 = t20 + t21 - t30 ;
ftype t23 = q0 * q0 ;
ftype t24 = q1 * q1 ;
ftype t25 = q2 * q2 ;
ftype t26 = q3 * q3 ;
ftype t27 = t23 + t24 - t25 - t26 ;
ftype t31 = P [ 1 ] [ 1 ] * t2 * t16 ;
ftype t32 = P [ 5 ] [ 0 ] * t2 * t8 ;
ftype t33 = P [ 1 ] [ 0 ] * t2 * t16 ;
ftype t34 = P [ 3 ] [ 0 ] * t2 * t22 ;
ftype t35 = P [ 4 ] [ 0 ] * t2 * t27 ;
ftype t80 = P [ 6 ] [ 0 ] * t2 * t10 ;
ftype t81 = P [ 2 ] [ 0 ] * t2 * t19 ;
ftype t36 = t12 + t32 + t33 + t34 + t35 - t80 - t81 ;
ftype t37 = t2 * t5 * t36 ;
ftype t38 = P [ 5 ] [ 1 ] * t2 * t8 ;
ftype t39 = P [ 0 ] [ 1 ] * t2 * t5 ;
ftype t40 = P [ 3 ] [ 1 ] * t2 * t22 ;
ftype t41 = P [ 4 ] [ 1 ] * t2 * t27 ;
ftype t82 = P [ 6 ] [ 1 ] * t2 * t10 ;
ftype t83 = P [ 2 ] [ 1 ] * t2 * t19 ;
ftype t42 = t31 + t38 + t39 + t40 + t41 - t82 - t83 ;
ftype t43 = t2 * t16 * t42 ;
ftype t44 = P [ 5 ] [ 2 ] * t2 * t8 ;
ftype t45 = P [ 0 ] [ 2 ] * t2 * t5 ;
ftype t46 = P [ 1 ] [ 2 ] * t2 * t16 ;
ftype t47 = P [ 3 ] [ 2 ] * t2 * t22 ;
ftype t48 = P [ 4 ] [ 2 ] * t2 * t27 ;
ftype t79 = P [ 2 ] [ 2 ] * t2 * t19 ;
ftype t84 = P [ 6 ] [ 2 ] * t2 * t10 ;
ftype t49 = t44 + t45 + t46 + t47 + t48 - t79 - t84 ;
ftype t50 = P [ 5 ] [ 3 ] * t2 * t8 ;
ftype t51 = P [ 0 ] [ 3 ] * t2 * t5 ;
ftype t52 = P [ 1 ] [ 3 ] * t2 * t16 ;
ftype t53 = P [ 3 ] [ 3 ] * t2 * t22 ;
ftype t54 = P [ 4 ] [ 3 ] * t2 * t27 ;
ftype t86 = P [ 6 ] [ 3 ] * t2 * t10 ;
ftype t87 = P [ 2 ] [ 3 ] * t2 * t19 ;
ftype t55 = t50 + t51 + t52 + t53 + t54 - t86 - t87 ;
ftype t56 = t2 * t22 * t55 ;
ftype t57 = P [ 5 ] [ 4 ] * t2 * t8 ;
ftype t58 = P [ 0 ] [ 4 ] * t2 * t5 ;
ftype t59 = P [ 1 ] [ 4 ] * t2 * t16 ;
ftype t60 = P [ 3 ] [ 4 ] * t2 * t22 ;
ftype t61 = P [ 4 ] [ 4 ] * t2 * t27 ;
ftype t88 = P [ 6 ] [ 4 ] * t2 * t10 ;
ftype t89 = P [ 2 ] [ 4 ] * t2 * t19 ;
ftype t62 = t57 + t58 + t59 + t60 + t61 - t88 - t89 ;
ftype t63 = t2 * t27 * t62 ;
ftype t64 = P [ 5 ] [ 5 ] * t2 * t8 ;
ftype t65 = P [ 0 ] [ 5 ] * t2 * t5 ;
ftype t66 = P [ 1 ] [ 5 ] * t2 * t16 ;
ftype t67 = P [ 3 ] [ 5 ] * t2 * t22 ;
ftype t68 = P [ 4 ] [ 5 ] * t2 * t27 ;
ftype t90 = P [ 6 ] [ 5 ] * t2 * t10 ;
ftype t91 = P [ 2 ] [ 5 ] * t2 * t19 ;
ftype t69 = t64 + t65 + t66 + t67 + t68 - t90 - t91 ;
ftype t70 = t2 * t8 * t69 ;
ftype t71 = P [ 5 ] [ 6 ] * t2 * t8 ;
ftype t72 = P [ 0 ] [ 6 ] * t2 * t5 ;
ftype t73 = P [ 1 ] [ 6 ] * t2 * t16 ;
ftype t74 = P [ 3 ] [ 6 ] * t2 * t22 ;
ftype t75 = P [ 4 ] [ 6 ] * t2 * t27 ;
ftype t92 = P [ 6 ] [ 6 ] * t2 * t10 ;
ftype t93 = P [ 2 ] [ 6 ] * t2 * t19 ;
ftype t76 = t71 + t72 + t73 + t74 + t75 - t92 - t93 ;
ftype t85 = t2 * t19 * t49 ;
ftype t94 = t2 * t10 * t76 ;
ftype t77 = R_LOS + t37 + t43 + t56 + t63 + t70 - t85 - t94 ;
ftype t78 ;
2016-07-14 02:08:43 -03:00
2019-02-22 19:35:24 -04:00
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
2016-07-14 02:08:43 -03:00
if ( t77 > R_LOS ) {
t78 = 1.0f / t77 ;
faultStatus . bad_yflow = false ;
} else {
t77 = R_LOS ;
t78 = 1.0f / R_LOS ;
faultStatus . bad_yflow = true ;
return ;
}
2021-08-16 10:00:52 -03:00
flowVarInnov [ 1 ] = t77 ;
2016-07-14 02:08:43 -03:00
// calculate innovation for Y observation
2021-08-16 10:00:52 -03:00
flowInnov [ 1 ] = losPred [ 1 ] - ofDataDelayed . flowRadXYcomp . y ;
2022-05-17 08:22:36 -03:00
flowInnovTime_ms = dal . millis ( ) ;
2016-07-14 02:08:43 -03:00
// calculate Kalman gains for the Y-axis observation
Kfusion [ 0 ] = - t78 * ( t12 + P [ 0 ] [ 5 ] * t2 * t8 - P [ 0 ] [ 6 ] * t2 * t10 + P [ 0 ] [ 1 ] * t2 * t16 - P [ 0 ] [ 2 ] * t2 * t19 + P [ 0 ] [ 3 ] * t2 * t22 + P [ 0 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 1 ] = - t78 * ( t31 + P [ 1 ] [ 0 ] * t2 * t5 + P [ 1 ] [ 5 ] * t2 * t8 - P [ 1 ] [ 6 ] * t2 * t10 - P [ 1 ] [ 2 ] * t2 * t19 + P [ 1 ] [ 3 ] * t2 * t22 + P [ 1 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 2 ] = - t78 * ( - t79 + P [ 2 ] [ 0 ] * t2 * t5 + P [ 2 ] [ 5 ] * t2 * t8 - P [ 2 ] [ 6 ] * t2 * t10 + P [ 2 ] [ 1 ] * t2 * t16 + P [ 2 ] [ 3 ] * t2 * t22 + P [ 2 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 3 ] = - t78 * ( t53 + P [ 3 ] [ 0 ] * t2 * t5 + P [ 3 ] [ 5 ] * t2 * t8 - P [ 3 ] [ 6 ] * t2 * t10 + P [ 3 ] [ 1 ] * t2 * t16 - P [ 3 ] [ 2 ] * t2 * t19 + P [ 3 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 4 ] = - t78 * ( t61 + P [ 4 ] [ 0 ] * t2 * t5 + P [ 4 ] [ 5 ] * t2 * t8 - P [ 4 ] [ 6 ] * t2 * t10 + P [ 4 ] [ 1 ] * t2 * t16 - P [ 4 ] [ 2 ] * t2 * t19 + P [ 4 ] [ 3 ] * t2 * t22 ) ;
Kfusion [ 5 ] = - t78 * ( t64 + P [ 5 ] [ 0 ] * t2 * t5 - P [ 5 ] [ 6 ] * t2 * t10 + P [ 5 ] [ 1 ] * t2 * t16 - P [ 5 ] [ 2 ] * t2 * t19 + P [ 5 ] [ 3 ] * t2 * t22 + P [ 5 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 6 ] = - t78 * ( - t92 + P [ 6 ] [ 0 ] * t2 * t5 + P [ 6 ] [ 5 ] * t2 * t8 + P [ 6 ] [ 1 ] * t2 * t16 - P [ 6 ] [ 2 ] * t2 * t19 + P [ 6 ] [ 3 ] * t2 * t22 + P [ 6 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 7 ] = - t78 * ( P [ 7 ] [ 0 ] * t2 * t5 + P [ 7 ] [ 5 ] * t2 * t8 - P [ 7 ] [ 6 ] * t2 * t10 + P [ 7 ] [ 1 ] * t2 * t16 - P [ 7 ] [ 2 ] * t2 * t19 + P [ 7 ] [ 3 ] * t2 * t22 + P [ 7 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 8 ] = - t78 * ( P [ 8 ] [ 0 ] * t2 * t5 + P [ 8 ] [ 5 ] * t2 * t8 - P [ 8 ] [ 6 ] * t2 * t10 + P [ 8 ] [ 1 ] * t2 * t16 - P [ 8 ] [ 2 ] * t2 * t19 + P [ 8 ] [ 3 ] * t2 * t22 + P [ 8 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 9 ] = - t78 * ( P [ 9 ] [ 0 ] * t2 * t5 + P [ 9 ] [ 5 ] * t2 * t8 - P [ 9 ] [ 6 ] * t2 * t10 + P [ 9 ] [ 1 ] * t2 * t16 - P [ 9 ] [ 2 ] * t2 * t19 + P [ 9 ] [ 3 ] * t2 * t22 + P [ 9 ] [ 4 ] * t2 * t27 ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = - t78 * ( P [ 10 ] [ 0 ] * t2 * t5 + P [ 10 ] [ 5 ] * t2 * t8 - P [ 10 ] [ 6 ] * t2 * t10 + P [ 10 ] [ 1 ] * t2 * t16 - P [ 10 ] [ 2 ] * t2 * t19 + P [ 10 ] [ 3 ] * t2 * t22 + P [ 10 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 11 ] = - t78 * ( P [ 11 ] [ 0 ] * t2 * t5 + P [ 11 ] [ 5 ] * t2 * t8 - P [ 11 ] [ 6 ] * t2 * t10 + P [ 11 ] [ 1 ] * t2 * t16 - P [ 11 ] [ 2 ] * t2 * t19 + P [ 11 ] [ 3 ] * t2 * t22 + P [ 11 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 12 ] = - t78 * ( P [ 12 ] [ 0 ] * t2 * t5 + P [ 12 ] [ 5 ] * t2 * t8 - P [ 12 ] [ 6 ] * t2 * t10 + P [ 12 ] [ 1 ] * t2 * t16 - P [ 12 ] [ 2 ] * t2 * t19 + P [ 12 ] [ 3 ] * t2 * t22 + P [ 12 ] [ 4 ] * t2 * t27 ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 10 to 12
zero_range ( & Kfusion [ 0 ] , 10 , 12 ) ;
2017-05-09 19:31:55 -03:00
}
2021-07-15 18:53:49 -03:00
if ( ! inhibitDelVelBiasStates & & ! badIMUdata ) {
2021-03-08 02:09:02 -04:00
for ( uint8_t index = 0 ; index < 3 ; index + + ) {
2021-03-22 05:47:53 -03:00
const uint8_t stateIndex = index + 13 ;
2021-03-08 02:09:02 -04:00
if ( ! dvelBiasAxisInhibit [ index ] ) {
Kfusion [ stateIndex ] = - t78 * ( P [ stateIndex ] [ 0 ] * t2 * t5 + P [ stateIndex ] [ 5 ] * t2 * t8 - P [ stateIndex ] [ 6 ] * t2 * t10 + P [ stateIndex ] [ 1 ] * t2 * t16 - P [ stateIndex ] [ 2 ] * t2 * t19 + P [ stateIndex ] [ 3 ] * t2 * t22 + P [ stateIndex ] [ 4 ] * t2 * t27 ) ;
} else {
Kfusion [ stateIndex ] = 0.0f ;
}
}
2016-07-14 02:08:43 -03:00
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 13 to 15
zero_range ( & Kfusion [ 0 ] , 13 , 15 ) ;
2016-07-14 02:08:43 -03:00
}
2017-05-09 19:31:55 -03:00
2016-07-14 02:08:43 -03:00
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = - t78 * ( P [ 16 ] [ 0 ] * t2 * t5 + P [ 16 ] [ 5 ] * t2 * t8 - P [ 16 ] [ 6 ] * t2 * t10 + P [ 16 ] [ 1 ] * t2 * t16 - P [ 16 ] [ 2 ] * t2 * t19 + P [ 16 ] [ 3 ] * t2 * t22 + P [ 16 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 17 ] = - t78 * ( P [ 17 ] [ 0 ] * t2 * t5 + P [ 17 ] [ 5 ] * t2 * t8 - P [ 17 ] [ 6 ] * t2 * t10 + P [ 17 ] [ 1 ] * t2 * t16 - P [ 17 ] [ 2 ] * t2 * t19 + P [ 17 ] [ 3 ] * t2 * t22 + P [ 17 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 18 ] = - t78 * ( P [ 18 ] [ 0 ] * t2 * t5 + P [ 18 ] [ 5 ] * t2 * t8 - P [ 18 ] [ 6 ] * t2 * t10 + P [ 18 ] [ 1 ] * t2 * t16 - P [ 18 ] [ 2 ] * t2 * t19 + P [ 18 ] [ 3 ] * t2 * t22 + P [ 18 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 19 ] = - t78 * ( P [ 19 ] [ 0 ] * t2 * t5 + P [ 19 ] [ 5 ] * t2 * t8 - P [ 19 ] [ 6 ] * t2 * t10 + P [ 19 ] [ 1 ] * t2 * t16 - P [ 19 ] [ 2 ] * t2 * t19 + P [ 19 ] [ 3 ] * t2 * t22 + P [ 19 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 20 ] = - t78 * ( P [ 20 ] [ 0 ] * t2 * t5 + P [ 20 ] [ 5 ] * t2 * t8 - P [ 20 ] [ 6 ] * t2 * t10 + P [ 20 ] [ 1 ] * t2 * t16 - P [ 20 ] [ 2 ] * t2 * t19 + P [ 20 ] [ 3 ] * t2 * t22 + P [ 20 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 21 ] = - t78 * ( P [ 21 ] [ 0 ] * t2 * t5 + P [ 21 ] [ 5 ] * t2 * t8 - P [ 21 ] [ 6 ] * t2 * t10 + P [ 21 ] [ 1 ] * t2 * t16 - P [ 21 ] [ 2 ] * t2 * t19 + P [ 21 ] [ 3 ] * t2 * t22 + P [ 21 ] [ 4 ] * t2 * t27 ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 16 to 21
zero_range ( & Kfusion [ 0 ] , 16 , 21 ) ;
2017-05-09 19:31:55 -03:00
}
2023-09-26 19:18:07 -03:00
if ( ! inhibitWindStates & & ! treatWindStatesAsTruth ) {
2017-05-09 19:31:55 -03:00
Kfusion [ 22 ] = - t78 * ( P [ 22 ] [ 0 ] * t2 * t5 + P [ 22 ] [ 5 ] * t2 * t8 - P [ 22 ] [ 6 ] * t2 * t10 + P [ 22 ] [ 1 ] * t2 * t16 - P [ 22 ] [ 2 ] * t2 * t19 + P [ 22 ] [ 3 ] * t2 * t22 + P [ 22 ] [ 4 ] * t2 * t27 ) ;
Kfusion [ 23 ] = - t78 * ( P [ 23 ] [ 0 ] * t2 * t5 + P [ 23 ] [ 5 ] * t2 * t8 - P [ 23 ] [ 6 ] * t2 * t10 + P [ 23 ] [ 1 ] * t2 * t16 - P [ 23 ] [ 2 ] * t2 * t19 + P [ 23 ] [ 3 ] * t2 * t22 + P [ 23 ] [ 4 ] * t2 * t27 ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 22 to 23
zero_range ( & Kfusion [ 0 ] , 22 , 23 ) ;
2016-07-14 02:08:43 -03:00
}
}
// calculate the innovation consistency test ratio
2021-08-16 10:00:52 -03:00
flowTestRatio [ obsIndex ] = sq ( flowInnov [ obsIndex ] ) / ( sq ( MAX ( 0.01f * ( ftype ) frontend - > _flowInnovGate , 1.0f ) ) * flowVarInnov [ obsIndex ] ) ;
2016-07-14 02:08:43 -03:00
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
2021-08-16 09:41:54 -03:00
if ( really_fuse & & ( flowTestRatio [ obsIndex ] ) < 1.0f & & ( ofDataDelayed . flowRadXY . x < frontend - > _maxFlowRate ) & & ( ofDataDelayed . flowRadXY . y < frontend - > _maxFlowRate ) ) {
2016-07-14 02:08:43 -03:00
// record the last time observations were accepted for fusion
prevFlowFuseTime_ms = imuSampleTime_ms ;
2017-03-16 02:59:19 -03:00
// notify first time only
if ( ! flowFusionActive ) {
flowFusionActive = true ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u fusing optical flow " , ( unsigned ) imu_index ) ;
2017-03-16 02:59:19 -03:00
}
2016-07-14 02:08:43 -03:00
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
2021-08-16 09:14:44 -03:00
for ( uint8_t i = 0 ; i < = stateIndexLim ; i + + ) {
for ( uint8_t j = 0 ; j < = 6 ; j + + ) {
2016-07-14 02:08:43 -03:00
KH [ i ] [ j ] = Kfusion [ i ] * H_LOS [ j ] ;
}
2021-08-16 09:14:44 -03:00
for ( uint8_t j = 7 ; j < = stateIndexLim ; j + + ) {
2016-07-14 02:08:43 -03:00
KH [ i ] [ j ] = 0.0f ;
}
}
2021-08-16 09:14:44 -03:00
for ( uint8_t j = 0 ; j < = stateIndexLim ; j + + ) {
for ( uint8_t i = 0 ; i < = stateIndexLim ; i + + ) {
2016-07-14 02:08:43 -03:00
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.
2016-07-14 02:08:43 -03:00
ForceSymmetry ( ) ;
ConstrainVariances ( ) ;
// correct the state vector
for ( uint8_t j = 0 ; j < = stateIndexLim ; j + + ) {
2021-08-16 10:00:52 -03:00
statesArray [ j ] = statesArray [ j ] - Kfusion [ j ] * flowInnov [ obsIndex ] ;
2016-07-14 02:08:43 -03:00
}
stateStruct . quat . normalize ( ) ;
} else {
// record bad axis
if ( obsIndex = = 0 ) {
faultStatus . bad_xflow = true ;
} else if ( obsIndex = = 1 ) {
faultStatus . bad_yflow = true ;
}
}
}
}
2022-01-19 23:17:40 -04:00
// store optical flow rates for use in external calibration
flowCalSample . timestamp_ms = imuSampleTime_ms ;
flowCalSample . flowRate . x = ofDataDelayed . flowRadXY . x ;
flowCalSample . flowRate . y = ofDataDelayed . flowRadXY . y ;
flowCalSample . bodyRate . x = ofDataDelayed . bodyRadXYZ . x ;
flowCalSample . bodyRate . y = ofDataDelayed . bodyRadXYZ . y ;
flowCalSample . losPred . x = losPred [ 0 ] ;
flowCalSample . losPred . y = losPred [ 1 ] ;
}
// retrieve latest corrected optical flow samples (used for calibration)
bool NavEKF3_core : : getOptFlowSample ( uint32_t & timestamp_ms , Vector2f & flowRate , Vector2f & bodyRate , Vector2f & losPred ) const
{
if ( flowCalSample . timestamp_ms ! = 0 ) {
timestamp_ms = flowCalSample . timestamp_ms ;
flowRate = flowCalSample . flowRate ;
bodyRate = flowCalSample . bodyRate ;
losPred = flowCalSample . losPred ;
return true ;
}
return false ;
2016-07-14 02:08:43 -03:00
}
/********************************************************
* MISC FUNCTIONS *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2023-02-10 03:46:40 -04:00
# endif // EK3_FEATURE_OPTFLOW_FUSION