2016-07-14 02:08:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF3.h"
# include "AP_NavEKF3_core.h"
2020-10-16 00:38:43 -03:00
2016-07-14 02:08:43 -03:00
# include <GCS_MAVLink/GCS.h>
2020-11-05 19:30:16 -04:00
# include <AP_DAL/AP_DAL.h>
2016-07-14 02:08:43 -03:00
2023-07-03 08:33:46 -03:00
// minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s)
# if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
# define GPS_VEL_YAW_ALIGN_MIN_SPD 5.0F
# else
# define GPS_VEL_YAW_ALIGN_MIN_SPD 1.0F
# endif
2016-07-14 02:08:43 -03:00
/********************************************************
* RESET FUNCTIONS *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// Control reset of yaw and magnetic field states
void NavEKF3_core : : controlMagYawReset ( )
{
// Vehicles that can use a zero sideslip assumption (Planes) are a special case
// They can use the GPS velocity to recover from bad initial compass data
// This allows recovery for heading alignment errors due to compass faults
2021-02-27 23:41:29 -04:00
if ( assume_zero_sideslip ( ) & & ( ! finalInflightYawInit | | ! yawAlignComplete ) & & inFlight ) {
2016-07-14 02:08:43 -03:00
gpsYawResetRequest = true ;
return ;
} else {
gpsYawResetRequest = false ;
}
// Quaternion and delta rotation vector that are re-used for different calculations
2021-05-04 08:12:23 -03:00
Vector3F deltaRotVecTemp ;
QuaternionF deltaQuatTemp ;
2016-07-14 02:08:43 -03:00
bool flightResetAllowed = false ;
bool initialResetAllowed = false ;
if ( ! finalInflightYawInit ) {
// Use a quaternion division to calculate the delta quaternion between the rotation at the current and last time
deltaQuatTemp = stateStruct . quat / prevQuatMagReset ;
prevQuatMagReset = stateStruct . quat ;
// convert the quaternion to a rotation vector and find its length
deltaQuatTemp . to_axis_angle ( deltaRotVecTemp ) ;
// check if the spin rate is OK - high spin rates can cause angular alignment errors
bool angRateOK = deltaRotVecTemp . length ( ) < 0.1745f ;
2021-03-13 02:38:23 -04:00
initialResetAllowed = angRateOK & & tiltAlignComplete ;
2016-07-14 02:08:43 -03:00
flightResetAllowed = angRateOK & & ! onGround ;
}
2020-03-10 03:48:08 -03:00
// reset the limit on the number of magnetic anomaly resets for each takeoff
if ( onGround ) {
magYawAnomallyCount = 0 ;
}
2016-07-14 02:08:43 -03:00
// Check if conditions for a interim or final yaw/mag reset are met
bool finalResetRequest = false ;
bool interimResetRequest = false ;
if ( flightResetAllowed & & ! assume_zero_sideslip ( ) ) {
// check that we have reached a height where ground magnetic interference effects are insignificant
// and can perform a final reset of the yaw and field states
2017-07-07 05:05:04 -03:00
finalResetRequest = ( stateStruct . position . z - posDownAtTakeoff ) < - EKF3_MAG_FINAL_RESET_ALT ;
2016-07-14 02:08:43 -03:00
// check for increasing height
bool hgtIncreasing = ( posDownAtLastMagReset - stateStruct . position . z ) > 0.5f ;
2021-05-04 08:12:23 -03:00
ftype yawInnovIncrease = fabsF ( innovYaw ) - fabsF ( yawInnovAtLastMagReset ) ;
2016-07-14 02:08:43 -03:00
// check for increasing yaw innovations
bool yawInnovIncreasing = yawInnovIncrease > 0.25f ;
// check that the yaw innovations haven't been caused by a large change in attitude
deltaQuatTemp = quatAtLastMagReset / stateStruct . quat ;
deltaQuatTemp . to_axis_angle ( deltaRotVecTemp ) ;
bool largeAngleChange = deltaRotVecTemp . length ( ) > yawInnovIncrease ;
// if yaw innovations and height have increased and we haven't rotated much
// then we are climbing away from a ground based magnetic anomaly and need to reset
2020-03-10 03:48:08 -03:00
interimResetRequest = ! finalInflightYawInit
& & ! finalResetRequest
& & ( magYawAnomallyCount < MAG_ANOMALY_RESET_MAX )
& & hgtIncreasing
& & yawInnovIncreasing
& & ! largeAngleChange ;
2016-07-14 02:08:43 -03:00
}
// an initial reset is required if we have not yet aligned the yaw angle
bool initialResetRequest = initialResetAllowed & & ! yawAlignComplete ;
// a combined yaw angle and magnetic field reset can be initiated by:
magYawResetRequest = magYawResetRequest | | // an external request
initialResetRequest | | // an initial alignment performed by all vehicle types using magnetometer
interimResetRequest | | // an interim alignment required to recover from ground based magnetic anomaly
2019-02-22 19:35:24 -04:00
finalResetRequest ; // the final reset when we have achieved enough height to be in stable magnetic field environment
2016-07-14 02:08:43 -03:00
// Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
2020-07-25 05:47:43 -03:00
if ( magYawResetRequest & & use_compass ( ) ) {
2020-03-10 03:48:08 -03:00
// send initial alignment status to console
if ( ! yawAlignComplete ) {
2021-05-20 13:09:17 -03:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u MAG%u initial yaw alignment complete " , ( unsigned ) imu_index , ( unsigned ) magSelectIndex ) ;
2020-03-10 03:48:08 -03:00
}
2016-07-14 02:08:43 -03:00
2020-09-22 06:13:58 -03:00
// set yaw from a single mag sample
setYawFromMag ( ) ;
2020-03-10 03:48:08 -03:00
// send in-flight yaw alignment status to console
if ( finalResetRequest ) {
2021-05-20 13:09:17 -03:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u MAG%u in-flight yaw alignment complete " , ( unsigned ) imu_index , ( unsigned ) magSelectIndex ) ;
2020-03-10 03:48:08 -03:00
} else if ( interimResetRequest ) {
magYawAnomallyCount + + ;
2021-05-20 13:09:17 -03:00
GCS_SEND_TEXT ( MAV_SEVERITY_WARNING , " EKF3 IMU%u MAG%u ground mag anomaly, yaw re-aligned " , ( unsigned ) imu_index , ( unsigned ) magSelectIndex ) ;
2020-03-10 03:48:08 -03:00
}
2016-07-14 02:08:43 -03:00
2020-03-10 03:48:08 -03:00
// clear the complete flags if an interim reset has been performed to allow subsequent
// and final reset to occur
if ( interimResetRequest ) {
finalInflightYawInit = false ;
finalInflightMagInit = false ;
}
2016-07-14 02:08:43 -03:00
2020-07-25 05:47:43 -03:00
// mag states
2020-03-10 03:48:08 -03:00
if ( ! magFieldLearned ) {
2020-07-25 05:47:43 -03:00
resetMagFieldStates ( ) ;
2016-07-14 02:08:43 -03:00
}
2020-07-25 05:47:43 -03:00
}
2020-03-10 03:48:08 -03:00
2020-07-25 05:47:43 -03:00
if ( magStateResetRequest ) {
resetMagFieldStates ( ) ;
2016-07-14 02:08:43 -03:00
}
}
// this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
// vector from GPS. It is used to align the yaw angle after launch or takeoff.
2023-03-19 04:52:21 -03:00
void NavEKF3_core : : realignYawGPS ( bool emergency_reset )
2016-07-14 02:08:43 -03:00
{
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
2021-05-04 08:12:23 -03:00
Vector3F eulerAngles ;
2016-07-14 02:08:43 -03:00
stateStruct . quat . to_euler ( eulerAngles . x , eulerAngles . y , eulerAngles . z ) ;
2023-04-15 21:14:31 -03:00
if ( gpsDataDelayed . vel . xy ( ) . length_squared ( ) > sq ( GPS_VEL_YAW_ALIGN_MIN_SPD ) ) {
2016-07-14 02:08:43 -03:00
// calculate course yaw angle
2021-05-04 08:12:23 -03:00
ftype velYaw = atan2F ( stateStruct . velocity . y , stateStruct . velocity . x ) ;
2016-07-14 02:08:43 -03:00
// calculate course yaw angle from GPS velocity
2021-05-04 08:12:23 -03:00
ftype gpsYaw = atan2F ( gpsDataDelayed . vel . y , gpsDataDelayed . vel . x ) ;
2016-07-14 02:08:43 -03:00
// Check the yaw angles for consistency
2021-05-04 08:12:23 -03:00
ftype yawErr = MAX ( fabsF ( wrap_PI ( gpsYaw - velYaw ) ) , fabsF ( wrap_PI ( gpsYaw - eulerAngles . z ) ) ) ;
2016-07-14 02:08:43 -03:00
// If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
2020-11-22 00:05:42 -04:00
bool badMagYaw = ( ( yawErr > 0.7854f ) & & ( velTestRatio > 1.0f ) & & ( PV_AidingMode = = AID_ABSOLUTE ) ) | | ! yawAlignComplete ;
2016-07-14 02:08:43 -03:00
// correct yaw angle using GPS ground course if compass yaw bad
if ( badMagYaw ) {
2020-03-10 03:48:08 -03:00
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
2023-03-19 04:52:21 -03:00
// by default fly forward vehicles use ground course for initial yaw unless the GSF is explicitly selected as the yaw source
2024-04-16 21:01:21 -03:00
const bool useGSF = ! assume_zero_sideslip ( ) | | ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GSF ) ;
2023-03-19 04:52:21 -03:00
if ( useGSF & & EKFGSF_resetMainFilterYaw ( emergency_reset ) ) {
2020-03-10 03:48:08 -03:00
return ;
}
2016-07-14 02:08:43 -03:00
2023-03-19 04:52:21 -03:00
// get yaw variance from GPS speed uncertainty
const ftype gpsVelAcc = fmaxF ( gpsSpdAccuracy , ftype ( frontend - > _gpsHorizVelNoise ) ) ;
const ftype gps_yaw_variance = sq ( asinF ( constrain_float ( gpsVelAcc / gpsDataDelayed . vel . xy ( ) . length ( ) , - 1.0F , 1.0F ) ) ) ;
2023-04-15 21:14:31 -03:00
if ( gps_yaw_variance < sq ( radians ( GPS_VEL_YAW_ALIGN_MAX_ANG_ERR ) ) ) {
yawAlignGpsValidCount + + ;
} else {
yawAlignGpsValidCount = 0 ;
}
2016-07-14 02:08:43 -03:00
2023-04-15 21:14:31 -03:00
if ( yawAlignGpsValidCount > = GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD ) {
yawAlignGpsValidCount = 0 ;
// keep roll and pitch and reset yaw
rotationOrder order ;
bestRotationOrder ( order ) ;
resetQuatStateYawOnly ( gpsYaw , gps_yaw_variance , order ) ;
// reset the velocity and position states as they will be inaccurate due to bad yaw
ResetVelocity ( resetDataSource : : GPS ) ;
ResetPosition ( resetDataSource : : GPS ) ;
// send yaw alignment information to console
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u yaw aligned to GPS velocity " , ( unsigned ) imu_index ) ;
if ( use_compass ( ) ) {
// request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation
magStateResetRequest = true ;
// clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
allMagSensorsFailed = false ;
}
2016-07-14 02:08:43 -03:00
}
}
2023-04-15 21:14:31 -03:00
} else {
yawAlignGpsValidCount = 0 ;
2016-07-14 02:08:43 -03:00
}
}
2020-11-30 08:35:24 -04:00
// align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
void NavEKF3_core : : alignYawAngle ( const yaw_elements & yawAngData )
2017-05-28 11:12:14 -03:00
{
2020-10-27 23:33:58 -03:00
// update quaternion states and covariances
2021-05-04 08:12:23 -03:00
resetQuatStateYawOnly ( yawAngData . yawAng , sq ( MAX ( yawAngData . yawAngErr , 1.0e-2 ) ) , yawAngData . order ) ;
2017-05-28 11:12:14 -03:00
// send yaw alignment information to console
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u yaw aligned " , ( unsigned ) imu_index ) ;
2017-05-28 11:12:14 -03:00
}
2016-07-14 02:08:43 -03:00
/********************************************************
* FUSE MEASURED_DATA *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// select fusion of magnetometer data
void NavEKF3_core : : SelectMagFusion ( )
{
2019-02-22 19:35:24 -04:00
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step
2016-07-14 02:08:43 -03:00
// used for load levelling
magFusePerformed = false ;
2020-12-14 17:43:46 -04:00
// Store yaw angle when moving for use as a static reference when not moving
if ( ! onGroundNotMoving ) {
2021-05-04 08:12:23 -03:00
if ( fabsF ( prevTnb [ 0 ] [ 2 ] ) < fabsF ( prevTnb [ 1 ] [ 2 ] ) ) {
2020-12-15 00:23:37 -04:00
// A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
yawAngDataStatic . order = rotationOrder : : TAIT_BRYAN_321 ;
2021-05-04 08:12:23 -03:00
yawAngDataStatic . yawAng = atan2F ( prevTnb [ 0 ] [ 1 ] , prevTnb [ 0 ] [ 0 ] ) ;
2020-12-15 00:23:37 -04:00
} else {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
yawAngDataStatic . order = rotationOrder : : TAIT_BRYAN_312 ;
2021-05-04 08:12:23 -03:00
yawAngDataStatic . yawAng = atan2F ( - prevTnb [ 1 ] [ 0 ] , prevTnb [ 1 ] [ 1 ] ) ;
2020-12-15 00:23:37 -04:00
}
yawAngDataStatic . yawAngErr = MAX ( frontend - > _yawNoise , 0.05f ) ;
yawAngDataStatic . time_ms = imuDataDelayed . time_ms ;
2020-12-14 17:43:46 -04:00
}
2020-12-22 03:15:43 -04:00
// Handle case where we are not using a yaw sensor of any type and attempt to reset the yaw in
2023-03-19 04:52:21 -03:00
// flight using the output from the GSF yaw estimator or GPS ground course.
2024-04-16 21:01:21 -03:00
if ( ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GSF ) | |
2020-12-22 03:15:43 -04:00
( ! use_compass ( ) & &
2024-04-16 21:01:21 -03:00
yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GPS & &
yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK & &
yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : EXTNAV ) ) {
2020-06-20 22:58:14 -03:00
2024-04-16 21:01:21 -03:00
if ( ( ! yawAlignComplete | | yaw_source_reset ) & & ( ( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GSF ) | | ( EKFGSF_yaw_valid_count > = GSF_YAW_VALID_HISTORY_THRESHOLD ) ) ) {
2023-03-19 04:52:21 -03:00
realignYawGPS ( false ) ;
yaw_source_reset = false ;
} else {
2020-07-13 02:40:03 -03:00
yaw_source_reset = false ;
2020-04-10 21:43:46 -03:00
}
if ( imuSampleTime_ms - lastSynthYawTime_ms > 140 ) {
2020-12-14 17:43:46 -04:00
// use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
// for non fixed wing platform types
2021-05-04 08:12:23 -03:00
ftype gsfYaw , gsfYawVariance ;
2024-04-16 21:01:21 -03:00
const bool didUseEKFGSF = yawAlignComplete & & ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GSF ) & & EKFGSF_getYaw ( gsfYaw , gsfYawVariance ) & & ! assume_zero_sideslip ( ) & & fuseEulerYaw ( yawFusionMethod : : GSF ) ;
2020-12-14 17:43:46 -04:00
// fallback methods
if ( ! didUseEKFGSF ) {
2020-04-10 21:43:46 -03:00
if ( onGroundNotMoving ) {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
2020-12-14 17:43:46 -04:00
fuseEulerYaw ( yawFusionMethod : : STATIC ) ;
2021-03-13 21:12:11 -04:00
} else if ( onGround | | PV_AidingMode = = AID_NONE | | ( P [ 0 ] [ 0 ] + P [ 1 ] [ 1 ] + P [ 2 ] [ 2 ] + P [ 3 ] [ 3 ] > 0.01f ) ) {
// prevent uncontrolled yaw variance growth that can destabilise the covariance matrix
// by fusing a zero innovation
2020-12-14 17:43:46 -04:00
fuseEulerYaw ( yawFusionMethod : : PREDICTED ) ;
2020-04-10 21:43:46 -03:00
}
}
magTestRatio . zero ( ) ;
yawTestRatio = 0.0f ;
lastSynthYawTime_ms = imuSampleTime_ms ;
}
return ;
}
2020-11-30 08:35:24 -04:00
// Handle case where we are using GPS yaw sensor instead of a magnetomer
2024-04-16 21:01:21 -03:00
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GPS | | yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK ) {
2020-01-22 05:56:57 -04:00
bool have_fused_gps_yaw = false ;
2017-07-19 21:40:11 -03:00
if ( storedYawAng . recall ( yawAngDataDelayed , imuDataDelayed . time_ms ) ) {
2020-07-13 02:40:03 -03:00
if ( tiltAlignComplete & & ( ! yawAlignComplete | | yaw_source_reset ) ) {
2020-11-30 08:35:24 -04:00
alignYawAngle ( yawAngDataDelayed ) ;
2020-07-13 02:40:03 -03:00
yaw_source_reset = false ;
2021-05-03 08:52:18 -03:00
have_fused_gps_yaw = true ;
lastSynthYawTime_ms = imuSampleTime_ms ;
last_gps_yaw_fuse_ms = imuSampleTime_ms ;
2017-07-19 21:40:11 -03:00
} else if ( tiltAlignComplete & & yawAlignComplete ) {
2021-05-03 08:52:18 -03:00
have_fused_gps_yaw = fuseEulerYaw ( yawFusionMethod : : GPS ) ;
if ( have_fused_gps_yaw ) {
last_gps_yaw_fuse_ms = imuSampleTime_ms ;
}
2017-07-19 21:40:11 -03:00
}
2021-05-03 08:52:18 -03:00
last_gps_yaw_ms = imuSampleTime_ms ;
2020-12-14 17:43:46 -04:00
} else if ( tiltAlignComplete & & ! yawAlignComplete ) {
2021-05-03 08:52:18 -03:00
// External yaw sources can take significant time to start providing yaw data so
2020-12-14 17:43:46 -04:00
// wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable
2023-03-19 04:52:21 -03:00
if ( imuSampleTime_ms - lastSynthYawTime_ms > 140 ) {
2020-12-14 17:43:46 -04:00
yawAngDataDelayed . yawAngErr = MAX ( frontend - > _yawNoise , 0.05f ) ;
// update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
2020-04-08 22:10:51 -03:00
if ( ! onGroundNotMoving ) {
2020-12-14 17:43:46 -04:00
// prevent uncontrolled yaw variance growth by fusing a zero innovation
fuseEulerYaw ( yawFusionMethod : : PREDICTED ) ;
} else {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
fuseEulerYaw ( yawFusionMethod : : STATIC ) ;
2020-04-08 22:10:51 -03:00
}
2020-12-14 17:43:46 -04:00
lastSynthYawTime_ms = imuSampleTime_ms ;
2020-04-08 07:07:22 -03:00
}
2021-05-03 08:52:18 -03:00
} else if ( tiltAlignComplete & & yawAlignComplete & & onGround & & imuSampleTime_ms - last_gps_yaw_fuse_ms > 10000 ) {
2021-05-04 03:05:13 -03:00
// handle scenario where we were using GPS yaw previously, but the yaw fusion has timed out.
2021-05-03 08:52:18 -03:00
yaw_source_reset = true ;
2017-07-19 21:40:11 -03:00
}
2021-05-03 08:52:18 -03:00
2024-04-16 21:01:21 -03:00
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GPS ) {
2020-01-22 05:56:57 -04:00
// no fallback
return ;
}
// get new mag data into delay buffer
readMagData ( ) ;
if ( have_fused_gps_yaw ) {
if ( gps_yaw_mag_fallback_active ) {
gps_yaw_mag_fallback_active = false ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u yaw external " , ( unsigned ) imu_index ) ;
2020-01-22 05:56:57 -04:00
}
// update mag bias from GPS yaw
gps_yaw_mag_fallback_ok = learnMagBiasFromGPS ( ) ;
return ;
}
// we don't have GPS yaw data and are configured for
// fallback. If we've only just lost GPS yaw
2021-05-03 08:52:18 -03:00
if ( imuSampleTime_ms - last_gps_yaw_ms < 10000 ) {
2020-01-22 05:56:57 -04:00
// don't fallback to magnetometer fusion for 10s
return ;
}
if ( ! gps_yaw_mag_fallback_ok ) {
// mag was not consistent enough with GPS to use it as
// fallback
return ;
}
if ( ! inFlight ) {
2021-05-03 08:52:18 -03:00
// don't fall back if not flying but reset to GPS yaw if it becomes available
2020-01-22 05:56:57 -04:00
return ;
}
if ( ! gps_yaw_mag_fallback_active ) {
gps_yaw_mag_fallback_active = true ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u yaw fallback active " , ( unsigned ) imu_index ) ;
2020-01-22 05:56:57 -04:00
}
// fall through to magnetometer fusion
2017-07-19 21:40:11 -03:00
}
2021-01-19 00:27:03 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2020-11-30 08:35:24 -04:00
// Handle case where we are using an external nav for yaw
const bool extNavYawDataToFuse = storedExtNavYawAng . recall ( extNavYawAngDataDelayed , imuDataDelayed . time_ms ) ;
2024-04-16 21:01:21 -03:00
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : EXTNAV ) {
2020-11-30 08:35:24 -04:00
if ( extNavYawDataToFuse ) {
if ( tiltAlignComplete & & ( ! yawAlignComplete | | yaw_source_reset ) ) {
alignYawAngle ( extNavYawAngDataDelayed ) ;
yaw_source_reset = false ;
} else if ( tiltAlignComplete & & yawAlignComplete ) {
fuseEulerYaw ( yawFusionMethod : : EXTNAV ) ;
}
last_extnav_yaw_fusion_ms = imuSampleTime_ms ;
} else if ( tiltAlignComplete & & ! yawAlignComplete ) {
// External yaw sources can take significant time to start providing yaw data so
// while waiting, fuse a 'fake' yaw observation at 7Hz to keep the filter stable
if ( imuSampleTime_ms - lastSynthYawTime_ms > 140 ) {
// update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
if ( ! onGroundNotMoving ) {
// prevent uncontrolled yaw variance growth by fusing a zero innovation
fuseEulerYaw ( yawFusionMethod : : PREDICTED ) ;
} else {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
fuseEulerYaw ( yawFusionMethod : : STATIC ) ;
}
lastSynthYawTime_ms = imuSampleTime_ms ;
}
}
}
2021-01-19 00:27:03 -04:00
# endif // EK3_FEATURE_EXTERNAL_NAV
2020-11-30 08:35:24 -04:00
2016-07-14 02:08:43 -03:00
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
if ( magHealth ) {
magTimeout = false ;
lastHealthyMagTime_ms = imuSampleTime_ms ;
} else if ( ( imuSampleTime_ms - lastHealthyMagTime_ms ) > frontend - > magFailTimeLimit_ms & & use_compass ( ) ) {
magTimeout = true ;
}
2024-04-16 21:01:21 -03:00
if ( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK ) {
2020-09-07 00:12:58 -03:00
// check for and read new magnetometer measurements. We don't
2020-12-22 04:08:30 -04:00
// read for GPS_COMPASS_FALLBACK as it has already been read
2020-09-07 00:12:58 -03:00
// above
readMagData ( ) ;
}
2017-05-28 11:12:14 -03:00
// check for availability of magnetometer or other yaw data to fuse
2016-07-14 02:08:43 -03:00
magDataToFuse = storedMag . recall ( magDataDelayed , imuDataDelayed . time_ms ) ;
// Control reset of yaw and magnetic field states if we are using compass data
2017-07-19 21:40:11 -03:00
if ( magDataToFuse ) {
2024-04-16 21:01:21 -03:00
if ( yaw_source_reset & & ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : COMPASS | |
yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK ) ) {
2020-07-13 02:40:03 -03:00
magYawResetRequest = true ;
yaw_source_reset = false ;
}
2016-07-14 02:08:43 -03:00
controlMagYawReset ( ) ;
}
// determine if conditions are right to start a new fusion cycle
// wait until the EKF time horizon catches up with the measurement
bool dataReady = ( magDataToFuse & & statesInitialised & & use_compass ( ) & & yawAlignComplete ) ;
if ( dataReady ) {
// use the simple method of declination to maintain heading if we cannot use the magnetic field states
if ( inhibitMagStates | | magStateResetRequest | | ! magStateInitComplete ) {
2020-12-15 03:28:00 -04:00
fuseEulerYaw ( yawFusionMethod : : MAGNETOMETER ) ;
2017-07-19 21:40:11 -03:00
2016-07-14 02:08:43 -03:00
// zero the test ratio output from the inactive 3-axis magnetometer fusion
magTestRatio . zero ( ) ;
2017-07-19 21:40:11 -03:00
2016-07-14 02:08:43 -03:00
} else {
// if we are not doing aiding with earth relative observations (eg GPS) then the declination is
// maintained by fusing declination as a synthesised observation
2019-09-22 09:28:19 -03:00
// We also fuse declination if we are using the WMM tables
if ( PV_AidingMode ! = AID_ABSOLUTE | |
( frontend - > _mag_ef_limit > 0 & & have_table_earth_field ) ) {
2016-07-14 02:08:43 -03:00
FuseDeclination ( 0.34f ) ;
}
2020-03-27 06:00:38 -03:00
// fuse the three magnetometer componenents using sequential fusion for each axis
FuseMagnetometer ( ) ;
2016-07-14 02:08:43 -03:00
// zero the test ratio output from the inactive simple magnetometer yaw fusion
yawTestRatio = 0.0f ;
}
}
// If the final yaw reset has been performed and the state variances are sufficiently low
// record that the earth field has been learned.
if ( ! magFieldLearned & & finalInflightMagInit ) {
magFieldLearned = ( P [ 16 ] [ 16 ] < sq ( 0.01f ) ) & & ( P [ 17 ] [ 17 ] < sq ( 0.01f ) ) & & ( P [ 18 ] [ 18 ] < sq ( 0.01f ) ) ;
}
// record the last learned field variances
if ( magFieldLearned & & ! inhibitMagStates ) {
earthMagFieldVar . x = P [ 16 ] [ 16 ] ;
earthMagFieldVar . y = P [ 17 ] [ 17 ] ;
earthMagFieldVar . z = P [ 18 ] [ 18 ] ;
bodyMagFieldVar . x = P [ 19 ] [ 19 ] ;
bodyMagFieldVar . y = P [ 20 ] [ 20 ] ;
bodyMagFieldVar . z = P [ 21 ] [ 21 ] ;
}
}
/*
* Fuse magnetometer 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 :
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
*/
void NavEKF3_core : : FuseMagnetometer ( )
{
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
// uncorrelated which is not true, however in the absence of covariance
// data fit is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion
// calculate observation jacobians and Kalman gains
2023-01-09 22:14:39 -04:00
// create aliases for state to make code easier to read:
const ftype q0 = stateStruct . quat [ 0 ] ;
const ftype q1 = stateStruct . quat [ 1 ] ;
const ftype q2 = stateStruct . quat [ 2 ] ;
const ftype q3 = stateStruct . quat [ 3 ] ;
const ftype magN = stateStruct . earth_magfield [ 0 ] ;
const ftype magE = stateStruct . earth_magfield [ 1 ] ;
const ftype magD = stateStruct . earth_magfield [ 2 ] ;
const ftype magXbias = stateStruct . body_magfield [ 0 ] ;
const ftype magYbias = stateStruct . body_magfield [ 1 ] ;
const ftype magZbias = stateStruct . body_magfield [ 2 ] ;
2016-07-14 02:08:43 -03:00
// rotate predicted earth components into body axes and calculate
// predicted measurements
2023-01-11 08:17:44 -04:00
const Matrix3F DCM {
q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3 ,
2.0f * ( q1 * q2 + q0 * q3 ) ,
2.0f * ( q1 * q3 - q0 * q2 ) ,
2.0f * ( q1 * q2 - q0 * q3 ) ,
q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3 ,
2.0f * ( q2 * q3 + q0 * q1 ) ,
2.0f * ( q1 * q3 + q0 * q2 ) ,
2.0f * ( q2 * q3 - q0 * q1 ) ,
q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
} ;
const Vector3F MagPred {
DCM [ 0 ] [ 0 ] * magN + DCM [ 0 ] [ 1 ] * magE + DCM [ 0 ] [ 2 ] * magD + magXbias ,
DCM [ 1 ] [ 0 ] * magN + DCM [ 1 ] [ 1 ] * magE + DCM [ 1 ] [ 2 ] * magD + magYbias ,
DCM [ 2 ] [ 0 ] * magN + DCM [ 2 ] [ 1 ] * magE + DCM [ 2 ] [ 2 ] * magD + magZbias
} ;
2016-07-14 02:08:43 -03:00
// calculate the measurement innovation for each axis
2023-01-11 08:17:44 -04:00
innovMag = MagPred - magDataDelayed . mag ;
2016-07-14 02:08:43 -03:00
// scale magnetometer observation error with total angular rate to allow for timing errors
2023-01-09 22:14:39 -04:00
const ftype R_MAG = sq ( constrain_ftype ( frontend - > _magNoise , 0.01f , 0.5f ) ) + sq ( frontend - > magVarRateScale * imuDataDelayed . delAng . length ( ) / imuDataDelayed . delAngDT ) ;
2016-07-14 02:08:43 -03:00
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
2023-01-11 08:17:44 -04:00
const Vector9 SH_MAG {
2.0f * magD * q3 + 2.0f * magE * q2 + 2.0f * magN * q1 ,
2.0f * magD * q0 - 2.0f * magE * q1 + 2.0f * magN * q2 ,
2.0f * magD * q1 + 2.0f * magE * q0 - 2.0f * magN * q3 ,
sq ( q3 ) ,
sq ( q2 ) ,
sq ( q1 ) ,
sq ( q0 ) ,
2.0f * magN * q0 ,
2.0f * magE * q3
} ;
2016-07-14 02:08:43 -03:00
// Calculate the innovation variance for each axis
// X axis
varInnovMag [ 0 ] = ( P [ 19 ] [ 19 ] + R_MAG + P [ 1 ] [ 19 ] * SH_MAG [ 0 ] - P [ 2 ] [ 19 ] * SH_MAG [ 1 ] + P [ 3 ] [ 19 ] * SH_MAG [ 2 ] - P [ 16 ] [ 19 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) * ( P [ 19 ] [ 17 ] + P [ 1 ] [ 17 ] * SH_MAG [ 0 ] - P [ 2 ] [ 17 ] * SH_MAG [ 1 ] + P [ 3 ] [ 17 ] * SH_MAG [ 2 ] - P [ 16 ] [ 17 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 17 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 17 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 17 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) * ( P [ 19 ] [ 18 ] + P [ 1 ] [ 18 ] * SH_MAG [ 0 ] - P [ 2 ] [ 18 ] * SH_MAG [ 1 ] + P [ 3 ] [ 18 ] * SH_MAG [ 2 ] - P [ 16 ] [ 18 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 18 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 18 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 18 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) * ( P [ 19 ] [ 0 ] + P [ 1 ] [ 0 ] * SH_MAG [ 0 ] - P [ 2 ] [ 0 ] * SH_MAG [ 1 ] + P [ 3 ] [ 0 ] * SH_MAG [ 2 ] - P [ 16 ] [ 0 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 0 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 0 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 0 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + P [ 17 ] [ 19 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 19 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + SH_MAG [ 0 ] * ( P [ 19 ] [ 1 ] + P [ 1 ] [ 1 ] * SH_MAG [ 0 ] - P [ 2 ] [ 1 ] * SH_MAG [ 1 ] + P [ 3 ] [ 1 ] * SH_MAG [ 2 ] - P [ 16 ] [ 1 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 1 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 1 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 1 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - SH_MAG [ 1 ] * ( P [ 19 ] [ 2 ] + P [ 1 ] [ 2 ] * SH_MAG [ 0 ] - P [ 2 ] [ 2 ] * SH_MAG [ 1 ] + P [ 3 ] [ 2 ] * SH_MAG [ 2 ] - P [ 16 ] [ 2 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 2 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 2 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 2 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + SH_MAG [ 2 ] * ( P [ 19 ] [ 3 ] + P [ 1 ] [ 3 ] * SH_MAG [ 0 ] - P [ 2 ] [ 3 ] * SH_MAG [ 1 ] + P [ 3 ] [ 3 ] * SH_MAG [ 2 ] - P [ 16 ] [ 3 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 3 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 3 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 3 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) * ( P [ 19 ] [ 16 ] + P [ 1 ] [ 16 ] * SH_MAG [ 0 ] - P [ 2 ] [ 16 ] * SH_MAG [ 1 ] + P [ 3 ] [ 16 ] * SH_MAG [ 2 ] - P [ 16 ] [ 16 ] * ( SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ) + P [ 17 ] [ 16 ] * ( 2.0f * q0 * q3 + 2.0f * q1 * q2 ) - P [ 18 ] [ 16 ] * ( 2.0f * q0 * q2 - 2.0f * q1 * q3 ) + P [ 0 ] [ 16 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + P [ 0 ] [ 19 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) ;
if ( varInnovMag [ 0 ] > = R_MAG ) {
faultStatus . bad_xmag = false ;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit ( ) ;
faultStatus . bad_xmag = true ;
return ;
}
// Y axis
varInnovMag [ 1 ] = ( P [ 20 ] [ 20 ] + R_MAG + P [ 0 ] [ 20 ] * SH_MAG [ 2 ] + P [ 1 ] [ 20 ] * SH_MAG [ 1 ] + P [ 2 ] [ 20 ] * SH_MAG [ 0 ] - P [ 17 ] [ 20 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) * ( P [ 20 ] [ 16 ] + P [ 0 ] [ 16 ] * SH_MAG [ 2 ] + P [ 1 ] [ 16 ] * SH_MAG [ 1 ] + P [ 2 ] [ 16 ] * SH_MAG [ 0 ] - P [ 17 ] [ 16 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 16 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 16 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 16 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) * ( P [ 20 ] [ 18 ] + P [ 0 ] [ 18 ] * SH_MAG [ 2 ] + P [ 1 ] [ 18 ] * SH_MAG [ 1 ] + P [ 2 ] [ 18 ] * SH_MAG [ 0 ] - P [ 17 ] [ 18 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 18 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 18 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 18 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) * ( P [ 20 ] [ 3 ] + P [ 0 ] [ 3 ] * SH_MAG [ 2 ] + P [ 1 ] [ 3 ] * SH_MAG [ 1 ] + P [ 2 ] [ 3 ] * SH_MAG [ 0 ] - P [ 17 ] [ 3 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 3 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 3 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 3 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - P [ 16 ] [ 20 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 20 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) + SH_MAG [ 2 ] * ( P [ 20 ] [ 0 ] + P [ 0 ] [ 0 ] * SH_MAG [ 2 ] + P [ 1 ] [ 0 ] * SH_MAG [ 1 ] + P [ 2 ] [ 0 ] * SH_MAG [ 0 ] - P [ 17 ] [ 0 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 0 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 0 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 0 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + SH_MAG [ 1 ] * ( P [ 20 ] [ 1 ] + P [ 0 ] [ 1 ] * SH_MAG [ 2 ] + P [ 1 ] [ 1 ] * SH_MAG [ 1 ] + P [ 2 ] [ 1 ] * SH_MAG [ 0 ] - P [ 17 ] [ 1 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 1 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 1 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 1 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + SH_MAG [ 0 ] * ( P [ 20 ] [ 2 ] + P [ 0 ] [ 2 ] * SH_MAG [ 2 ] + P [ 1 ] [ 2 ] * SH_MAG [ 1 ] + P [ 2 ] [ 2 ] * SH_MAG [ 0 ] - P [ 17 ] [ 2 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 2 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 2 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 2 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) * ( P [ 20 ] [ 17 ] + P [ 0 ] [ 17 ] * SH_MAG [ 2 ] + P [ 1 ] [ 17 ] * SH_MAG [ 1 ] + P [ 2 ] [ 17 ] * SH_MAG [ 0 ] - P [ 17 ] [ 17 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ) - P [ 16 ] [ 17 ] * ( 2.0f * q0 * q3 - 2.0f * q1 * q2 ) + P [ 18 ] [ 17 ] * ( 2.0f * q0 * q1 + 2.0f * q2 * q3 ) - P [ 3 ] [ 17 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - P [ 3 ] [ 20 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) ;
if ( varInnovMag [ 1 ] > = R_MAG ) {
faultStatus . bad_ymag = false ;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit ( ) ;
faultStatus . bad_ymag = true ;
return ;
}
// Z axis
varInnovMag [ 2 ] = ( P [ 21 ] [ 21 ] + R_MAG + P [ 0 ] [ 21 ] * SH_MAG [ 1 ] - P [ 1 ] [ 21 ] * SH_MAG [ 2 ] + P [ 3 ] [ 21 ] * SH_MAG [ 0 ] + P [ 18 ] [ 21 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) * ( P [ 21 ] [ 16 ] + P [ 0 ] [ 16 ] * SH_MAG [ 1 ] - P [ 1 ] [ 16 ] * SH_MAG [ 2 ] + P [ 3 ] [ 16 ] * SH_MAG [ 0 ] + P [ 18 ] [ 16 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 16 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 16 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 16 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) * ( P [ 21 ] [ 17 ] + P [ 0 ] [ 17 ] * SH_MAG [ 1 ] - P [ 1 ] [ 17 ] * SH_MAG [ 2 ] + P [ 3 ] [ 17 ] * SH_MAG [ 0 ] + P [ 18 ] [ 17 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 17 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 17 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 17 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) * ( P [ 21 ] [ 2 ] + P [ 0 ] [ 2 ] * SH_MAG [ 1 ] - P [ 1 ] [ 2 ] * SH_MAG [ 2 ] + P [ 3 ] [ 2 ] * SH_MAG [ 0 ] + P [ 18 ] [ 2 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 2 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 2 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 2 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + P [ 16 ] [ 21 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 21 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + SH_MAG [ 1 ] * ( P [ 21 ] [ 0 ] + P [ 0 ] [ 0 ] * SH_MAG [ 1 ] - P [ 1 ] [ 0 ] * SH_MAG [ 2 ] + P [ 3 ] [ 0 ] * SH_MAG [ 0 ] + P [ 18 ] [ 0 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 0 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 0 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 0 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) - SH_MAG [ 2 ] * ( P [ 21 ] [ 1 ] + P [ 0 ] [ 1 ] * SH_MAG [ 1 ] - P [ 1 ] [ 1 ] * SH_MAG [ 2 ] + P [ 3 ] [ 1 ] * SH_MAG [ 0 ] + P [ 18 ] [ 1 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 1 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 1 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 1 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + SH_MAG [ 0 ] * ( P [ 21 ] [ 3 ] + P [ 0 ] [ 3 ] * SH_MAG [ 1 ] - P [ 1 ] [ 3 ] * SH_MAG [ 2 ] + P [ 3 ] [ 3 ] * SH_MAG [ 0 ] + P [ 18 ] [ 3 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 3 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 3 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 3 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) * ( P [ 21 ] [ 18 ] + P [ 0 ] [ 18 ] * SH_MAG [ 1 ] - P [ 1 ] [ 18 ] * SH_MAG [ 2 ] + P [ 3 ] [ 18 ] * SH_MAG [ 0 ] + P [ 18 ] [ 18 ] * ( SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ) + P [ 16 ] [ 18 ] * ( 2.0f * q0 * q2 + 2.0f * q1 * q3 ) - P [ 17 ] [ 18 ] * ( 2.0f * q0 * q1 - 2.0f * q2 * q3 ) + P [ 2 ] [ 18 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) + P [ 2 ] [ 21 ] * ( SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ) ) ;
if ( varInnovMag [ 2 ] > = R_MAG ) {
faultStatus . bad_zmag = false ;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit ( ) ;
faultStatus . bad_zmag = true ;
return ;
}
// calculate the innovation test ratios
for ( uint8_t i = 0 ; i < = 2 ; i + + ) {
2021-05-04 08:12:23 -03:00
magTestRatio [ i ] = sq ( innovMag [ i ] ) / ( sq ( MAX ( 0.01f * ( ftype ) frontend - > _magInnovGate , 1.0f ) ) * varInnovMag [ i ] ) ;
2016-07-14 02:08:43 -03:00
}
// check the last values from all components and set magnetometer health accordingly
magHealth = ( magTestRatio [ 0 ] < 1.0f & & magTestRatio [ 1 ] < 1.0f & & magTestRatio [ 2 ] < 1.0f ) ;
// if the magnetometer is unhealthy, do not proceed further
if ( ! magHealth ) {
return ;
}
2023-01-11 08:17:44 -04:00
Vector24 H_MAG ;
2020-03-27 06:00:38 -03:00
for ( uint8_t obsIndex = 0 ; obsIndex < = 2 ; obsIndex + + ) {
2016-07-14 02:08:43 -03:00
if ( obsIndex = = 0 ) {
for ( uint8_t i = 0 ; i < = stateIndexLim ; i + + ) H_MAG [ i ] = 0.0f ;
H_MAG [ 0 ] = SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ;
H_MAG [ 1 ] = SH_MAG [ 0 ] ;
H_MAG [ 2 ] = - SH_MAG [ 1 ] ;
H_MAG [ 3 ] = SH_MAG [ 2 ] ;
H_MAG [ 16 ] = SH_MAG [ 5 ] - SH_MAG [ 4 ] - SH_MAG [ 3 ] + SH_MAG [ 6 ] ;
H_MAG [ 17 ] = 2.0f * q0 * q3 + 2.0f * q1 * q2 ;
H_MAG [ 18 ] = 2.0f * q1 * q3 - 2.0f * q0 * q2 ;
H_MAG [ 19 ] = 1.0f ;
2019-09-20 22:38:33 -03:00
H_MAG [ 20 ] = 0.0f ;
H_MAG [ 21 ] = 0.0f ;
2016-07-14 02:08:43 -03:00
// calculate Kalman gain
2023-01-11 08:17:44 -04:00
const Vector5 SK_MX {
1.0f / varInnovMag [ 0 ] ,
SH_MAG [ 3 ] + SH_MAG [ 4 ] - SH_MAG [ 5 ] - SH_MAG [ 6 ] ,
SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ,
2.0f * q0 * q2 - 2.0f * q1 * q3 ,
2.0f * q0 * q3 + 2.0f * q1 * q2
} ;
2017-05-09 19:31:55 -03:00
2016-07-14 02:08:43 -03:00
Kfusion [ 0 ] = SK_MX [ 0 ] * ( P [ 0 ] [ 19 ] + P [ 0 ] [ 1 ] * SH_MAG [ 0 ] - P [ 0 ] [ 2 ] * SH_MAG [ 1 ] + P [ 0 ] [ 3 ] * SH_MAG [ 2 ] + P [ 0 ] [ 0 ] * SK_MX [ 2 ] - P [ 0 ] [ 16 ] * SK_MX [ 1 ] + P [ 0 ] [ 17 ] * SK_MX [ 4 ] - P [ 0 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 1 ] = SK_MX [ 0 ] * ( P [ 1 ] [ 19 ] + P [ 1 ] [ 1 ] * SH_MAG [ 0 ] - P [ 1 ] [ 2 ] * SH_MAG [ 1 ] + P [ 1 ] [ 3 ] * SH_MAG [ 2 ] + P [ 1 ] [ 0 ] * SK_MX [ 2 ] - P [ 1 ] [ 16 ] * SK_MX [ 1 ] + P [ 1 ] [ 17 ] * SK_MX [ 4 ] - P [ 1 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 2 ] = SK_MX [ 0 ] * ( P [ 2 ] [ 19 ] + P [ 2 ] [ 1 ] * SH_MAG [ 0 ] - P [ 2 ] [ 2 ] * SH_MAG [ 1 ] + P [ 2 ] [ 3 ] * SH_MAG [ 2 ] + P [ 2 ] [ 0 ] * SK_MX [ 2 ] - P [ 2 ] [ 16 ] * SK_MX [ 1 ] + P [ 2 ] [ 17 ] * SK_MX [ 4 ] - P [ 2 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 3 ] = SK_MX [ 0 ] * ( P [ 3 ] [ 19 ] + P [ 3 ] [ 1 ] * SH_MAG [ 0 ] - P [ 3 ] [ 2 ] * SH_MAG [ 1 ] + P [ 3 ] [ 3 ] * SH_MAG [ 2 ] + P [ 3 ] [ 0 ] * SK_MX [ 2 ] - P [ 3 ] [ 16 ] * SK_MX [ 1 ] + P [ 3 ] [ 17 ] * SK_MX [ 4 ] - P [ 3 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 4 ] = SK_MX [ 0 ] * ( P [ 4 ] [ 19 ] + P [ 4 ] [ 1 ] * SH_MAG [ 0 ] - P [ 4 ] [ 2 ] * SH_MAG [ 1 ] + P [ 4 ] [ 3 ] * SH_MAG [ 2 ] + P [ 4 ] [ 0 ] * SK_MX [ 2 ] - P [ 4 ] [ 16 ] * SK_MX [ 1 ] + P [ 4 ] [ 17 ] * SK_MX [ 4 ] - P [ 4 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 5 ] = SK_MX [ 0 ] * ( P [ 5 ] [ 19 ] + P [ 5 ] [ 1 ] * SH_MAG [ 0 ] - P [ 5 ] [ 2 ] * SH_MAG [ 1 ] + P [ 5 ] [ 3 ] * SH_MAG [ 2 ] + P [ 5 ] [ 0 ] * SK_MX [ 2 ] - P [ 5 ] [ 16 ] * SK_MX [ 1 ] + P [ 5 ] [ 17 ] * SK_MX [ 4 ] - P [ 5 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 6 ] = SK_MX [ 0 ] * ( P [ 6 ] [ 19 ] + P [ 6 ] [ 1 ] * SH_MAG [ 0 ] - P [ 6 ] [ 2 ] * SH_MAG [ 1 ] + P [ 6 ] [ 3 ] * SH_MAG [ 2 ] + P [ 6 ] [ 0 ] * SK_MX [ 2 ] - P [ 6 ] [ 16 ] * SK_MX [ 1 ] + P [ 6 ] [ 17 ] * SK_MX [ 4 ] - P [ 6 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 7 ] = SK_MX [ 0 ] * ( P [ 7 ] [ 19 ] + P [ 7 ] [ 1 ] * SH_MAG [ 0 ] - P [ 7 ] [ 2 ] * SH_MAG [ 1 ] + P [ 7 ] [ 3 ] * SH_MAG [ 2 ] + P [ 7 ] [ 0 ] * SK_MX [ 2 ] - P [ 7 ] [ 16 ] * SK_MX [ 1 ] + P [ 7 ] [ 17 ] * SK_MX [ 4 ] - P [ 7 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 8 ] = SK_MX [ 0 ] * ( P [ 8 ] [ 19 ] + P [ 8 ] [ 1 ] * SH_MAG [ 0 ] - P [ 8 ] [ 2 ] * SH_MAG [ 1 ] + P [ 8 ] [ 3 ] * SH_MAG [ 2 ] + P [ 8 ] [ 0 ] * SK_MX [ 2 ] - P [ 8 ] [ 16 ] * SK_MX [ 1 ] + P [ 8 ] [ 17 ] * SK_MX [ 4 ] - P [ 8 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 9 ] = SK_MX [ 0 ] * ( P [ 9 ] [ 19 ] + P [ 9 ] [ 1 ] * SH_MAG [ 0 ] - P [ 9 ] [ 2 ] * SH_MAG [ 1 ] + P [ 9 ] [ 3 ] * SH_MAG [ 2 ] + P [ 9 ] [ 0 ] * SK_MX [ 2 ] - P [ 9 ] [ 16 ] * SK_MX [ 1 ] + P [ 9 ] [ 17 ] * SK_MX [ 4 ] - P [ 9 ] [ 18 ] * SK_MX [ 3 ] ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = SK_MX [ 0 ] * ( P [ 10 ] [ 19 ] + P [ 10 ] [ 1 ] * SH_MAG [ 0 ] - P [ 10 ] [ 2 ] * SH_MAG [ 1 ] + P [ 10 ] [ 3 ] * SH_MAG [ 2 ] + P [ 10 ] [ 0 ] * SK_MX [ 2 ] - P [ 10 ] [ 16 ] * SK_MX [ 1 ] + P [ 10 ] [ 17 ] * SK_MX [ 4 ] - P [ 10 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 11 ] = SK_MX [ 0 ] * ( P [ 11 ] [ 19 ] + P [ 11 ] [ 1 ] * SH_MAG [ 0 ] - P [ 11 ] [ 2 ] * SH_MAG [ 1 ] + P [ 11 ] [ 3 ] * SH_MAG [ 2 ] + P [ 11 ] [ 0 ] * SK_MX [ 2 ] - P [ 11 ] [ 16 ] * SK_MX [ 1 ] + P [ 11 ] [ 17 ] * SK_MX [ 4 ] - P [ 11 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 12 ] = SK_MX [ 0 ] * ( P [ 12 ] [ 19 ] + P [ 12 ] [ 1 ] * SH_MAG [ 0 ] - P [ 12 ] [ 2 ] * SH_MAG [ 1 ] + P [ 12 ] [ 3 ] * SH_MAG [ 2 ] + P [ 12 ] [ 0 ] * SK_MX [ 2 ] - P [ 12 ] [ 16 ] * SK_MX [ 1 ] + P [ 12 ] [ 17 ] * SK_MX [ 4 ] - P [ 12 ] [ 18 ] * SK_MX [ 3 ] ) ;
} 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
}
if ( ! inhibitDelVelBiasStates ) {
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 ] = SK_MX [ 0 ] * ( P [ stateIndex ] [ 19 ] + P [ stateIndex ] [ 1 ] * SH_MAG [ 0 ] - P [ stateIndex ] [ 2 ] * SH_MAG [ 1 ] + P [ stateIndex ] [ 3 ] * SH_MAG [ 2 ] + P [ stateIndex ] [ 0 ] * SK_MX [ 2 ] - P [ stateIndex ] [ 16 ] * SK_MX [ 1 ] + P [ stateIndex ] [ 17 ] * SK_MX [ 4 ] - P [ stateIndex ] [ 18 ] * SK_MX [ 3 ] ) ;
} 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
}
// zero Kalman gains to inhibit magnetic field state estimation
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = SK_MX [ 0 ] * ( P [ 16 ] [ 19 ] + P [ 16 ] [ 1 ] * SH_MAG [ 0 ] - P [ 16 ] [ 2 ] * SH_MAG [ 1 ] + P [ 16 ] [ 3 ] * SH_MAG [ 2 ] + P [ 16 ] [ 0 ] * SK_MX [ 2 ] - P [ 16 ] [ 16 ] * SK_MX [ 1 ] + P [ 16 ] [ 17 ] * SK_MX [ 4 ] - P [ 16 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 17 ] = SK_MX [ 0 ] * ( P [ 17 ] [ 19 ] + P [ 17 ] [ 1 ] * SH_MAG [ 0 ] - P [ 17 ] [ 2 ] * SH_MAG [ 1 ] + P [ 17 ] [ 3 ] * SH_MAG [ 2 ] + P [ 17 ] [ 0 ] * SK_MX [ 2 ] - P [ 17 ] [ 16 ] * SK_MX [ 1 ] + P [ 17 ] [ 17 ] * SK_MX [ 4 ] - P [ 17 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 18 ] = SK_MX [ 0 ] * ( P [ 18 ] [ 19 ] + P [ 18 ] [ 1 ] * SH_MAG [ 0 ] - P [ 18 ] [ 2 ] * SH_MAG [ 1 ] + P [ 18 ] [ 3 ] * SH_MAG [ 2 ] + P [ 18 ] [ 0 ] * SK_MX [ 2 ] - P [ 18 ] [ 16 ] * SK_MX [ 1 ] + P [ 18 ] [ 17 ] * SK_MX [ 4 ] - P [ 18 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 19 ] = SK_MX [ 0 ] * ( P [ 19 ] [ 19 ] + P [ 19 ] [ 1 ] * SH_MAG [ 0 ] - P [ 19 ] [ 2 ] * SH_MAG [ 1 ] + P [ 19 ] [ 3 ] * SH_MAG [ 2 ] + P [ 19 ] [ 0 ] * SK_MX [ 2 ] - P [ 19 ] [ 16 ] * SK_MX [ 1 ] + P [ 19 ] [ 17 ] * SK_MX [ 4 ] - P [ 19 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 20 ] = SK_MX [ 0 ] * ( P [ 20 ] [ 19 ] + P [ 20 ] [ 1 ] * SH_MAG [ 0 ] - P [ 20 ] [ 2 ] * SH_MAG [ 1 ] + P [ 20 ] [ 3 ] * SH_MAG [ 2 ] + P [ 20 ] [ 0 ] * SK_MX [ 2 ] - P [ 20 ] [ 16 ] * SK_MX [ 1 ] + P [ 20 ] [ 17 ] * SK_MX [ 4 ] - P [ 20 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 21 ] = SK_MX [ 0 ] * ( P [ 21 ] [ 19 ] + P [ 21 ] [ 1 ] * SH_MAG [ 0 ] - P [ 21 ] [ 2 ] * SH_MAG [ 1 ] + P [ 21 ] [ 3 ] * SH_MAG [ 2 ] + P [ 21 ] [ 0 ] * SK_MX [ 2 ] - P [ 21 ] [ 16 ] * SK_MX [ 1 ] + P [ 21 ] [ 17 ] * SK_MX [ 4 ] - P [ 21 ] [ 18 ] * SK_MX [ 3 ] ) ;
} 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
}
// zero Kalman gains to inhibit wind state estimation
2023-09-26 19:18:07 -03:00
if ( ! inhibitWindStates & & ! treatWindStatesAsTruth ) {
2017-05-09 19:31:55 -03:00
Kfusion [ 22 ] = SK_MX [ 0 ] * ( P [ 22 ] [ 19 ] + P [ 22 ] [ 1 ] * SH_MAG [ 0 ] - P [ 22 ] [ 2 ] * SH_MAG [ 1 ] + P [ 22 ] [ 3 ] * SH_MAG [ 2 ] + P [ 22 ] [ 0 ] * SK_MX [ 2 ] - P [ 22 ] [ 16 ] * SK_MX [ 1 ] + P [ 22 ] [ 17 ] * SK_MX [ 4 ] - P [ 22 ] [ 18 ] * SK_MX [ 3 ] ) ;
Kfusion [ 23 ] = SK_MX [ 0 ] * ( P [ 23 ] [ 19 ] + P [ 23 ] [ 1 ] * SH_MAG [ 0 ] - P [ 23 ] [ 2 ] * SH_MAG [ 1 ] + P [ 23 ] [ 3 ] * SH_MAG [ 2 ] + P [ 23 ] [ 0 ] * SK_MX [ 2 ] - P [ 23 ] [ 16 ] * SK_MX [ 1 ] + P [ 23 ] [ 17 ] * SK_MX [ 4 ] - P [ 23 ] [ 18 ] * SK_MX [ 3 ] ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 22 to 23 = 2
zero_range ( & Kfusion [ 0 ] , 22 , 23 ) ;
2016-07-14 02:08:43 -03:00
}
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true ;
} else if ( obsIndex = = 1 ) { // Fuse Y axis
// calculate observation jacobians
for ( uint8_t i = 0 ; i < = stateIndexLim ; i + + ) H_MAG [ i ] = 0.0f ;
H_MAG [ 0 ] = SH_MAG [ 2 ] ;
H_MAG [ 1 ] = SH_MAG [ 1 ] ;
H_MAG [ 2 ] = SH_MAG [ 0 ] ;
H_MAG [ 3 ] = 2.0f * magD * q2 - SH_MAG [ 8 ] - SH_MAG [ 7 ] ;
H_MAG [ 16 ] = 2.0f * q1 * q2 - 2.0f * q0 * q3 ;
H_MAG [ 17 ] = SH_MAG [ 4 ] - SH_MAG [ 3 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ;
H_MAG [ 18 ] = 2.0f * q0 * q1 + 2.0f * q2 * q3 ;
2019-09-20 22:38:33 -03:00
H_MAG [ 19 ] = 0.0f ;
2016-07-14 02:08:43 -03:00
H_MAG [ 20 ] = 1.0f ;
2019-09-20 22:38:33 -03:00
H_MAG [ 21 ] = 0.0f ;
2016-07-14 02:08:43 -03:00
// calculate Kalman gain
2023-01-11 08:17:44 -04:00
const Vector5 SK_MY {
1.0f / varInnovMag [ 1 ] ,
SH_MAG [ 3 ] - SH_MAG [ 4 ] + SH_MAG [ 5 ] - SH_MAG [ 6 ] ,
SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ,
2.0f * q0 * q3 - 2.0f * q1 * q2 ,
2.0f * q0 * q1 + 2.0f * q2 * q3
} ;
2016-07-14 02:08:43 -03:00
Kfusion [ 0 ] = SK_MY [ 0 ] * ( P [ 0 ] [ 20 ] + P [ 0 ] [ 0 ] * SH_MAG [ 2 ] + P [ 0 ] [ 1 ] * SH_MAG [ 1 ] + P [ 0 ] [ 2 ] * SH_MAG [ 0 ] - P [ 0 ] [ 3 ] * SK_MY [ 2 ] - P [ 0 ] [ 17 ] * SK_MY [ 1 ] - P [ 0 ] [ 16 ] * SK_MY [ 3 ] + P [ 0 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 1 ] = SK_MY [ 0 ] * ( P [ 1 ] [ 20 ] + P [ 1 ] [ 0 ] * SH_MAG [ 2 ] + P [ 1 ] [ 1 ] * SH_MAG [ 1 ] + P [ 1 ] [ 2 ] * SH_MAG [ 0 ] - P [ 1 ] [ 3 ] * SK_MY [ 2 ] - P [ 1 ] [ 17 ] * SK_MY [ 1 ] - P [ 1 ] [ 16 ] * SK_MY [ 3 ] + P [ 1 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 2 ] = SK_MY [ 0 ] * ( P [ 2 ] [ 20 ] + P [ 2 ] [ 0 ] * SH_MAG [ 2 ] + P [ 2 ] [ 1 ] * SH_MAG [ 1 ] + P [ 2 ] [ 2 ] * SH_MAG [ 0 ] - P [ 2 ] [ 3 ] * SK_MY [ 2 ] - P [ 2 ] [ 17 ] * SK_MY [ 1 ] - P [ 2 ] [ 16 ] * SK_MY [ 3 ] + P [ 2 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 3 ] = SK_MY [ 0 ] * ( P [ 3 ] [ 20 ] + P [ 3 ] [ 0 ] * SH_MAG [ 2 ] + P [ 3 ] [ 1 ] * SH_MAG [ 1 ] + P [ 3 ] [ 2 ] * SH_MAG [ 0 ] - P [ 3 ] [ 3 ] * SK_MY [ 2 ] - P [ 3 ] [ 17 ] * SK_MY [ 1 ] - P [ 3 ] [ 16 ] * SK_MY [ 3 ] + P [ 3 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 4 ] = SK_MY [ 0 ] * ( P [ 4 ] [ 20 ] + P [ 4 ] [ 0 ] * SH_MAG [ 2 ] + P [ 4 ] [ 1 ] * SH_MAG [ 1 ] + P [ 4 ] [ 2 ] * SH_MAG [ 0 ] - P [ 4 ] [ 3 ] * SK_MY [ 2 ] - P [ 4 ] [ 17 ] * SK_MY [ 1 ] - P [ 4 ] [ 16 ] * SK_MY [ 3 ] + P [ 4 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 5 ] = SK_MY [ 0 ] * ( P [ 5 ] [ 20 ] + P [ 5 ] [ 0 ] * SH_MAG [ 2 ] + P [ 5 ] [ 1 ] * SH_MAG [ 1 ] + P [ 5 ] [ 2 ] * SH_MAG [ 0 ] - P [ 5 ] [ 3 ] * SK_MY [ 2 ] - P [ 5 ] [ 17 ] * SK_MY [ 1 ] - P [ 5 ] [ 16 ] * SK_MY [ 3 ] + P [ 5 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 6 ] = SK_MY [ 0 ] * ( P [ 6 ] [ 20 ] + P [ 6 ] [ 0 ] * SH_MAG [ 2 ] + P [ 6 ] [ 1 ] * SH_MAG [ 1 ] + P [ 6 ] [ 2 ] * SH_MAG [ 0 ] - P [ 6 ] [ 3 ] * SK_MY [ 2 ] - P [ 6 ] [ 17 ] * SK_MY [ 1 ] - P [ 6 ] [ 16 ] * SK_MY [ 3 ] + P [ 6 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 7 ] = SK_MY [ 0 ] * ( P [ 7 ] [ 20 ] + P [ 7 ] [ 0 ] * SH_MAG [ 2 ] + P [ 7 ] [ 1 ] * SH_MAG [ 1 ] + P [ 7 ] [ 2 ] * SH_MAG [ 0 ] - P [ 7 ] [ 3 ] * SK_MY [ 2 ] - P [ 7 ] [ 17 ] * SK_MY [ 1 ] - P [ 7 ] [ 16 ] * SK_MY [ 3 ] + P [ 7 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 8 ] = SK_MY [ 0 ] * ( P [ 8 ] [ 20 ] + P [ 8 ] [ 0 ] * SH_MAG [ 2 ] + P [ 8 ] [ 1 ] * SH_MAG [ 1 ] + P [ 8 ] [ 2 ] * SH_MAG [ 0 ] - P [ 8 ] [ 3 ] * SK_MY [ 2 ] - P [ 8 ] [ 17 ] * SK_MY [ 1 ] - P [ 8 ] [ 16 ] * SK_MY [ 3 ] + P [ 8 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 9 ] = SK_MY [ 0 ] * ( P [ 9 ] [ 20 ] + P [ 9 ] [ 0 ] * SH_MAG [ 2 ] + P [ 9 ] [ 1 ] * SH_MAG [ 1 ] + P [ 9 ] [ 2 ] * SH_MAG [ 0 ] - P [ 9 ] [ 3 ] * SK_MY [ 2 ] - P [ 9 ] [ 17 ] * SK_MY [ 1 ] - P [ 9 ] [ 16 ] * SK_MY [ 3 ] + P [ 9 ] [ 18 ] * SK_MY [ 4 ] ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = SK_MY [ 0 ] * ( P [ 10 ] [ 20 ] + P [ 10 ] [ 0 ] * SH_MAG [ 2 ] + P [ 10 ] [ 1 ] * SH_MAG [ 1 ] + P [ 10 ] [ 2 ] * SH_MAG [ 0 ] - P [ 10 ] [ 3 ] * SK_MY [ 2 ] - P [ 10 ] [ 17 ] * SK_MY [ 1 ] - P [ 10 ] [ 16 ] * SK_MY [ 3 ] + P [ 10 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 11 ] = SK_MY [ 0 ] * ( P [ 11 ] [ 20 ] + P [ 11 ] [ 0 ] * SH_MAG [ 2 ] + P [ 11 ] [ 1 ] * SH_MAG [ 1 ] + P [ 11 ] [ 2 ] * SH_MAG [ 0 ] - P [ 11 ] [ 3 ] * SK_MY [ 2 ] - P [ 11 ] [ 17 ] * SK_MY [ 1 ] - P [ 11 ] [ 16 ] * SK_MY [ 3 ] + P [ 11 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 12 ] = SK_MY [ 0 ] * ( P [ 12 ] [ 20 ] + P [ 12 ] [ 0 ] * SH_MAG [ 2 ] + P [ 12 ] [ 1 ] * SH_MAG [ 1 ] + P [ 12 ] [ 2 ] * SH_MAG [ 0 ] - P [ 12 ] [ 3 ] * SK_MY [ 2 ] - P [ 12 ] [ 17 ] * SK_MY [ 1 ] - P [ 12 ] [ 16 ] * SK_MY [ 3 ] + P [ 12 ] [ 18 ] * SK_MY [ 4 ] ) ;
} 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
}
if ( ! inhibitDelVelBiasStates ) {
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 ] = SK_MY [ 0 ] * ( P [ stateIndex ] [ 20 ] + P [ stateIndex ] [ 0 ] * SH_MAG [ 2 ] + P [ stateIndex ] [ 1 ] * SH_MAG [ 1 ] + P [ stateIndex ] [ 2 ] * SH_MAG [ 0 ] - P [ stateIndex ] [ 3 ] * SK_MY [ 2 ] - P [ stateIndex ] [ 17 ] * SK_MY [ 1 ] - P [ stateIndex ] [ 16 ] * SK_MY [ 3 ] + P [ stateIndex ] [ 18 ] * SK_MY [ 4 ] ) ;
} 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
// zero Kalman gains to inhibit magnetic field state estimation
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = SK_MY [ 0 ] * ( P [ 16 ] [ 20 ] + P [ 16 ] [ 0 ] * SH_MAG [ 2 ] + P [ 16 ] [ 1 ] * SH_MAG [ 1 ] + P [ 16 ] [ 2 ] * SH_MAG [ 0 ] - P [ 16 ] [ 3 ] * SK_MY [ 2 ] - P [ 16 ] [ 17 ] * SK_MY [ 1 ] - P [ 16 ] [ 16 ] * SK_MY [ 3 ] + P [ 16 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 17 ] = SK_MY [ 0 ] * ( P [ 17 ] [ 20 ] + P [ 17 ] [ 0 ] * SH_MAG [ 2 ] + P [ 17 ] [ 1 ] * SH_MAG [ 1 ] + P [ 17 ] [ 2 ] * SH_MAG [ 0 ] - P [ 17 ] [ 3 ] * SK_MY [ 2 ] - P [ 17 ] [ 17 ] * SK_MY [ 1 ] - P [ 17 ] [ 16 ] * SK_MY [ 3 ] + P [ 17 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 18 ] = SK_MY [ 0 ] * ( P [ 18 ] [ 20 ] + P [ 18 ] [ 0 ] * SH_MAG [ 2 ] + P [ 18 ] [ 1 ] * SH_MAG [ 1 ] + P [ 18 ] [ 2 ] * SH_MAG [ 0 ] - P [ 18 ] [ 3 ] * SK_MY [ 2 ] - P [ 18 ] [ 17 ] * SK_MY [ 1 ] - P [ 18 ] [ 16 ] * SK_MY [ 3 ] + P [ 18 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 19 ] = SK_MY [ 0 ] * ( P [ 19 ] [ 20 ] + P [ 19 ] [ 0 ] * SH_MAG [ 2 ] + P [ 19 ] [ 1 ] * SH_MAG [ 1 ] + P [ 19 ] [ 2 ] * SH_MAG [ 0 ] - P [ 19 ] [ 3 ] * SK_MY [ 2 ] - P [ 19 ] [ 17 ] * SK_MY [ 1 ] - P [ 19 ] [ 16 ] * SK_MY [ 3 ] + P [ 19 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 20 ] = SK_MY [ 0 ] * ( P [ 20 ] [ 20 ] + P [ 20 ] [ 0 ] * SH_MAG [ 2 ] + P [ 20 ] [ 1 ] * SH_MAG [ 1 ] + P [ 20 ] [ 2 ] * SH_MAG [ 0 ] - P [ 20 ] [ 3 ] * SK_MY [ 2 ] - P [ 20 ] [ 17 ] * SK_MY [ 1 ] - P [ 20 ] [ 16 ] * SK_MY [ 3 ] + P [ 20 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 21 ] = SK_MY [ 0 ] * ( P [ 21 ] [ 20 ] + P [ 21 ] [ 0 ] * SH_MAG [ 2 ] + P [ 21 ] [ 1 ] * SH_MAG [ 1 ] + P [ 21 ] [ 2 ] * SH_MAG [ 0 ] - P [ 21 ] [ 3 ] * SK_MY [ 2 ] - P [ 21 ] [ 17 ] * SK_MY [ 1 ] - P [ 21 ] [ 16 ] * SK_MY [ 3 ] + P [ 21 ] [ 18 ] * SK_MY [ 4 ] ) ;
} 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
}
// zero Kalman gains to inhibit wind state estimation
2023-09-26 19:18:07 -03:00
if ( ! inhibitWindStates & & ! treatWindStatesAsTruth ) {
2017-05-09 19:31:55 -03:00
Kfusion [ 22 ] = SK_MY [ 0 ] * ( P [ 22 ] [ 20 ] + P [ 22 ] [ 0 ] * SH_MAG [ 2 ] + P [ 22 ] [ 1 ] * SH_MAG [ 1 ] + P [ 22 ] [ 2 ] * SH_MAG [ 0 ] - P [ 22 ] [ 3 ] * SK_MY [ 2 ] - P [ 22 ] [ 17 ] * SK_MY [ 1 ] - P [ 22 ] [ 16 ] * SK_MY [ 3 ] + P [ 22 ] [ 18 ] * SK_MY [ 4 ] ) ;
Kfusion [ 23 ] = SK_MY [ 0 ] * ( P [ 23 ] [ 20 ] + P [ 23 ] [ 0 ] * SH_MAG [ 2 ] + P [ 23 ] [ 1 ] * SH_MAG [ 1 ] + P [ 23 ] [ 2 ] * SH_MAG [ 0 ] - P [ 23 ] [ 3 ] * SK_MY [ 2 ] - P [ 23 ] [ 17 ] * SK_MY [ 1 ] - P [ 23 ] [ 16 ] * SK_MY [ 3 ] + P [ 23 ] [ 18 ] * SK_MY [ 4 ] ) ;
} 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
}
2019-02-22 19:35:24 -04:00
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
2016-07-14 02:08:43 -03:00
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true ;
}
else if ( obsIndex = = 2 ) // we are now fusing the Z measurement
{
// calculate observation jacobians
for ( uint8_t i = 0 ; i < = stateIndexLim ; i + + ) H_MAG [ i ] = 0.0f ;
H_MAG [ 0 ] = SH_MAG [ 1 ] ;
H_MAG [ 1 ] = - SH_MAG [ 2 ] ;
H_MAG [ 2 ] = SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ;
H_MAG [ 3 ] = SH_MAG [ 0 ] ;
H_MAG [ 16 ] = 2.0f * q0 * q2 + 2.0f * q1 * q3 ;
H_MAG [ 17 ] = 2.0f * q2 * q3 - 2.0f * q0 * q1 ;
H_MAG [ 18 ] = SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ;
2019-09-20 22:38:33 -03:00
H_MAG [ 19 ] = 0.0f ;
H_MAG [ 20 ] = 0.0f ;
2016-07-14 02:08:43 -03:00
H_MAG [ 21 ] = 1.0f ;
// calculate Kalman gain
2023-01-11 08:17:44 -04:00
const Vector5 SK_MZ {
1.0f / varInnovMag [ 2 ] ,
SH_MAG [ 3 ] - SH_MAG [ 4 ] - SH_MAG [ 5 ] + SH_MAG [ 6 ] ,
SH_MAG [ 7 ] + SH_MAG [ 8 ] - 2.0f * magD * q2 ,
2.0f * q0 * q1 - 2.0f * q2 * q3 ,
2.0f * q0 * q2 + 2.0f * q1 * q3
} ;
2016-07-14 02:08:43 -03:00
Kfusion [ 0 ] = SK_MZ [ 0 ] * ( P [ 0 ] [ 21 ] + P [ 0 ] [ 0 ] * SH_MAG [ 1 ] - P [ 0 ] [ 1 ] * SH_MAG [ 2 ] + P [ 0 ] [ 3 ] * SH_MAG [ 0 ] + P [ 0 ] [ 2 ] * SK_MZ [ 2 ] + P [ 0 ] [ 18 ] * SK_MZ [ 1 ] + P [ 0 ] [ 16 ] * SK_MZ [ 4 ] - P [ 0 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 1 ] = SK_MZ [ 0 ] * ( P [ 1 ] [ 21 ] + P [ 1 ] [ 0 ] * SH_MAG [ 1 ] - P [ 1 ] [ 1 ] * SH_MAG [ 2 ] + P [ 1 ] [ 3 ] * SH_MAG [ 0 ] + P [ 1 ] [ 2 ] * SK_MZ [ 2 ] + P [ 1 ] [ 18 ] * SK_MZ [ 1 ] + P [ 1 ] [ 16 ] * SK_MZ [ 4 ] - P [ 1 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 2 ] = SK_MZ [ 0 ] * ( P [ 2 ] [ 21 ] + P [ 2 ] [ 0 ] * SH_MAG [ 1 ] - P [ 2 ] [ 1 ] * SH_MAG [ 2 ] + P [ 2 ] [ 3 ] * SH_MAG [ 0 ] + P [ 2 ] [ 2 ] * SK_MZ [ 2 ] + P [ 2 ] [ 18 ] * SK_MZ [ 1 ] + P [ 2 ] [ 16 ] * SK_MZ [ 4 ] - P [ 2 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 3 ] = SK_MZ [ 0 ] * ( P [ 3 ] [ 21 ] + P [ 3 ] [ 0 ] * SH_MAG [ 1 ] - P [ 3 ] [ 1 ] * SH_MAG [ 2 ] + P [ 3 ] [ 3 ] * SH_MAG [ 0 ] + P [ 3 ] [ 2 ] * SK_MZ [ 2 ] + P [ 3 ] [ 18 ] * SK_MZ [ 1 ] + P [ 3 ] [ 16 ] * SK_MZ [ 4 ] - P [ 3 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 4 ] = SK_MZ [ 0 ] * ( P [ 4 ] [ 21 ] + P [ 4 ] [ 0 ] * SH_MAG [ 1 ] - P [ 4 ] [ 1 ] * SH_MAG [ 2 ] + P [ 4 ] [ 3 ] * SH_MAG [ 0 ] + P [ 4 ] [ 2 ] * SK_MZ [ 2 ] + P [ 4 ] [ 18 ] * SK_MZ [ 1 ] + P [ 4 ] [ 16 ] * SK_MZ [ 4 ] - P [ 4 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 5 ] = SK_MZ [ 0 ] * ( P [ 5 ] [ 21 ] + P [ 5 ] [ 0 ] * SH_MAG [ 1 ] - P [ 5 ] [ 1 ] * SH_MAG [ 2 ] + P [ 5 ] [ 3 ] * SH_MAG [ 0 ] + P [ 5 ] [ 2 ] * SK_MZ [ 2 ] + P [ 5 ] [ 18 ] * SK_MZ [ 1 ] + P [ 5 ] [ 16 ] * SK_MZ [ 4 ] - P [ 5 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 6 ] = SK_MZ [ 0 ] * ( P [ 6 ] [ 21 ] + P [ 6 ] [ 0 ] * SH_MAG [ 1 ] - P [ 6 ] [ 1 ] * SH_MAG [ 2 ] + P [ 6 ] [ 3 ] * SH_MAG [ 0 ] + P [ 6 ] [ 2 ] * SK_MZ [ 2 ] + P [ 6 ] [ 18 ] * SK_MZ [ 1 ] + P [ 6 ] [ 16 ] * SK_MZ [ 4 ] - P [ 6 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 7 ] = SK_MZ [ 0 ] * ( P [ 7 ] [ 21 ] + P [ 7 ] [ 0 ] * SH_MAG [ 1 ] - P [ 7 ] [ 1 ] * SH_MAG [ 2 ] + P [ 7 ] [ 3 ] * SH_MAG [ 0 ] + P [ 7 ] [ 2 ] * SK_MZ [ 2 ] + P [ 7 ] [ 18 ] * SK_MZ [ 1 ] + P [ 7 ] [ 16 ] * SK_MZ [ 4 ] - P [ 7 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 8 ] = SK_MZ [ 0 ] * ( P [ 8 ] [ 21 ] + P [ 8 ] [ 0 ] * SH_MAG [ 1 ] - P [ 8 ] [ 1 ] * SH_MAG [ 2 ] + P [ 8 ] [ 3 ] * SH_MAG [ 0 ] + P [ 8 ] [ 2 ] * SK_MZ [ 2 ] + P [ 8 ] [ 18 ] * SK_MZ [ 1 ] + P [ 8 ] [ 16 ] * SK_MZ [ 4 ] - P [ 8 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 9 ] = SK_MZ [ 0 ] * ( P [ 9 ] [ 21 ] + P [ 9 ] [ 0 ] * SH_MAG [ 1 ] - P [ 9 ] [ 1 ] * SH_MAG [ 2 ] + P [ 9 ] [ 3 ] * SH_MAG [ 0 ] + P [ 9 ] [ 2 ] * SK_MZ [ 2 ] + P [ 9 ] [ 18 ] * SK_MZ [ 1 ] + P [ 9 ] [ 16 ] * SK_MZ [ 4 ] - P [ 9 ] [ 17 ] * SK_MZ [ 3 ] ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = SK_MZ [ 0 ] * ( P [ 10 ] [ 21 ] + P [ 10 ] [ 0 ] * SH_MAG [ 1 ] - P [ 10 ] [ 1 ] * SH_MAG [ 2 ] + P [ 10 ] [ 3 ] * SH_MAG [ 0 ] + P [ 10 ] [ 2 ] * SK_MZ [ 2 ] + P [ 10 ] [ 18 ] * SK_MZ [ 1 ] + P [ 10 ] [ 16 ] * SK_MZ [ 4 ] - P [ 10 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 11 ] = SK_MZ [ 0 ] * ( P [ 11 ] [ 21 ] + P [ 11 ] [ 0 ] * SH_MAG [ 1 ] - P [ 11 ] [ 1 ] * SH_MAG [ 2 ] + P [ 11 ] [ 3 ] * SH_MAG [ 0 ] + P [ 11 ] [ 2 ] * SK_MZ [ 2 ] + P [ 11 ] [ 18 ] * SK_MZ [ 1 ] + P [ 11 ] [ 16 ] * SK_MZ [ 4 ] - P [ 11 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 12 ] = SK_MZ [ 0 ] * ( P [ 12 ] [ 21 ] + P [ 12 ] [ 0 ] * SH_MAG [ 1 ] - P [ 12 ] [ 1 ] * SH_MAG [ 2 ] + P [ 12 ] [ 3 ] * SH_MAG [ 0 ] + P [ 12 ] [ 2 ] * SK_MZ [ 2 ] + P [ 12 ] [ 18 ] * SK_MZ [ 1 ] + P [ 12 ] [ 16 ] * SK_MZ [ 4 ] - P [ 12 ] [ 17 ] * SK_MZ [ 3 ] ) ;
} 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
}
if ( ! inhibitDelVelBiasStates ) {
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 ] = SK_MZ [ 0 ] * ( P [ stateIndex ] [ 21 ] + P [ stateIndex ] [ 0 ] * SH_MAG [ 1 ] - P [ stateIndex ] [ 1 ] * SH_MAG [ 2 ] + P [ stateIndex ] [ 3 ] * SH_MAG [ 0 ] + P [ stateIndex ] [ 2 ] * SK_MZ [ 2 ] + P [ stateIndex ] [ 18 ] * SK_MZ [ 1 ] + P [ stateIndex ] [ 16 ] * SK_MZ [ 4 ] - P [ stateIndex ] [ 17 ] * SK_MZ [ 3 ] ) ;
} 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
// zero Kalman gains to inhibit magnetic field state estimation
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = SK_MZ [ 0 ] * ( P [ 16 ] [ 21 ] + P [ 16 ] [ 0 ] * SH_MAG [ 1 ] - P [ 16 ] [ 1 ] * SH_MAG [ 2 ] + P [ 16 ] [ 3 ] * SH_MAG [ 0 ] + P [ 16 ] [ 2 ] * SK_MZ [ 2 ] + P [ 16 ] [ 18 ] * SK_MZ [ 1 ] + P [ 16 ] [ 16 ] * SK_MZ [ 4 ] - P [ 16 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 17 ] = SK_MZ [ 0 ] * ( P [ 17 ] [ 21 ] + P [ 17 ] [ 0 ] * SH_MAG [ 1 ] - P [ 17 ] [ 1 ] * SH_MAG [ 2 ] + P [ 17 ] [ 3 ] * SH_MAG [ 0 ] + P [ 17 ] [ 2 ] * SK_MZ [ 2 ] + P [ 17 ] [ 18 ] * SK_MZ [ 1 ] + P [ 17 ] [ 16 ] * SK_MZ [ 4 ] - P [ 17 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 18 ] = SK_MZ [ 0 ] * ( P [ 18 ] [ 21 ] + P [ 18 ] [ 0 ] * SH_MAG [ 1 ] - P [ 18 ] [ 1 ] * SH_MAG [ 2 ] + P [ 18 ] [ 3 ] * SH_MAG [ 0 ] + P [ 18 ] [ 2 ] * SK_MZ [ 2 ] + P [ 18 ] [ 18 ] * SK_MZ [ 1 ] + P [ 18 ] [ 16 ] * SK_MZ [ 4 ] - P [ 18 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 19 ] = SK_MZ [ 0 ] * ( P [ 19 ] [ 21 ] + P [ 19 ] [ 0 ] * SH_MAG [ 1 ] - P [ 19 ] [ 1 ] * SH_MAG [ 2 ] + P [ 19 ] [ 3 ] * SH_MAG [ 0 ] + P [ 19 ] [ 2 ] * SK_MZ [ 2 ] + P [ 19 ] [ 18 ] * SK_MZ [ 1 ] + P [ 19 ] [ 16 ] * SK_MZ [ 4 ] - P [ 19 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 20 ] = SK_MZ [ 0 ] * ( P [ 20 ] [ 21 ] + P [ 20 ] [ 0 ] * SH_MAG [ 1 ] - P [ 20 ] [ 1 ] * SH_MAG [ 2 ] + P [ 20 ] [ 3 ] * SH_MAG [ 0 ] + P [ 20 ] [ 2 ] * SK_MZ [ 2 ] + P [ 20 ] [ 18 ] * SK_MZ [ 1 ] + P [ 20 ] [ 16 ] * SK_MZ [ 4 ] - P [ 20 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 21 ] = SK_MZ [ 0 ] * ( P [ 21 ] [ 21 ] + P [ 21 ] [ 0 ] * SH_MAG [ 1 ] - P [ 21 ] [ 1 ] * SH_MAG [ 2 ] + P [ 21 ] [ 3 ] * SH_MAG [ 0 ] + P [ 21 ] [ 2 ] * SK_MZ [ 2 ] + P [ 21 ] [ 18 ] * SK_MZ [ 1 ] + P [ 21 ] [ 16 ] * SK_MZ [ 4 ] - P [ 21 ] [ 17 ] * SK_MZ [ 3 ] ) ;
} 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
}
// zero Kalman gains to inhibit wind state estimation
2023-09-26 19:18:07 -03:00
if ( ! inhibitWindStates & & ! treatWindStatesAsTruth ) {
2017-05-09 19:31:55 -03:00
Kfusion [ 22 ] = SK_MZ [ 0 ] * ( P [ 22 ] [ 21 ] + P [ 22 ] [ 0 ] * SH_MAG [ 1 ] - P [ 22 ] [ 1 ] * SH_MAG [ 2 ] + P [ 22 ] [ 3 ] * SH_MAG [ 0 ] + P [ 22 ] [ 2 ] * SK_MZ [ 2 ] + P [ 22 ] [ 18 ] * SK_MZ [ 1 ] + P [ 22 ] [ 16 ] * SK_MZ [ 4 ] - P [ 22 ] [ 17 ] * SK_MZ [ 3 ] ) ;
Kfusion [ 23 ] = SK_MZ [ 0 ] * ( P [ 23 ] [ 21 ] + P [ 23 ] [ 0 ] * SH_MAG [ 1 ] - P [ 23 ] [ 1 ] * SH_MAG [ 2 ] + P [ 23 ] [ 3 ] * SH_MAG [ 0 ] + P [ 23 ] [ 2 ] * SK_MZ [ 2 ] + P [ 23 ] [ 18 ] * SK_MZ [ 1 ] + P [ 23 ] [ 16 ] * SK_MZ [ 4 ] - P [ 23 ] [ 17 ] * SK_MZ [ 3 ] ) ;
} 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
}
2019-02-22 19:35:24 -04:00
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
2016-07-14 02:08:43 -03:00
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true ;
}
// 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 < = 3 ; j + + ) {
KH [ i ] [ j ] = Kfusion [ i ] * H_MAG [ j ] ;
}
for ( unsigned j = 4 ; j < = 15 ; j + + ) {
KH [ i ] [ j ] = 0.0f ;
}
for ( unsigned j = 16 ; j < = 21 ; j + + ) {
KH [ i ] [ j ] = Kfusion [ i ] * H_MAG [ j ] ;
}
for ( unsigned j = 22 ; j < = 23 ; 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 ] [ 16 ] * P [ 16 ] [ j ] ;
res + = KH [ i ] [ 17 ] * P [ 17 ] [ j ] ;
res + = KH [ i ] [ 18 ] * P [ 18 ] [ j ] ;
res + = KH [ i ] [ 19 ] * P [ 19 ] [ j ] ;
res + = KH [ i ] [ 20 ] * P [ 20 ] [ j ] ;
res + = KH [ i ] [ 21 ] * P [ 21 ] [ 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 + + ) {
statesArray [ j ] = statesArray [ j ] - Kfusion [ j ] * innovMag [ obsIndex ] ;
}
2019-09-22 09:28:19 -03:00
// add table constraint here for faster convergence
if ( have_table_earth_field & & frontend - > _mag_ef_limit > 0 ) {
MagTableConstrain ( ) ;
}
2016-07-14 02:08:43 -03:00
stateStruct . quat . normalize ( ) ;
} else {
// record bad axis
if ( obsIndex = = 0 ) {
faultStatus . bad_xmag = true ;
} else if ( obsIndex = = 1 ) {
faultStatus . bad_ymag = true ;
} else if ( obsIndex = = 2 ) {
faultStatus . bad_zmag = true ;
}
2017-04-27 03:39:49 -03:00
CovarianceInit ( ) ;
return ;
2016-07-14 02:08:43 -03:00
}
}
}
/*
2020-12-14 17:43:46 -04:00
* Fuse direct yaw measurements using explicit algebraic equations auto - generated from
2020-09-16 09:04:40 -03:00
* / AP_NavEKF3 / derivation / main . py with output recorded in / AP_NavEKF3 / derivation / generated / yaw_generated . cpp
2020-12-14 17:43:46 -04:00
* Returns true if the fusion was successful
2016-07-14 02:08:43 -03:00
*/
2020-12-14 17:43:46 -04:00
bool NavEKF3_core : : fuseEulerYaw ( yawFusionMethod method )
2016-07-14 02:08:43 -03:00
{
2021-05-04 08:12:23 -03:00
const ftype & q0 = stateStruct . quat [ 0 ] ;
const ftype & q1 = stateStruct . quat [ 1 ] ;
const ftype & q2 = stateStruct . quat [ 2 ] ;
const ftype & q3 = stateStruct . quat [ 3 ] ;
2016-07-14 02:08:43 -03:00
2021-05-04 08:12:23 -03:00
ftype gsfYaw , gsfYawVariance ;
2020-12-14 17:43:46 -04:00
if ( method = = yawFusionMethod : : GSF ) {
2020-12-15 03:20:31 -04:00
if ( ! EKFGSF_getYaw ( gsfYaw , gsfYawVariance ) ) {
2020-12-14 17:43:46 -04:00
return false ;
}
2020-03-10 03:48:08 -03:00
}
2017-07-19 21:40:11 -03:00
// yaw measurement error variance (rad^2)
2021-05-04 08:12:23 -03:00
ftype R_YAW ;
2020-12-14 17:43:46 -04:00
switch ( method ) {
2020-12-22 04:08:30 -04:00
case yawFusionMethod : : GPS :
2020-12-14 17:43:46 -04:00
R_YAW = sq ( yawAngDataDelayed . yawAngErr ) ;
break ;
case yawFusionMethod : : GSF :
2020-03-10 03:48:08 -03:00
R_YAW = gsfYawVariance ;
2020-12-14 17:43:46 -04:00
break ;
case yawFusionMethod : : STATIC :
R_YAW = sq ( yawAngDataStatic . yawAngErr ) ;
break ;
2020-12-15 03:28:00 -04:00
case yawFusionMethod : : MAGNETOMETER :
2020-12-14 17:43:46 -04:00
case yawFusionMethod : : PREDICTED :
default :
2017-07-19 21:40:11 -03:00
R_YAW = sq ( frontend - > _yawNoise ) ;
2020-12-15 03:50:31 -04:00
break ;
2020-11-30 08:35:24 -04:00
2021-01-19 00:27:03 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2020-11-30 08:35:24 -04:00
case yawFusionMethod : : EXTNAV :
R_YAW = sq ( MAX ( extNavYawAngDataDelayed . yawAngErr , 0.05f ) ) ;
break ;
2021-01-19 00:27:03 -04:00
# endif
2017-07-19 21:40:11 -03:00
}
2016-07-14 02:08:43 -03:00
// determine if a 321 or 312 Euler sequence is best
2020-12-14 17:43:46 -04:00
rotationOrder order ;
switch ( method ) {
2020-12-22 04:08:30 -04:00
case yawFusionMethod : : GPS :
2020-12-14 17:43:46 -04:00
order = yawAngDataDelayed . order ;
break ;
case yawFusionMethod : : STATIC :
order = yawAngDataStatic . order ;
break ;
2020-12-15 03:28:00 -04:00
case yawFusionMethod : : MAGNETOMETER :
2020-12-14 17:43:46 -04:00
case yawFusionMethod : : GSF :
case yawFusionMethod : : PREDICTED :
default :
// determined automatically
2021-05-04 08:12:23 -03:00
order = ( fabsF ( prevTnb [ 0 ] [ 2 ] ) < fabsF ( prevTnb [ 1 ] [ 2 ] ) ) ? rotationOrder : : TAIT_BRYAN_321 : rotationOrder : : TAIT_BRYAN_312 ;
2020-12-15 03:50:31 -04:00
break ;
2020-11-30 08:35:24 -04:00
2021-01-19 00:27:03 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2020-11-30 08:35:24 -04:00
case yawFusionMethod : : EXTNAV :
order = extNavYawAngDataDelayed . order ;
break ;
2021-01-19 00:27:03 -04:00
# endif
2017-07-19 21:40:11 -03:00
}
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
2021-05-04 08:12:23 -03:00
ftype yawAngPredicted ;
ftype H_YAW [ 4 ] ;
Matrix3F Tbn_zeroYaw ;
2017-07-19 21:40:11 -03:00
2020-12-14 17:43:46 -04:00
if ( order = = rotationOrder : : TAIT_BRYAN_321 ) {
2020-09-16 09:04:40 -03:00
// calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false ;
2021-05-04 08:12:23 -03:00
const ftype SA0 = 2 * q3 ;
const ftype SA1 = 2 * q2 ;
const ftype SA2 = SA0 * q0 + SA1 * q1 ;
const ftype SA3 = sq ( q0 ) + sq ( q1 ) - sq ( q2 ) - sq ( q3 ) ;
ftype SA4 , SA5_inv ;
2020-09-16 09:04:40 -03:00
if ( is_positive ( sq ( SA3 ) ) ) {
SA4 = 1.0F / sq ( SA3 ) ;
SA5_inv = sq ( SA2 ) * SA4 + 1 ;
2021-05-04 08:12:23 -03:00
canUseA = is_positive ( fabsF ( SA5_inv ) ) ;
2020-09-16 09:04:40 -03:00
}
bool canUseB = false ;
2021-05-04 08:12:23 -03:00
const ftype SB0 = 2 * q0 ;
const ftype SB1 = 2 * q1 ;
const ftype SB2 = SB0 * q3 + SB1 * q2 ;
const ftype SB4 = sq ( q0 ) + sq ( q1 ) - sq ( q2 ) - sq ( q3 ) ;
ftype SB3 , SB5_inv ;
2020-09-16 09:04:40 -03:00
if ( is_positive ( sq ( SB2 ) ) ) {
SB3 = 1.0F / sq ( SB2 ) ;
SB5_inv = SB3 * sq ( SB4 ) + 1 ;
2021-05-04 08:12:23 -03:00
canUseB = is_positive ( fabsF ( SB5_inv ) ) ;
2016-07-14 02:08:43 -03:00
}
2020-09-16 09:04:40 -03:00
2021-05-04 08:12:23 -03:00
if ( canUseA & & ( ! canUseB | | fabsF ( SA5_inv ) > = fabsF ( SB5_inv ) ) ) {
const ftype SA5 = 1.0F / SA5_inv ;
const ftype SA6 = 1.0F / SA3 ;
const ftype SA7 = SA2 * SA4 ;
const ftype SA8 = 2 * SA7 ;
const ftype SA9 = 2 * SA6 ;
2020-09-16 09:04:40 -03:00
H_YAW [ 0 ] = SA5 * ( SA0 * SA6 - SA8 * q0 ) ;
H_YAW [ 1 ] = SA5 * ( SA1 * SA6 - SA8 * q1 ) ;
H_YAW [ 2 ] = SA5 * ( SA1 * SA7 + SA9 * q1 ) ;
H_YAW [ 3 ] = SA5 * ( SA0 * SA7 + SA9 * q0 ) ;
2021-05-04 08:12:23 -03:00
} else if ( canUseB & & ( ! canUseA | | fabsF ( SB5_inv ) > fabsF ( SA5_inv ) ) ) {
const ftype SB5 = 1.0F / SB5_inv ;
const ftype SB6 = 1.0F / SB2 ;
const ftype SB7 = SB3 * SB4 ;
const ftype SB8 = 2 * SB7 ;
const ftype SB9 = 2 * SB6 ;
2020-09-16 09:04:40 -03:00
H_YAW [ 0 ] = - SB5 * ( SB0 * SB6 - SB8 * q3 ) ;
H_YAW [ 1 ] = - SB5 * ( SB1 * SB6 - SB8 * q2 ) ;
H_YAW [ 2 ] = - SB5 * ( - SB1 * SB7 - SB9 * q2 ) ;
H_YAW [ 3 ] = - SB5 * ( - SB0 * SB7 - SB9 * q3 ) ;
2016-07-14 02:08:43 -03:00
} else {
2020-12-14 17:43:46 -04:00
return false ;
2016-07-14 02:08:43 -03:00
}
// Get the 321 euler angles
2021-05-04 08:12:23 -03:00
Vector3F euler321 ;
2016-07-14 02:08:43 -03:00
stateStruct . quat . to_euler ( euler321 . x , euler321 . y , euler321 . z ) ;
2017-07-19 21:40:11 -03:00
yawAngPredicted = euler321 . z ;
2016-07-14 02:08:43 -03:00
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
Tbn_zeroYaw . from_euler ( euler321 . x , euler321 . y , 0.0f ) ;
2020-12-14 17:43:46 -04:00
} else if ( order = = rotationOrder : : TAIT_BRYAN_312 ) {
2020-09-16 09:04:40 -03:00
// calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false ;
2021-05-04 08:12:23 -03:00
const ftype SA0 = 2 * q3 ;
const ftype SA1 = 2 * q2 ;
const ftype SA2 = SA0 * q0 - SA1 * q1 ;
const ftype SA3 = sq ( q0 ) - sq ( q1 ) + sq ( q2 ) - sq ( q3 ) ;
ftype SA4 , SA5_inv ;
2020-09-16 09:04:40 -03:00
if ( is_positive ( sq ( SA3 ) ) ) {
SA4 = 1.0F / sq ( SA3 ) ;
SA5_inv = sq ( SA2 ) * SA4 + 1 ;
2021-05-04 08:12:23 -03:00
canUseA = is_positive ( fabsF ( SA5_inv ) ) ;
2020-09-16 09:04:40 -03:00
}
bool canUseB = false ;
2021-05-04 08:12:23 -03:00
const ftype SB0 = 2 * q0 ;
const ftype SB1 = 2 * q1 ;
const ftype SB2 = - SB0 * q3 + SB1 * q2 ;
const ftype SB4 = - sq ( q0 ) + sq ( q1 ) - sq ( q2 ) + sq ( q3 ) ;
ftype SB3 , SB5_inv ;
2020-09-16 09:04:40 -03:00
if ( is_positive ( sq ( SB2 ) ) ) {
SB3 = 1.0F / sq ( SB2 ) ;
SB5_inv = SB3 * sq ( SB4 ) + 1 ;
2021-05-04 08:12:23 -03:00
canUseB = is_positive ( fabsF ( SB5_inv ) ) ;
2016-07-14 02:08:43 -03:00
}
2020-09-16 09:04:40 -03:00
2021-05-04 08:12:23 -03:00
if ( canUseA & & ( ! canUseB | | fabsF ( SA5_inv ) > = fabsF ( SB5_inv ) ) ) {
const ftype SA5 = 1.0F / SA5_inv ;
const ftype SA6 = 1.0F / SA3 ;
const ftype SA7 = SA2 * SA4 ;
const ftype SA8 = 2 * SA7 ;
const ftype SA9 = 2 * SA6 ;
2020-09-16 09:04:40 -03:00
H_YAW [ 0 ] = SA5 * ( SA0 * SA6 - SA8 * q0 ) ;
H_YAW [ 1 ] = SA5 * ( - SA1 * SA6 + SA8 * q1 ) ;
H_YAW [ 2 ] = SA5 * ( - SA1 * SA7 - SA9 * q1 ) ;
H_YAW [ 3 ] = SA5 * ( SA0 * SA7 + SA9 * q0 ) ;
2021-05-04 08:12:23 -03:00
} else if ( canUseB & & ( ! canUseA | | fabsF ( SB5_inv ) > fabsF ( SA5_inv ) ) ) {
const ftype SB5 = 1.0F / SB5_inv ;
const ftype SB6 = 1.0F / SB2 ;
const ftype SB7 = SB3 * SB4 ;
const ftype SB8 = 2 * SB7 ;
const ftype SB9 = 2 * SB6 ;
2020-09-16 09:04:40 -03:00
H_YAW [ 0 ] = - SB5 * ( - SB0 * SB6 + SB8 * q3 ) ;
H_YAW [ 1 ] = - SB5 * ( SB1 * SB6 - SB8 * q2 ) ;
H_YAW [ 2 ] = - SB5 * ( - SB1 * SB7 - SB9 * q2 ) ;
H_YAW [ 3 ] = - SB5 * ( SB0 * SB7 + SB9 * q3 ) ;
2016-07-14 02:08:43 -03:00
} else {
2020-12-14 17:43:46 -04:00
return false ;
2016-07-14 02:08:43 -03:00
}
2020-09-16 09:04:40 -03:00
// Get the 312 Tait Bryan rotation angles
2021-05-04 08:12:23 -03:00
Vector3F euler312 = stateStruct . quat . to_vector312 ( ) ;
2017-07-19 21:40:11 -03:00
yawAngPredicted = euler312 . z ;
2016-07-14 02:08:43 -03:00
// set the yaw to zero and calculate the zero yaw rotation from body to earth frame
Tbn_zeroYaw . from_euler312 ( euler312 . x , euler312 . y , 0.0f ) ;
2020-12-14 17:43:46 -04:00
} else {
// order not supported
return false ;
2016-07-14 02:08:43 -03:00
}
2017-07-19 21:40:11 -03:00
// Calculate the innovation
2020-12-14 17:43:46 -04:00
switch ( method ) {
2020-12-15 03:28:00 -04:00
case yawFusionMethod : : MAGNETOMETER :
2020-12-14 17:43:46 -04:00
{
// Use the difference between the horizontal projection and declination to give the measured yaw
// rotate measured mag components into earth frame
2021-05-04 08:12:23 -03:00
Vector3F magMeasNED = Tbn_zeroYaw * magDataDelayed . mag ;
ftype yawAngMeasured = wrap_PI ( - atan2F ( magMeasNED . y , magMeasNED . x ) + MagDeclination ( ) ) ;
2020-12-14 17:43:46 -04:00
innovYaw = wrap_PI ( yawAngPredicted - yawAngMeasured ) ;
break ;
2016-07-14 02:08:43 -03:00
}
2020-12-22 04:08:30 -04:00
case yawFusionMethod : : GPS :
2020-12-14 17:43:46 -04:00
innovYaw = wrap_PI ( yawAngPredicted - yawAngDataDelayed . yawAng ) ;
break ;
case yawFusionMethod : : STATIC :
innovYaw = wrap_PI ( yawAngPredicted - yawAngDataStatic . yawAng ) ;
break ;
case yawFusionMethod : : GSF :
innovYaw = wrap_PI ( yawAngPredicted - gsfYaw ) ;
break ;
case yawFusionMethod : : PREDICTED :
default :
innovYaw = 0.0f ;
break ;
2021-01-19 00:27:03 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2020-11-30 08:35:24 -04:00
case yawFusionMethod : : EXTNAV :
innovYaw = wrap_PI ( yawAngPredicted - extNavYawAngDataDelayed . yawAng ) ;
break ;
2021-01-19 00:27:03 -04:00
# endif
2020-12-14 17:43:46 -04:00
}
2016-07-14 02:08:43 -03:00
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
2021-05-04 08:12:23 -03:00
ftype PH [ 4 ] ;
ftype varInnov = R_YAW ;
2016-07-14 02:08:43 -03:00
for ( uint8_t rowIndex = 0 ; rowIndex < = 3 ; rowIndex + + ) {
PH [ rowIndex ] = 0.0f ;
for ( uint8_t colIndex = 0 ; colIndex < = 3 ; colIndex + + ) {
PH [ rowIndex ] + = P [ rowIndex ] [ colIndex ] * H_YAW [ colIndex ] ;
}
varInnov + = H_YAW [ rowIndex ] * PH [ rowIndex ] ;
}
2021-05-04 08:12:23 -03:00
ftype varInnovInv ;
2016-07-14 02:08:43 -03:00
if ( varInnov > = R_YAW ) {
varInnovInv = 1.0f / varInnov ;
// output numerical health status
faultStatus . bad_yaw = false ;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit ( ) ;
// output numerical health status
faultStatus . bad_yaw = true ;
2020-12-14 17:43:46 -04:00
return false ;
2016-07-14 02:08:43 -03:00
}
// calculate Kalman gain
for ( uint8_t rowIndex = 0 ; rowIndex < = stateIndexLim ; rowIndex + + ) {
Kfusion [ rowIndex ] = 0.0f ;
for ( uint8_t colIndex = 0 ; colIndex < = 3 ; colIndex + + ) {
Kfusion [ rowIndex ] + = P [ rowIndex ] [ colIndex ] * H_YAW [ colIndex ] ;
}
Kfusion [ rowIndex ] * = varInnovInv ;
}
// calculate the innovation test ratio
2021-05-04 08:12:23 -03:00
yawTestRatio = sq ( innovYaw ) / ( sq ( MAX ( 0.01f * ( ftype ) frontend - > _yawInnovGate , 1.0f ) ) * varInnov ) ;
2016-07-14 02:08:43 -03:00
// Declare the magnetometer unhealthy if the innovation test fails
if ( yawTestRatio > 1.0f ) {
magHealth = false ;
// On the ground a large innovation could be due to large initial gyro bias or magnetic interference from nearby objects
// If we are flying, then it is more likely due to a magnetometer fault and we should not fuse the data
if ( inFlight ) {
2020-12-14 17:43:46 -04:00
return false ;
2016-07-14 02:08:43 -03:00
}
} else {
magHealth = true ;
}
// correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
// calculate K*H*P
for ( uint8_t row = 0 ; row < = stateIndexLim ; row + + ) {
for ( uint8_t column = 0 ; column < = 3 ; column + + ) {
KH [ row ] [ column ] = Kfusion [ row ] * H_YAW [ column ] ;
}
}
for ( uint8_t row = 0 ; row < = stateIndexLim ; row + + ) {
for ( uint8_t column = 0 ; column < = stateIndexLim ; column + + ) {
2021-05-04 08:12:23 -03:00
ftype tmp = KH [ row ] [ 0 ] * P [ 0 ] [ column ] ;
2016-07-14 02:08:43 -03:00
tmp + = KH [ row ] [ 1 ] * P [ 1 ] [ column ] ;
tmp + = KH [ row ] [ 2 ] * P [ 2 ] [ column ] ;
tmp + = KH [ row ] [ 3 ] * P [ 3 ] [ column ] ;
KHP [ row ] [ column ] = tmp ;
}
}
// 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 i = 0 ; i < = stateIndexLim ; i + + ) {
2021-05-04 08:12:23 -03:00
statesArray [ i ] - = Kfusion [ i ] * constrain_ftype ( innovYaw , - 0.5f , 0.5f ) ;
2016-07-14 02:08:43 -03:00
}
stateStruct . quat . normalize ( ) ;
// record fusion numerical health status
faultStatus . bad_yaw = false ;
} else {
// record fusion numerical health status
faultStatus . bad_yaw = true ;
}
2020-12-14 17:43:46 -04:00
return true ;
2016-07-14 02:08:43 -03:00
}
/*
* Fuse declination angle 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
* This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS
* or some other absolute position or velocity reference
*/
2021-05-04 08:12:23 -03:00
void NavEKF3_core : : FuseDeclination ( ftype declErr )
2016-07-14 02:08:43 -03:00
{
// declination error variance (rad^2)
2021-05-04 08:12:23 -03:00
const ftype R_DECL = sq ( declErr ) ;
2016-07-14 02:08:43 -03:00
// copy required states to local variables
2021-05-04 08:12:23 -03:00
ftype magN = stateStruct . earth_magfield . x ;
ftype magE = stateStruct . earth_magfield . y ;
2016-07-14 02:08:43 -03:00
// prevent bad earth field states from causing numerical errors or exceptions
if ( magN < 1e-3 f ) {
return ;
}
// Calculate observation Jacobian and Kalman gains
// Calculate intermediate variables
2021-05-04 08:12:23 -03:00
ftype t2 = magE * magE ;
ftype t3 = magN * magN ;
ftype t4 = t2 + t3 ;
2016-07-14 02:08:43 -03:00
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
if ( t4 < 1e-4 f ) {
return ;
}
2021-05-04 08:12:23 -03:00
ftype t5 = P [ 16 ] [ 16 ] * t2 ;
ftype t6 = P [ 17 ] [ 17 ] * t3 ;
ftype t7 = t2 * t2 ;
ftype t8 = R_DECL * t7 ;
ftype t9 = t3 * t3 ;
ftype t10 = R_DECL * t9 ;
ftype t11 = R_DECL * t2 * t3 * 2.0f ;
ftype t14 = P [ 16 ] [ 17 ] * magE * magN ;
ftype t15 = P [ 17 ] [ 16 ] * magE * magN ;
ftype t12 = t5 + t6 + t8 + t10 + t11 - t14 - t15 ;
ftype t13 ;
if ( fabsF ( t12 ) > 1e-6 f ) {
2016-07-14 02:08:43 -03:00
t13 = 1.0f / t12 ;
} else {
return ;
}
2021-05-04 08:12:23 -03:00
ftype t18 = magE * magE ;
ftype t19 = magN * magN ;
ftype t20 = t18 + t19 ;
ftype t21 ;
if ( fabsF ( t20 ) > 1e-6 f ) {
2016-07-14 02:08:43 -03:00
t21 = 1.0f / t20 ;
} else {
return ;
}
// Calculate the observation Jacobian
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
2021-05-04 08:12:23 -03:00
ftype H_DECL [ 24 ] = { } ;
2016-07-14 02:08:43 -03:00
H_DECL [ 16 ] = - magE * t21 ;
H_DECL [ 17 ] = magN * t21 ;
Kfusion [ 0 ] = - t4 * t13 * ( P [ 0 ] [ 16 ] * magE - P [ 0 ] [ 17 ] * magN ) ;
Kfusion [ 1 ] = - t4 * t13 * ( P [ 1 ] [ 16 ] * magE - P [ 1 ] [ 17 ] * magN ) ;
Kfusion [ 2 ] = - t4 * t13 * ( P [ 2 ] [ 16 ] * magE - P [ 2 ] [ 17 ] * magN ) ;
Kfusion [ 3 ] = - t4 * t13 * ( P [ 3 ] [ 16 ] * magE - P [ 3 ] [ 17 ] * magN ) ;
Kfusion [ 4 ] = - t4 * t13 * ( P [ 4 ] [ 16 ] * magE - P [ 4 ] [ 17 ] * magN ) ;
Kfusion [ 5 ] = - t4 * t13 * ( P [ 5 ] [ 16 ] * magE - P [ 5 ] [ 17 ] * magN ) ;
Kfusion [ 6 ] = - t4 * t13 * ( P [ 6 ] [ 16 ] * magE - P [ 6 ] [ 17 ] * magN ) ;
Kfusion [ 7 ] = - t4 * t13 * ( P [ 7 ] [ 16 ] * magE - P [ 7 ] [ 17 ] * magN ) ;
Kfusion [ 8 ] = - t4 * t13 * ( P [ 8 ] [ 16 ] * magE - P [ 8 ] [ 17 ] * magN ) ;
Kfusion [ 9 ] = - t4 * t13 * ( P [ 9 ] [ 16 ] * magE - P [ 9 ] [ 17 ] * magN ) ;
2017-05-09 19:31:55 -03:00
if ( ! inhibitDelAngBiasStates ) {
Kfusion [ 10 ] = - t4 * t13 * ( P [ 10 ] [ 16 ] * magE - P [ 10 ] [ 17 ] * magN ) ;
Kfusion [ 11 ] = - t4 * t13 * ( P [ 11 ] [ 16 ] * magE - P [ 11 ] [ 17 ] * magN ) ;
Kfusion [ 12 ] = - t4 * t13 * ( P [ 12 ] [ 16 ] * magE - P [ 12 ] [ 17 ] * magN ) ;
} 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
}
if ( ! inhibitDelVelBiasStates ) {
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 ] = - t4 * t13 * ( P [ stateIndex ] [ 16 ] * magE - P [ stateIndex ] [ 17 ] * magN ) ;
} else {
Kfusion [ stateIndex ] = 0.0f ;
}
}
2017-05-09 19:31:55 -03:00
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 13 to 15
zero_range ( & Kfusion [ 0 ] , 13 , 15 ) ;
2017-05-09 19:31:55 -03:00
}
if ( ! inhibitMagStates ) {
Kfusion [ 16 ] = - t4 * t13 * ( P [ 16 ] [ 16 ] * magE - P [ 16 ] [ 17 ] * magN ) ;
Kfusion [ 17 ] = - t4 * t13 * ( P [ 17 ] [ 16 ] * magE - P [ 17 ] [ 17 ] * magN ) ;
Kfusion [ 18 ] = - t4 * t13 * ( P [ 18 ] [ 16 ] * magE - P [ 18 ] [ 17 ] * magN ) ;
Kfusion [ 19 ] = - t4 * t13 * ( P [ 19 ] [ 16 ] * magE - P [ 19 ] [ 17 ] * magN ) ;
Kfusion [ 20 ] = - t4 * t13 * ( P [ 20 ] [ 16 ] * magE - P [ 20 ] [ 17 ] * magN ) ;
Kfusion [ 21 ] = - t4 * t13 * ( P [ 21 ] [ 16 ] * magE - P [ 21 ] [ 17 ] * magN ) ;
} 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 ] = - t4 * t13 * ( P [ 22 ] [ 16 ] * magE - P [ 22 ] [ 17 ] * magN ) ;
Kfusion [ 23 ] = - t4 * t13 * ( P [ 23 ] [ 16 ] * magE - P [ 23 ] [ 17 ] * magN ) ;
} else {
2021-05-04 08:12:23 -03:00
// zero indexes 22 to 23
zero_range ( & Kfusion [ 0 ] , 22 , 23 ) ;
2017-05-09 19:31:55 -03:00
}
2016-07-14 02:08:43 -03:00
// get the magnetic declination
2021-05-04 08:12:23 -03:00
ftype magDecAng = MagDeclination ( ) ;
2016-07-14 02:08:43 -03:00
// Calculate the innovation
2021-05-04 08:12:23 -03:00
ftype innovation = atan2F ( magE , magN ) - magDecAng ;
2016-07-14 02:08:43 -03:00
// limit the innovation to protect against data errors
if ( innovation > 0.5f ) {
innovation = 0.5f ;
} else if ( innovation < - 0.5f ) {
innovation = - 0.5f ;
}
// 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 < = 15 ; j + + ) {
KH [ i ] [ j ] = 0.0f ;
}
KH [ i ] [ 16 ] = Kfusion [ i ] * H_DECL [ 16 ] ;
KH [ i ] [ 17 ] = Kfusion [ i ] * H_DECL [ 17 ] ;
for ( unsigned j = 18 ; j < = 23 ; j + + ) {
KH [ i ] [ j ] = 0.0f ;
}
}
for ( unsigned j = 0 ; j < = stateIndexLim ; j + + ) {
for ( unsigned i = 0 ; i < = stateIndexLim ; i + + ) {
KHP [ i ] [ j ] = KH [ i ] [ 16 ] * P [ 16 ] [ j ] + KH [ i ] [ 17 ] * P [ 17 ] [ 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 ( ) ;
// correct the state vector
for ( uint8_t j = 0 ; j < = stateIndexLim ; j + + ) {
statesArray [ j ] = statesArray [ j ] - Kfusion [ j ] * innovation ;
}
stateStruct . quat . normalize ( ) ;
// record fusion health status
faultStatus . bad_decl = false ;
} else {
// record fusion health status
faultStatus . bad_decl = true ;
}
}
/********************************************************
* MISC FUNCTIONS *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// align the NE earth magnetic field states with the published declination
void NavEKF3_core : : alignMagStateDeclination ( )
{
// don't do this if we already have a learned magnetic field
if ( magFieldLearned ) {
return ;
}
// get the magnetic declination
2021-05-04 08:12:23 -03:00
ftype magDecAng = MagDeclination ( ) ;
2016-07-14 02:08:43 -03:00
// rotate the NE values so that the declination matches the published value
2021-05-04 08:12:23 -03:00
Vector3F initMagNED = stateStruct . earth_magfield ;
2021-09-11 07:34:45 -03:00
ftype magLengthNE = initMagNED . xy ( ) . length ( ) ;
2021-05-04 08:12:23 -03:00
stateStruct . earth_magfield . x = magLengthNE * cosF ( magDecAng ) ;
stateStruct . earth_magfield . y = magLengthNE * sinF ( magDecAng ) ;
2016-07-14 02:08:43 -03:00
if ( ! inhibitMagStates ) {
// zero the corresponding state covariances if magnetic field state learning is active
2021-05-04 08:12:23 -03:00
ftype var_16 = P [ 16 ] [ 16 ] ;
ftype var_17 = P [ 17 ] [ 17 ] ;
2016-07-14 02:08:43 -03:00
zeroRows ( P , 16 , 17 ) ;
zeroCols ( P , 16 , 17 ) ;
P [ 16 ] [ 16 ] = var_16 ;
P [ 17 ] [ 17 ] = var_17 ;
// fuse the declination angle to establish covariances and prevent large swings in declination
// during initial fusion
FuseDeclination ( 0.1f ) ;
}
}
2017-05-16 07:27:57 -03:00
// record a magnetic field state reset event
2016-07-14 02:08:43 -03:00
void NavEKF3_core : : recordMagReset ( )
{
2020-07-25 05:47:43 -03:00
magStateResetRequest = false ;
2016-07-14 02:08:43 -03:00
magStateInitComplete = true ;
if ( inFlight ) {
finalInflightMagInit = true ;
}
// take a snap-shot of the vertical position, quaternion and yaw innovation to use as a reference
// for post alignment checks
posDownAtLastMagReset = stateStruct . position . z ;
quatAtLastMagReset = stateStruct . quat ;
yawInnovAtLastMagReset = innovYaw ;
}
2020-01-22 05:56:57 -04:00
/*
learn magnetometer biases from GPS yaw . Return true if the
resulting mag vector is close enough to the one predicted by GPS
yaw to use it for fallback
*/
bool NavEKF3_core : : learnMagBiasFromGPS ( void )
{
if ( ! have_table_earth_field ) {
// we need the earth field from WMM
return false ;
}
if ( ! inFlight ) {
// don't start learning till we've started flying
return false ;
}
mag_elements mag_data ;
if ( ! storedMag . recall ( mag_data , imuDataDelayed . time_ms ) ) {
// no mag data to correct
return false ;
}
// combine yaw with current quaternion to get yaw corrected quaternion
2021-05-04 08:12:23 -03:00
QuaternionF quat = stateStruct . quat ;
2020-12-14 17:43:46 -04:00
if ( yawAngDataDelayed . order = = rotationOrder : : TAIT_BRYAN_321 ) {
2021-05-04 08:12:23 -03:00
Vector3F euler321 ;
2020-01-22 05:56:57 -04:00
quat . to_euler ( euler321 . x , euler321 . y , euler321 . z ) ;
quat . from_euler ( euler321 . x , euler321 . y , yawAngDataDelayed . yawAng ) ;
2020-12-14 17:43:46 -04:00
} else if ( yawAngDataDelayed . order = = rotationOrder : : TAIT_BRYAN_312 ) {
2021-05-04 08:12:23 -03:00
Vector3F euler312 = quat . to_vector312 ( ) ;
2020-01-22 05:56:57 -04:00
quat . from_vector312 ( euler312 . x , euler312 . y , yawAngDataDelayed . yawAng ) ;
2020-12-14 17:43:46 -04:00
} else {
// rotation order not supported
return false ;
2020-01-22 05:56:57 -04:00
}
// build the expected body field from orientation and table earth field
2021-05-04 08:12:23 -03:00
Matrix3F dcm ;
2020-01-22 05:56:57 -04:00
quat . rotation_matrix ( dcm ) ;
2021-05-04 08:12:23 -03:00
Vector3F expected_body_field = dcm . transposed ( ) * table_earth_field_ga ;
2020-01-22 05:56:57 -04:00
// calculate error in field
2021-05-04 08:12:23 -03:00
Vector3F err = ( expected_body_field - mag_data . mag ) + stateStruct . body_magfield ;
2020-01-22 05:56:57 -04:00
// learn body frame mag biases
stateStruct . body_magfield - = err * EK3_GPS_MAG_LEARN_RATE ;
// check if error is below threshold. If it is then we can
// fallback to magnetometer on failure of external yaw
2021-05-04 08:12:23 -03:00
ftype err_length = err . length ( ) ;
2020-01-22 05:56:57 -04:00
// we allow for yaw backback to compass if we have had 50 samples
// in a row below the threshold. This corresponds to 10 seconds
// for a 5Hz GPS
const uint8_t fallback_count_threshold = 50 ;
if ( err_length > EK3_GPS_MAG_LEARN_LIMIT ) {
gps_yaw_fallback_good_counter = 0 ;
} else if ( gps_yaw_fallback_good_counter < fallback_count_threshold ) {
gps_yaw_fallback_good_counter + + ;
}
bool ok = gps_yaw_fallback_good_counter > = fallback_count_threshold ;
if ( ok ) {
// mark mag healthy to prevent a magTimeout when we start using it
lastHealthyMagTime_ms = imuSampleTime_ms ;
}
return ok ;
}
2020-04-10 21:43:46 -03:00
2020-03-10 03:48:08 -03:00
// Reset states using yaw from EKF-GSF and velocity and position from GPS
2021-08-05 00:24:33 -03:00
bool NavEKF3_core : : EKFGSF_resetMainFilterYaw ( bool emergency_reset )
2020-03-10 03:48:08 -03:00
{
2022-02-14 09:32:57 -04:00
// Don't do a reset unless permitted by the EK3_GSF_USE_MASK and EK3_GSF_RUN_MASK parameter masks
2020-03-10 03:48:08 -03:00
if ( ( yawEstimator = = nullptr )
2021-08-05 00:24:33 -03:00
| | ! ( frontend - > _gsfUseMask & ( 1U < < core_index ) ) ) {
2020-03-10 03:48:08 -03:00
return false ;
} ;
2021-08-05 00:24:33 -03:00
// limit the number of emergency resets
if ( emergency_reset & & ( EKFGSF_yaw_reset_count > = frontend - > _gsfResetMaxCount ) ) {
return false ;
}
2021-05-04 08:12:23 -03:00
ftype yawEKFGSF , yawVarianceEKFGSF ;
2020-12-16 20:47:13 -04:00
if ( EKFGSF_getYaw ( yawEKFGSF , yawVarianceEKFGSF ) ) {
2020-03-10 03:48:08 -03:00
// keep roll and pitch and reset yaw
2020-09-16 10:28:02 -03:00
rotationOrder order ;
bestRotationOrder ( order ) ;
resetQuatStateYawOnly ( yawEKFGSF , yawVarianceEKFGSF , order ) ;
2020-03-10 03:48:08 -03:00
// record the emergency reset event
EKFGSF_yaw_reset_request_ms = 0 ;
EKFGSF_yaw_reset_ms = imuSampleTime_ms ;
EKFGSF_yaw_reset_count + + ;
2024-04-16 21:01:21 -03:00
if ( ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GSF ) | |
2020-12-22 03:15:43 -04:00
! use_compass ( ) | | ( dal . compass ( ) . get_num_enabled ( ) = = 0 ) ) {
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u yaw aligned using GPS " , ( unsigned ) imu_index ) ;
2020-03-10 03:48:08 -03:00
} else {
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_WARNING , " EKF3 IMU%u emergency yaw reset " , ( unsigned ) imu_index ) ;
2020-03-10 03:48:08 -03:00
}
// Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
2021-08-05 00:24:33 -03:00
if ( emergency_reset ) {
allMagSensorsFailed = true ;
}
2020-03-10 03:48:08 -03:00
2020-06-07 23:13:49 -03:00
// record the yaw reset event
recordYawReset ( ) ;
2020-03-10 03:48:08 -03:00
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
2020-07-17 05:47:43 -03:00
ResetVelocity ( resetDataSource : : DEFAULT ) ;
ResetPosition ( resetDataSource : : DEFAULT ) ;
2020-03-10 03:48:08 -03:00
2020-04-18 07:58:00 -03:00
// reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
velTestRatio = 0.0f ;
posTestRatio = 0.0f ;
2020-03-10 03:48:08 -03:00
return true ;
}
return false ;
}
2020-12-15 03:20:31 -04:00
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
2021-05-04 08:12:23 -03:00
bool NavEKF3_core : : EKFGSF_getYaw ( ftype & yaw , ftype & yawVariance ) const
2020-12-15 03:20:31 -04:00
{
// return immediately if no yaw estimator
if ( yawEstimator = = nullptr ) {
return false ;
}
2021-05-04 08:12:23 -03:00
ftype velInnovLength ;
2020-12-15 03:20:31 -04:00
if ( yawEstimator - > getYawData ( yaw , yawVariance ) & &
is_positive ( yawVariance ) & &
yawVariance < sq ( radians ( GSF_YAW_ACCURACY_THRESHOLD_DEG ) ) & &
( assume_zero_sideslip ( ) | | ( yawEstimator - > getVelInnovLength ( velInnovLength ) & & velInnovLength < frontend - > maxYawEstVelInnov ) ) ) {
return true ;
}
return false ;
}
2021-05-04 08:12:23 -03:00
void NavEKF3_core : : resetQuatStateYawOnly ( ftype yaw , ftype yawVariance , rotationOrder order )
2020-03-10 03:48:08 -03:00
{
2021-05-04 08:12:23 -03:00
QuaternionF quatBeforeReset = stateStruct . quat ;
2020-03-10 03:48:08 -03:00
2020-09-16 10:28:02 -03:00
// check if we should use a 321 or 312 Rotation order and update the quaternion
2020-03-10 03:48:08 -03:00
// states using the preferred yaw definition
stateStruct . quat . inverse ( ) . rotation_matrix ( prevTnb ) ;
2021-05-04 08:12:23 -03:00
Vector3F eulerAngles ;
2020-09-16 10:28:02 -03:00
if ( order = = rotationOrder : : TAIT_BRYAN_321 ) {
2020-03-10 03:48:08 -03:00
// rolled more than pitched so use 321 rotation order
stateStruct . quat . to_euler ( eulerAngles . x , eulerAngles . y , eulerAngles . z ) ;
stateStruct . quat . from_euler ( eulerAngles . x , eulerAngles . y , yaw ) ;
2020-09-16 10:28:02 -03:00
} else if ( order = = rotationOrder : : TAIT_BRYAN_312 ) {
2020-03-10 03:48:08 -03:00
// pitched more than rolled so use 312 rotation order
eulerAngles = stateStruct . quat . to_vector312 ( ) ;
stateStruct . quat . from_vector312 ( eulerAngles . x , eulerAngles . y , yaw ) ;
2020-09-16 10:28:02 -03:00
} else {
// rotation order not supported
return ;
2020-03-10 03:48:08 -03:00
}
// Update the rotation matrix
stateStruct . quat . inverse ( ) . rotation_matrix ( prevTnb ) ;
2021-05-04 08:12:23 -03:00
ftype deltaYaw = wrap_PI ( yaw - eulerAngles . z ) ;
2020-03-10 03:48:08 -03:00
// calculate the change in the quaternion state and apply it to the output history buffer
2021-05-04 08:12:23 -03:00
QuaternionF quat_delta = stateStruct . quat / quatBeforeReset ;
2020-03-10 03:48:08 -03:00
StoreQuatRotate ( quat_delta ) ;
2020-09-16 19:10:10 -03:00
// assume tilt uncertainty split equally between roll and pitch
2021-05-04 08:12:23 -03:00
Vector3F angleErrVarVec = Vector3F ( 0.5 * tiltErrorVariance , 0.5 * tiltErrorVariance , yawVariance ) ;
2020-09-15 22:43:32 -03:00
CovariancePrediction ( & angleErrVarVec ) ;
2020-03-10 03:48:08 -03:00
// record the yaw reset event
yawResetAngle + = deltaYaw ;
lastYawReset_ms = imuSampleTime_ms ;
// record the yaw reset event
recordYawReset ( ) ;
// clear all pending yaw reset requests
gpsYawResetRequest = false ;
magYawResetRequest = false ;
}