Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Remove singularity in yaw fusion at +-90deg yaw 

Uses sympy derivation
This commit is contained in:
Paul Riseborough 2020-09-16 22:04:40 +10:00 committed by Andrew Tridgell
parent 4e41e9f5f1
commit 69632336e1
1 changed files with 101 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -866,11 +866,9 @@ void NavEKF3_core::FuseMagnetometer()
}
}
/*
* Fuse magnetic heading measurement using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
* Fuse yaw measurements using explicit algebraic equations auto-generated from
* /AP_NavEKF3/derivation/main.py with output recorded in /AP_NavEKF3/derivation/generated/yaw_generated.cpp
* This fusion method only modifies the orientation, does not require use of the magnetic field states and is computationally cheaper.
* It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground).
* It is not as robust to magnetometer failures.
@ -885,10 +883,10 @@ void NavEKF3_core::FuseMagnetometer()
*/
void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
{
float q0 = stateStruct.quat[0];
float q1 = stateStruct.quat[1];
float q2 = stateStruct.quat[2];
float q3 = stateStruct.quat[3];
const float &q0 = stateStruct.quat[0];
const float &q1 = stateStruct.quat[1];
const float &q2 = stateStruct.quat[2];
const float &q3 = stateStruct.quat[3];
// external yaw available check
bool canUseGsfYaw = false;
@ -933,35 +931,56 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
Matrix3f Tbn_zeroYaw;
if (useEuler321) {
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9+t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3+t4-t5-t6;
float t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
}
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
return;
// calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false;
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SA4, SA5_inv;
if (is_positive(sq(SA3))) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = is_positive(fabsf(SA5_inv));
}
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
bool canUseB = false;
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = SB0*q3 + SB1*q2;
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SB3, SB5_inv;
if (is_positive(sq(SB2))) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = is_positive(fabsf(SB5_inv));
}
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
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);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
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);
} else {
return;
}
// Get the 321 euler angles
Vector3f euler321;
@ -972,37 +991,58 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
} else {
// calculate observation jacobian when we are observing a rotation in a 312 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9-t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3-t4+t5-t6;
float t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
// calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false;
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
float SA4, SA5_inv;
if (is_positive(sq(SA3))) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = is_positive(fabsf(SA5_inv));
}
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
bool canUseB = false;
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = -SB0*q3 + SB1*q2;
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
float SB3, SB5_inv;
if (is_positive(sq(SB2))) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = is_positive(fabsf(SB5_inv));
}
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
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);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
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);
} else {
return;
}
H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f;
// Get the 321 euler angles
// Get the 312 Tait Bryan rotation angles
Vector3f euler312 = stateStruct.quat.to_vector312();
yawAngPredicted = euler312.z;