From 6ca8a367821903e329cd3cb18f432d8d6aaa9def Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 10:49:02 +1000 Subject: [PATCH 1/6] EKF: Add matlab derivation for calculation of rotation vector variance --- .../QuatErrTransferEquations.m | 40 +++++++++++++++++++ matlab/scripts/Inertial Nav EKF/rotVarVec.c | 19 +++++++++ 2 files changed, 59 insertions(+) create mode 100644 matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m create mode 100644 matlab/scripts/Inertial Nav EKF/rotVarVec.c diff --git a/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m b/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m new file mode 100644 index 0000000000..25b37b6a88 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m @@ -0,0 +1,40 @@ +% calculate the variances for an equivalent rotation vector +% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions +% output is a vector of variances for the rotation vector that is equivalent to the quaternion +clear all; +reset(symengine); +syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED + +% define quaternion rotation +quat = [q0;q1;q2;q3]; + +% convert to a rotation vector +delta = 2*acos(q0); +rotX = delta*(q1/sin(delta/2)); +rotY = delta*(q2/sin(delta/2)); +rotZ = delta*(q3/sin(delta/2)); +rotVec = [rotX;rotY;rotZ]; + +% calculate transfer matrix from quaternion to rotation vector +G = jacobian(rotVec, quat); + +% define a symbolic covariance matrix using strings to represent +% '_l_' to represent '( ' +% '_c_' to represent , +% '_r_' to represent ')' +% these can be substituted later to create executable code +for rowIndex = 1:4 + for colIndex = 1:4 + eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']); + eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']); + end +end + +% rotate the covariance from quaternion to rotation vector +rotCovMat = G*quatCovMat*transpose(G); + +% take the variances +rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)]; + +% convert to c-code +ccode(rotVarVec,'file','rotVarVec.c'); diff --git a/matlab/scripts/Inertial Nav EKF/rotVarVec.c b/matlab/scripts/Inertial Nav EKF/rotVarVec.c new file mode 100644 index 0000000000..7eae50465d --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/rotVarVec.c @@ -0,0 +1,19 @@ + t2 = q0*q0; + t3 = acos(q0); + t4 = -t2+1.0; + t5 = t2-1.0; + t6 = 1.0/t5; + t7 = q1*t6*2.0; + t8 = 1.0/pow(t4,3.0/2.0); + t9 = q0*q1*t3*t8*2.0; + t10 = t7+t9; + t11 = 1.0/sqrt(t4); + t12 = q2*t6*2.0; + t13 = q0*q2*t3*t8*2.0; + t14 = t12+t13; + t15 = q3*t6*2.0; + t16 = q0*q3*t3*t8*2.0; + t17 = t15+t16; + A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0; + A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0; + A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0; From 1540e937b184345c1bd4becb30757200ed80f4e1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 10:42:37 +1000 Subject: [PATCH 2/6] EKF: Improve tilt alignment monitoring Convert quaternion covariances into an angular alignment variance vector and discard the z component so that yaw uncertainty does not affect the result. --- EKF/control.cpp | 15 ++++++++++----- EKF/ekf.h | 3 +++ EKF/ekf_helper.cpp | 39 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 52 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 006e933564..ffa7f37ce4 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -49,11 +49,16 @@ void Ekf::controlFusionModes() // Get the magnetic declination calcMagDeclination(); - // Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states - float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3]; - if (total_angle_variance < 0.002f && !_control_status.flags.tilt_align) { - _control_status.flags.tilt_align = true; - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + // monitor the tilt alignment + if (!_control_status.flags.tilt_align) { + // whilst we are aligning the tilt, monitor the variances + Vector3f angle_err_var_vec = calcRotVecVariances(); + + // Once the tilt variances have reduced sufficiently, initialise the yaw and magnetic field states + if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < 0.002f) { + _control_status.flags.tilt_align = true; + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } } // control use of various external souces for positon and velocity aiding diff --git a/EKF/ekf.h b/EKF/ekf.h index 554cd74977..013e01a5c6 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -363,4 +363,7 @@ private: // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(); + // rotate quaternion covariances into variances for an equivalent rotation vector + Vector3f calcRotVecVariances(); + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0e3ac8b008..05071fd7f4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -654,3 +654,42 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion quat) return dcm; } + +// calculate the variances for the rotation vector equivalent +Vector3f Ekf::calcRotVecVariances() +{ + Vector3f rot_var_vec; + float q0,q1,q2,q3; + if (_state.quat_nominal(0) >= 0.0f) { + q0 = _state.quat_nominal(0); + q1 = _state.quat_nominal(1); + q2 = _state.quat_nominal(2); + q3 = _state.quat_nominal(3); + } else { + q0 = -_state.quat_nominal(0); + q1 = -_state.quat_nominal(1); + q2 = -_state.quat_nominal(2); + q3 = -_state.quat_nominal(3); + } + float t2 = q0*q0; + float t3 = acos(q0); + float t4 = -t2+1.0f; + float t5 = t2-1.0f; + float t6 = 1.0f/t5; + float t7 = q1*t6*2.0f; + float t8 = 1.0f/powf(t4,1.5f); + float t9 = q0*q1*t3*t8*2.0f; + float t10 = t7+t9; + float t11 = 1.0f/sqrtf(t4); + float t12 = q2*t6*2.0f; + float t13 = q0*q2*t3*t8*2.0f; + float t14 = t12+t13; + float t15 = q3*t6*2.0f; + float t16 = q0*q3*t3*t8*2.0f; + float t17 = t15+t16; + rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; + rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; + rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; + + return rot_var_vec; +} From 88860d0307ffcf014a3adb1236dd7a7fe6df0558 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 11:02:56 +1000 Subject: [PATCH 3/6] EKF: Enable tuning for IMU switch on bias errors --- EKF/common.h | 8 ++++++++ EKF/covariance.cpp | 4 ++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index f7ac319286..aabf413b82 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -197,6 +197,10 @@ struct parameters { float terrain_p_noise; // process noise for terrain offset (m/sec) float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m) + // initial switch on bias uncertainty + float switch_on_gyro_bias; // 1-sigma gyro bias uncertainty at switch on (rad/sec) + float switch_on_accel_bias; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2) + // position and velocity fusion float gps_vel_noise; // observation noise for gps velocity fusion (m/sec) float gps_pos_noise; // observation noise for gps position fusion (m) @@ -288,6 +292,10 @@ struct parameters { terrain_p_noise = 5.0f; terrain_gradient = 0.5f; + // initial switch on bias uncertainty + switch_on_gyro_bias = 0.1f; + switch_on_accel_bias = 0.2f; + // position and velocity fusion gps_vel_noise = 5.0e-1f; gps_pos_noise = 0.5f; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 1947505877..1d485963e4 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -80,12 +80,12 @@ void Ekf::initialiseCovariance() } // gyro bias - P[10][10] = sq(0.1f * dt); + P[10][10] = sq(_params.switch_on_gyro_bias * dt); P[11][11] = P[10][10]; P[12][12] = P[10][10]; // accel bias - P[13][13] = sq(0.2f * dt); + P[13][13] = sq(_params.switch_on_accel_bias * dt); P[14][14] = P[13][13]; P[15][15] = P[13][13]; From a4728bc748b727cf4fc2567737f5768b0ee48ed9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 12:28:58 +1000 Subject: [PATCH 4/6] EKF: derive auto-code for quaternion state covariance initialisation --- .../QuatErrTransferEquations.m | 32 ++++++++-- matlab/scripts/Inertial Nav EKF/quatCovMat.c | 59 +++++++++++++++++++ 2 files changed, 86 insertions(+), 5 deletions(-) create mode 100644 matlab/scripts/Inertial Nav EKF/quatCovMat.c diff --git a/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m b/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m index 25b37b6a88..c603ae8817 100644 --- a/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m +++ b/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m @@ -1,4 +1,4 @@ -% calculate the variances for an equivalent rotation vector +%% calculate the rotation vector variances from an equivalent quaternion % inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions % output is a vector of variances for the rotation vector that is equivalent to the quaternion clear all; @@ -10,10 +10,7 @@ quat = [q0;q1;q2;q3]; % convert to a rotation vector delta = 2*acos(q0); -rotX = delta*(q1/sin(delta/2)); -rotY = delta*(q2/sin(delta/2)); -rotZ = delta*(q3/sin(delta/2)); -rotVec = [rotX;rotY;rotZ]; +rotVec = (delta/sin(delta/2))*[q1;q2;q3]; % calculate transfer matrix from quaternion to rotation vector G = jacobian(rotVec, quat); @@ -38,3 +35,28 @@ rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)]; % convert to c-code ccode(rotVarVec,'file','rotVarVec.c'); + +%% calculate the quaternion variances from an equivalent rotation vector + +% define a rotation vector +syms rotX rotY rotZ real; +rotVec = [rotX;rotY;rotZ]; + +% convert to a quaternion +vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2); +quat = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)]; + +% calculate transfer matrix from rotation vector to quaternion +G = jacobian(quat, rotVec); + +% define the rotation vector variances +syms rotVarX rotVarY rotVarZ real; + +% define the rotation vector covariance matrix +rotCovMat = diag([rotVarX;rotVarY;rotVarZ]); + +% rotate the covariance matrix into quaternion coordinates +quatCovMat = G*rotCovMat*transpose(G); + +% convert to c-code +ccode(quatCovMat,'file','quatCovMat.c'); diff --git a/matlab/scripts/Inertial Nav EKF/quatCovMat.c b/matlab/scripts/Inertial Nav EKF/quatCovMat.c new file mode 100644 index 0000000000..02fa81c243 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/quatCovMat.c @@ -0,0 +1,59 @@ +float t2 = rotX*rotX; +float t4 = rotY*rotY; +float t5 = rotZ*rotZ; +float t6 = t2+t4+t5; +float t7 = sqrtf(t6); +float t8 = t7*0.5f; +float t3 = sinf(t8); +float t9 = t3*t3; +float t10 = 1.0f/t6; +float t11 = 1.0f/sqrtf(t6); +float t12 = cosf(t8); +float t13 = 1.0f/powf(t6,1.5f); +float t14 = t3*t11; +float t15 = rotX*rotY*t3*t13; +float t16 = rotX*rotZ*t3*t13; +float t17 = rotY*rotZ*t3*t13; +float t18 = t2*t10*t12*0.5f; +float t27 = t2*t3*t13; +float t19 = t14+t18-t27; +float t23 = rotX*rotY*t10*t12*0.5f; +float t28 = t15-t23; +float t20 = rotY*rotVarY*t3*t11*t28*0.5f; +float t25 = rotX*rotZ*t10*t12*0.5f; +float t31 = t16-t25; +float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f; +float t22 = t20+t21-rotX*rotVarX*t3*t11*t19*0.5f; +float t24 = t15-t23; +float t26 = t16-t25; +float t29 = t4*t10*t12*0.5f; +float t34 = t3*t4*t13; +float t30 = t14+t29-t34; +float t32 = t5*t10*t12*0.5f; +float t40 = t3*t5*t13; +float t33 = t14+t32-t40; +float t36 = rotY*rotZ*t10*t12*0.5f; +float t39 = t17-t36; +float t35 = rotZ*rotVarZ*t3*t11*t39*0.5f; +float t37 = t15-t23; +float t38 = t17-t36; +float t41 = rotVarX*(t15-t23)*(t16-t25); +float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39; +float t43 = t16-t25; +float t44 = t17-t36; +P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f; +P[0][1] = t22; +P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f; +P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f; +P[1][0] = t22; +P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26); +P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30; +P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33; +P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f; +P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30; +P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38); +P[2][3] = t42; +P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f; +P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33; +P[3][2] = t42; +P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44); From 57b2a256f783c9041a843dcc9876be5b4a9933ce Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 12:39:11 +1000 Subject: [PATCH 5/6] EKF: Improve initialisation of quaternion covariances Convert uncertainty in initial rotate vector into quaternion covariances using symbolic toolbox derived expressions. Enable setting of initial angle uncertainty via a parameter --- EKF/common.h | 6 ++- EKF/covariance.cpp | 11 ++--- EKF/ekf.h | 3 ++ EKF/ekf_helper.cpp | 108 +++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 121 insertions(+), 7 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index aabf413b82..6c972d85d6 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -197,9 +197,10 @@ struct parameters { float terrain_p_noise; // process noise for terrain offset (m/sec) float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m) - // initial switch on bias uncertainty + // initialisation errors float switch_on_gyro_bias; // 1-sigma gyro bias uncertainty at switch on (rad/sec) float switch_on_accel_bias; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2) + float initial_tilt_err; // 1-sigma tilt error after initial alignment using gravity vector (rad) // position and velocity fusion float gps_vel_noise; // observation noise for gps velocity fusion (m/sec) @@ -292,9 +293,10 @@ struct parameters { terrain_p_noise = 5.0f; terrain_gradient = 0.5f; - // initial switch on bias uncertainty + // initialisation errors switch_on_gyro_bias = 0.1f; switch_on_accel_bias = 0.2f; + initial_tilt_err = 0.1f; // position and velocity fusion gps_vel_noise = 5.0e-1f; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 1d485963e4..eff7d3d161 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -55,11 +55,12 @@ void Ekf::initialiseCovariance() // calculate average prediction time step in sec float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS; - // quaternion error - P[0][0] = 0.01f; - P[1][1] = 0.01f; - P[2][2] = 0.01f; - P[3][3] = 0.01f; + // define the initial angle uncertainty as variances for a rotation vector + Vector3f rot_vec_var; + rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(_params.initial_tilt_err); + + // update the quaternion state covariances + initialiseQuatCovariances(rot_vec_var); // velocity P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f)); diff --git a/EKF/ekf.h b/EKF/ekf.h index 013e01a5c6..66691d5b46 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -366,4 +366,7 @@ private: // rotate quaternion covariances into variances for an equivalent rotation vector Vector3f calcRotVecVariances(); + // initialise the quaternion covariances using rotation vector variances + void initialiseQuatCovariances(Vector3f &rot_vec_var); + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 05071fd7f4..93a8549f56 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -693,3 +693,111 @@ Vector3f Ekf::calcRotVecVariances() return rot_var_vec; } + +// initialise the quaternion covariances using rotation vector variances +void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) +{ + // calculate an equivalent rotation vector from the quaternion + float q0,q1,q2,q3; + if (_state.quat_nominal(0) >= 0.0f) { + q0 = _state.quat_nominal(0); + q1 = _state.quat_nominal(1); + q2 = _state.quat_nominal(2); + q3 = _state.quat_nominal(3); + } else { + q0 = -_state.quat_nominal(0); + q1 = -_state.quat_nominal(1); + q2 = -_state.quat_nominal(2); + q3 = -_state.quat_nominal(3); + } + float delta = 2.0f*acosf(q0); + float scaler = (delta/sinf(delta*0.5f)); + float rotX = scaler*q1; + float rotY = scaler*q2; + float rotZ = scaler*q3; + + // autocode generated using matlab symbolic toolbox + float t2 = rotX*rotX; + float t4 = rotY*rotY; + float t5 = rotZ*rotZ; + float t6 = t2+t4+t5; + if (t6 > 1e-9f) { + float t7 = sqrtf(t6); + float t8 = t7*0.5f; + float t3 = sinf(t8); + float t9 = t3*t3; + float t10 = 1.0f/t6; + float t11 = 1.0f/sqrtf(t6); + float t12 = cosf(t8); + float t13 = 1.0f/powf(t6,1.5f); + float t14 = t3*t11; + float t15 = rotX*rotY*t3*t13; + float t16 = rotX*rotZ*t3*t13; + float t17 = rotY*rotZ*t3*t13; + float t18 = t2*t10*t12*0.5f; + float t27 = t2*t3*t13; + float t19 = t14+t18-t27; + float t23 = rotX*rotY*t10*t12*0.5f; + float t28 = t15-t23; + float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f; + float t25 = rotX*rotZ*t10*t12*0.5f; + float t31 = t16-t25; + float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f; + float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f; + float t24 = t15-t23; + float t26 = t16-t25; + float t29 = t4*t10*t12*0.5f; + float t34 = t3*t4*t13; + float t30 = t14+t29-t34; + float t32 = t5*t10*t12*0.5f; + float t40 = t3*t5*t13; + float t33 = t14+t32-t40; + float t36 = rotY*rotZ*t10*t12*0.5f; + float t39 = t17-t36; + float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f; + float t37 = t15-t23; + float t38 = t17-t36; + float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25); + float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39; + float t43 = t16-t25; + float t44 = t17-t36; + + // auto-code generated using matlab symbolic toolbox + P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f; + P[0][1] = t22; + P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f; + P[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f; + P[1][0] = t22; + P[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26); + P[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; + P[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; + P[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f; + P[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; + P[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38); + P[2][3] = t42; + P[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f; + P[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; + P[3][2] = t42; + P[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44); + + } else { + // the equations are badly conditioned so use a small angle approximation + P[0][0] = 0.0f; + P[0][1] = 0.0f; + P[0][2] = 0.0f; + P[0][3] = 0.0f; + P[1][0] = 0.0f; + P[1][1] = 0.25f*rot_vec_var(0); + P[1][2] = 0.0f; + P[1][3] = 0.0f; + P[2][0] = 0.0f; + P[2][1] = 0.0f; + P[2][2] = 0.25f*rot_vec_var(1); + P[2][3] = 0.0f; + P[3][0] = 0.0f; + P[3][1] = 0.0f; + P[3][2] = 0.0f; + P[3][3] = 0.25f*rot_vec_var(2); + + } +} From e4b2e9c93dfad03aec14e974991a309d732ba1ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 May 2016 10:34:23 +1000 Subject: [PATCH 6/6] EKF: Improve yaw alignment Uses best conditioned of 321 or 312 Euler sequence to calculate initial yaw angle. Allows alignment of yaw angle using external vision data --- EKF/ekf_helper.cpp | 124 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 104 insertions(+), 20 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 93a8549f56..a6748de7f1 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -302,35 +302,119 @@ void Ekf::alignOutputFilter() // Reset heading and magnetic field states bool Ekf::resetMagHeading(Vector3f &mag_init) { - // If we don't a tilt estimate then we cannot initialise the yaw - if (!_control_status.flags.tilt_align) { - return false; + + // update transformation matrix from body to world frame using the current estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + // calculate the initial quaternion + // determine if a 321 or 312 Euler sequence is best + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { + // use a 321 sequence + + // rotate the magnetometer measurement into earth frame + matrix::Euler euler321(_state.quat_nominal); + + // Set the yaw angle to zero and calculate the rotation matrix from body to earth frame + euler321(2) = 0.0f; + matrix::Dcm R_to_earth(euler321); + + // calculate the observed yaw angle + if (_params.fusion_mode & MASK_USE_EVYAW) { + // convert the observed quaternion to a rotation matrix + matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0)); + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // the angle of the projection onto the horizontal gives the yaw angle + euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else { + // there is no yaw observation + return false; + } + + // calculate initial quaternion states for the ekf + // we don't change the output attitude to avoid jumps + _state.quat_nominal = Quaternion(euler321); + + } else { + // use a 312 sequence + + // Calculate the 312 sequence euler angles that rotate from earth to body frame + // See http://www.atacolorado.com/eulersequences.doc + Vector3f euler312; + euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw) + euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll) + euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch) + + // Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame + euler312(0) = 0.0f; + + // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence + float c2 = cosf(euler312(2)); + float s2 = sinf(euler312(2)); + float s1 = sinf(euler312(1)); + float c1 = cosf(euler312(1)); + float s0 = sinf(euler312(0)); + float c0 = cosf(euler312(0)); + + matrix::Dcm R_to_earth; + R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; + R_to_earth(1, 1) = c0 * c1; + R_to_earth(2, 2) = c2 * c1; + R_to_earth(0, 1) = -c1 * s0; + R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0; + R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0; + R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2; + R_to_earth(2, 0) = -s2 * c1; + R_to_earth(2, 1) = s1; + + // calculate the observed yaw angle + if (_params.fusion_mode & MASK_USE_EVYAW) { + // convert the observed quaternion to a rotation matrix + matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1)); + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // the angle of the projection onto the horizontal gives the yaw angle + euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else { + // there is no yaw observation + return false; + } + + // re-calculate the rotation matrix using the updated yaw angle + s0 = sinf(euler312(0)); + c0 = cosf(euler312(0)); + R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2; + R_to_earth(1, 1) = c0 * c1; + R_to_earth(2, 2) = c2 * c1; + R_to_earth(0, 1) = -c1 * s0; + R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0; + R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0; + R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2; + R_to_earth(2, 0) = -s2 * c1; + R_to_earth(2, 1) = s1; + + // calculate initial quaternion states for the ekf + // we don't change the output attitude to avoid jumps + _state.quat_nominal = Quaternion(R_to_earth); } - // get the roll, pitch, yaw estimates and set the yaw to zero - matrix::Quaternion q(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), - _state.quat_nominal(3)); - matrix::Euler euler_init(q); - euler_init(2) = 0.0f; - - // rotate the magnetometer measurements into earth axes - matrix::Dcm R_to_earth_zeroyaw(euler_init); - Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init; - euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); - - // calculate initial quaternion states for the ekf - // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quaternion(euler_init); - // reset the quaternion variances because the yaw angle could have changed by a significant amount // by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise // will grow them again. zeroRows(P, 0, 3); zeroCols(P, 0, 3); + // update transformation matrix from body to world frame using the current estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + // calculate initial earth magnetic field states - matrix::Dcm R_to_earth(euler_init); - _state.mag_I = R_to_earth * mag_init; + _state.mag_I = _R_to_earth * mag_init; // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance zeroRows(P, 16, 21);