zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

Uses sympy derivation
zr-v5.1
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
4e41e9f5f1
commit
69632336e1
1 changed files with 99 additions and 59 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 158
      libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

158
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
Unescape View File

@ -866,11 +866,9 @@ void NavEKF3_core::FuseMagnetometer() @@ -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() @@ -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,36 +931,57 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) @@ -933,36 +931,57 @@ 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;
// 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));
}
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));
}
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;
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
Vector3f euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
@ -972,37 +991,58 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) @@ -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));
}
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));
}
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;
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;

Write Preview
Loading…
Cancel
Save