From a19c29e708cada427058bdd4021a6d0a64616b72 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 22 Jan 2020 14:41:04 +1100 Subject: [PATCH] EKF: Rework yaw fusion to remove singularity at +-90 deg yaw --- EKF/mag_fusion.cpp | 139 +++++++++++++----- .../GenerateNavFilterEquations.m | 12 +- .../scripts/Inertial Nav EKF/calcH_YAW312.c | 29 +++- .../scripts/Inertial Nav EKF/calcH_YAW321.c | 33 ++++- 4 files changed, 168 insertions(+), 45 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index b89dcc7571..dea3f67f74 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -460,26 +460,64 @@ void Ekf::fuseHeading() 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; - } - 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; + float t16 = q3*t3; + float t17 = q3*t5; + float t18 = q0*q1*q2*2.0f; + float t19 = t16+t17+t18-q3*t4+q3*t6; + + float t24 = q2*t4; + float t25 = q2*t6; + float t26 = q0*q1*q3*2.0f; + float t27 = t24+t25+t26-q2*t3+q2*t5; + float t28 = q1*t3; + float t29 = q1*t5; + float t30 = q0*q2*q3*2.0f; + float t31 = t28+t29+t30+q1*t4-q1*t6; + float t32 = q0*t4; + float t33 = q0*t6; + float t34 = q1*q2*q3*2.0f; + float t35 = t32+t33+t34+q0*t3-q0*t5; + + // two computational paths are provided to work around singularities in calculation of the Jacobians + float t8 = t7*t7; + float t15 = t2*t2; + if (t8 > t15 && t8 > 1E-6f) { + // this path has a singularities at yaw = +-90 degrees + t8 = 1.0f/t8; + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; + float t14 = 1.0f/t13; + + H_YAW[0] = t8*t14*t19*(-2.0f); + H_YAW[1] = t8*t14*t27*(-2.0f); + H_YAW[2] = t8*t14*t31*2.0f; + H_YAW[3] = t8*t14*t35*2.0f; + + } else if (t15 > 1E-6f) { + // this path has singularities at yaw = 0 and +-180 deg + t15 = 1.0f/t15; + float t20 = t7*t7; + float t21 = t15*t20*0.25f; + float t22 = t21+1.0f; + + if (fabsf(t22) > 1E-6f) { + float t23 = 1.0f/t22; + + H_YAW[0] = t15*t19*t23*(-0.5f); + H_YAW[1] = t15*t23*t27*(-0.5f); + H_YAW[2] = t15*t23*t31*0.5f; + H_YAW[3] = t15*t23*t35*0.5f; + + } else { + return; + + } + } else { + return; + + } // rotate the magnetometer measurement into earth frame Eulerf euler321(_state.quat_nominal); @@ -531,27 +569,62 @@ void Ekf::fuseHeading() float t5 = q2*q2; float t6 = q3*q3; float t7 = t3-t4+t5-t6; + + float t16 = q3*t3; + float t17 = q3*t4; + float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f; + float t23 = q2*t3; + float t24 = q2*t4; + float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f; + float t26 = q1*t5; + float t27 = q1*t6; + float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f; + float t29 = q0*t5; + float t30 = q0*t6; + float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f; + + // two computational paths are provided to work around singularities in calculation of the Jacobians float t8 = t7*t7; - if (t8 > 1e-6f) { + float t15 = t2*t2; + if (t8 > t15 && t8 > 1E-6f) { + // this path has a singularities at yaw = +-90 degrees t8 = 1.0f/t8; + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; + float t14 = 1.0f/t13; + + H_YAW[0] = t8*t14*t18*(-2.0f); + H_YAW[1] = t8*t14*t25*(-2.0f); + H_YAW[2] = t8*t14*t28*2.0f; + H_YAW[3] = t8*t14*t31*2.0f; + + } else if (t15 > 1E-6f) { + // this path has singularities at yaw = 0 and +-180 deg + t15 = 1.0f/t15; + float t19 = t7*t7; + float t20 = t15*t19*0.25f; + float t21 = t20+1.0f; + + if (fabsf(t21) > 1E-6f) { + float t22 = 1.0f/t21; + + H_YAW[0] = t15*t18*t22*(-0.5f); + H_YAW[1] = t15*t22*t25*(-0.5f); + H_YAW[2] = t15*t22*t28*0.5f; + H_YAW[3] = t15*t22*t31*0.5f; + + } else { + return; + + } + } 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; } - 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; + /* Calculate the 312 sequence euler angles that rotate from earth to body frame * Derived from https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m diff --git a/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index 1d5b1e5bc9..9eec7cb79f 100644 --- a/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -391,8 +391,10 @@ fix_c_code('K_VEL.c'); load('StatePrediction.mat'); % Calculate the yaw (first rotation) angle from the 321 rotation sequence -angMeas = atan(Tbn(2,1)/Tbn(1,1)); -H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian +% Provide alternative angle that avoids singularity at +-pi/2 +angMeasA = atan(Tbn(2,1)/Tbn(1,1)); +angMeasB = pi/2 - atan(Tbn(1,1)/Tbn(2,1)); +H_YAW321 = jacobian([angMeasA;angMeasB],stateVector); % measurement Jacobian H_YAW321 = simplify(H_YAW321); ccode(H_YAW321,'file','calcH_YAW321.c'); fix_c_code('calcH_YAW321.c'); @@ -405,8 +407,10 @@ reset(symengine); load('StatePrediction.mat'); % Calculate the yaw (first rotation) angle from an Euler 312 sequence -angMeas = atan(-Tbn(1,2)/Tbn(2,2)); -H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea +% Provide alternative angle that avoids singularity at +-pi/2 +angMeasA = atan(-Tbn(1,2)/Tbn(2,2)); +angMeasB = pi/2 - atan(-Tbn(2,2)/Tbn(1,2)); +H_YAW312 = jacobian([angMeasA;angMeasB],stateVector); % measurement Jacobian H_YAW312 = simplify(H_YAW312); ccode(H_YAW312,'file','calcH_YAW312.c'); fix_c_code('calcH_YAW312.c'); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c index 399e544299..91567af894 100644 --- a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c +++ b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c @@ -11,7 +11,28 @@ t11 = t2*t2; t12 = t8*t11*4.0; t13 = t12+1.0; t14 = 1.0/t13; -A0[0][0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0)*-2.0; -A0[0][1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0)*-2.0; -A0[0][2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0)*2.0; -A0[0][3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0)*2.0; +t15 = 1.0/(t2*t2); +t16 = q3*t3; +t17 = q3*t4; +t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0; +t19 = t7*t7; +t20 = t15*t19*(1.0/4.0); +t21 = t20+1.0; +t22 = 1.0/t21; +t23 = q2*t3; +t24 = q2*t4; +t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0; +t26 = q1*t5; +t27 = q1*t6; +t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0; +t29 = q0*t5; +t30 = q0*t6; +t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0; +A0[0][0] = t8*t14*t18*-2.0; +A0[0][1] = t8*t14*t25*-2.0; +A0[0][2] = t8*t14*t28*2.0; +A0[0][3] = t8*t14*t31*2.0; +A0[1][0] = t15*t18*t22*(-1.0/2.0); +A0[1][1] = t15*t22*t25*(-1.0/2.0); +A0[1][2] = t15*t22*t28*(1.0/2.0); +A0[1][3] = t15*t22*t31*(1.0/2.0); diff --git a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c index 90ad9098c2..9fe4d7db47 100644 --- a/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c +++ b/EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c @@ -11,7 +11,32 @@ t11 = t2*t2; t12 = t8*t11*4.0; t13 = t12+1.0; t14 = 1.0/t13; -A0[0][0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0)*-2.0; -A0[0][1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0)*-2.0; -A0[0][2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0)*2.0; -A0[0][3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0)*2.0; +t15 = 1.0/(t2*t2); +t16 = q3*t3; +t17 = q3*t5; +t18 = q0*q1*q2*2.0; +t19 = t16+t17+t18-q3*t4+q3*t6; +t20 = t7*t7; +t21 = t15*t20*(1.0/4.0); +t22 = t21+1.0; +t23 = 1.0/t22; +t24 = q2*t4; +t25 = q2*t6; +t26 = q0*q1*q3*2.0; +t27 = t24+t25+t26-q2*t3+q2*t5; +t28 = q1*t3; +t29 = q1*t5; +t30 = q0*q2*q3*2.0; +t31 = t28+t29+t30+q1*t4-q1*t6; +t32 = q0*t4; +t33 = q0*t6; +t34 = q1*q2*q3*2.0; +t35 = t32+t33+t34+q0*t3-q0*t5; +A0[0][0] = t8*t14*t19*-2.0; +A0[0][1] = t8*t14*t27*-2.0; +A0[0][2] = t8*t14*t31*2.0; +A0[0][3] = t8*t14*t35*2.0; +A0[1][0] = t15*t19*t23*(-1.0/2.0); +A0[1][1] = t15*t23*t27*(-1.0/2.0); +A0[1][2] = t15*t23*t31*(1.0/2.0); +A0[1][3] = t15*t23*t35*(1.0/2.0);