diff --git a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt index ea4a5ba1d3..f098aeab8a 100644 --- a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt @@ -1,32 +1,57 @@ /* -Autocode for fusion of a yaw error measurement where the innovation is given by: +Observation jacobian for fusion of the horizontal components of magnetometer measurements. -innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle; +innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained +by rotating the measured magnetometer readings from body into earth axes. -magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame. -declinationAngle is the estimated declination as that location +This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use -Protection against /0 and covariance matrix errors will need to be added. +Divide by zero protection hs been added */ -// intermediate variables +// calculate intermediate variables for observation jacobian float t2 = q0*q0; float t3 = q1*q1; float t4 = q2*q2; float t5 = q3*q3; -float t6 = t2+t3-t4-t5; -float t7 = q0*q3*2.0f; -float t8 = q1*q2*2.0f; -float t9 = t7+t8; -float t10 = 1.0f/(t6*t6); -float t11 = t9*t9; -float t12 = t10*t11; -float t13 = t12+1.0f; -float t14 = 1.0f/t13; -float t15 = 1.0f/t6; +float t6 = q0*q3*2.0f; +float t8 = t2-t3+t4-t5; +float t9 = q0*q1*2.0f; +float t10 = q2*q3*2.0f; +float t11 = t9-t10; +float t14 = q1*q2*2.0f; +float t21 = magY*t8; +float t22 = t6+t14; +float t23 = magX*t22; +float t24 = magZ*t11; +float t7 = t21+t23-t24; +float t12 = t2+t3-t4-t5; +float t13 = magX*t12; +float t15 = q0*q2*2.0f; +float t16 = q1*q3*2.0f; +float t17 = t15+t16; +float t18 = magZ*t17; +float t19 = t6-t14; +float t25 = magY*t19; +float t20 = t13+t18-t25; +if (fabsf(t20) < 1e-6f) { + return; +} +float t26 = 1.0f/(t20*t20); +float t27 = t7*t7; +float t28 = t26*t27; +float t29 = t28+1.0; +if (fabsf(t29) < 1e-12f) { + return; +} +float t30 = 1.0f/t29; +if (fabsf(t20) < 1e-12f) { + return; +} +float t31 = 1.0f/t20; -// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only -// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update -float H_MAG[3]; -H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f)); -H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); \ No newline at end of file +// calculate observation jacobian +float H_DECL[3] = {}; +H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19)); +H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17)); +H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19)); diff --git a/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt b/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt new file mode 100644 index 0000000000..010ede7cd5 --- /dev/null +++ b/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt @@ -0,0 +1,27 @@ +/* +Autocode for fusion of an Euler yaw measurement. + +Protection against /0 and covariance matrix errors will need to be added. +*/ + +// intermediate variables +float t2 = q0*q0; +float t3 = q1*q1; +float t4 = q2*q2; +float t5 = q3*q3; +float t6 = t2+t3-t4-t5; +float t7 = q0*q3*2.0f; +float t8 = q1*q2*2.0f; +float t9 = t7+t8; +float t10 = 1.0f/(t6*t6); +float t11 = t9*t9; +float t12 = t10*t11; +float t13 = t12+1.0f; +float t14 = 1.0f/t13; +float t15 = 1.0f/t6; + +// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only +// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update +float H_MAG[3]; +H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f)); +H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index 1a200935e8..89715472bd 100644 --- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -1,6 +1,6 @@ % IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run -% Derivation of Navigation EKF using a local NED earth Tangent Frame and +% Derivation of Navigation EKF using a local NED earth Tangent Frame and % XYZ body fixed frame % Sequential fusion of velocity and position measurements % Fusion of true airspeed @@ -15,8 +15,8 @@ % Based on use of a rotation vector for attitude estimation as described % here: -% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", -% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003), +% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", +% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003), % pp. 855-860. % State vector: @@ -68,8 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2 syms ptd real % location of terrain in D axis syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame syms decl real; % earth magnetic field declination from true north -syms R_YAW real; % variance for magnetic deviation measurement -syms R_DECL real; % variance of supplied declination +syms R_DECL R_YAW real; % variance of declination or yaw angle observation syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes syms rho real % air density (kg/m^3) syms R_ACC real % variance of accelerometer measurements (m/s^2)^2 @@ -102,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat); Tbn = Quat2Tbn(truthQuat); % define the truth delta angle -% ignore coning compensation as these effects are negligible in terms of +% ignore coning compensation as these effects are negligible in terms of % covariance growth for our application and grade of sensor dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise]; @@ -189,10 +188,10 @@ F = jacobian(newStateVector, stateVector); F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [F,SF]=OptimiseAlgebra(F,'SF'); -% define a symbolic covariance matrix using strings to represent +% define a symbolic covariance matrix using strings to represent % '_l_' to represent '( ' % '_c_' to represent , -% '_r_' to represent ')' +% '_r_' to represent ')' % these can be substituted later to create executable code for rowIndex = 1:nStates for colIndex = 1:nStates @@ -264,12 +263,33 @@ K_LOS = [K_LOSX,K_LOSY]; simplify(K_LOS); [K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS'); -% Use matlab c code converter for an alternate method of +% Use matlab c code converter for an alternate method of ccode(H_LOS,'file','H_LOS.txt'); ccode(K_LOSX,'file','K_LOSX.txt'); ccode(K_LOSY,'file','K_LOSY.txt'); -%% derive equations for fusion of direct yaw measurement +%% derive equations for simple fusion of 2-D magnetic heading measurements + +% rotate magnetic field into earth axes +magMeasNED = Tbn*[magX;magY;magZ]; +% the predicted measurement is the angle wrt true north of the horizontal +% component of the measured field +angMeas = atan(magMeasNED(2)/magMeasNED(1)); +simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b]; +Psimple = P(1:16,1:16); +H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian +%H_MAGS = H_MAGS(1:3); +H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +%H_MAGS = simplify(H_MAGS); +%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS'); +ccode(H_MAGS,'file','calcH_MAGS.c'); +% Calculate Kalman gain vector +K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL); +%K_MAGS = simplify(K_MAGS); +%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS'); +ccode(K_MAGS,'file','calcK_MAGS.c'); + +%% derive equations for fusion of yaw measurements % rotate X body axis into earth axes yawVec = Tbn*[1;0;0]; @@ -298,7 +318,7 @@ H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_MAGD = simplify(H_MAGD); %[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD'); -%ccode(H_MAGD,'file','calcH_MAGD.c'); +ccode(H_MAGD,'file','calcH_MAGD.c'); % Calculate Kalman gain vector K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL); ccode([H_MAGD',K_MAGD],'file','calcMAGD.c'); @@ -319,7 +339,7 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity % accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis % Use a simple viscous drag model for the linear estimator equations -% Use the the derivative from speed to acceleration averaged across the +% Use the the derivative from speed to acceleration averaged across the % speed range % The nonlinear equation will be used to calculate the predicted % measurement in implementation @@ -339,14 +359,12 @@ H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC); +ccode([H_ACCY',K_ACCY],'file','calcACCY.c'); [K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector -% generate c code for jacobians using the matlab code generator -ccode([H_ACCX;H_ACCY],'file','calcH_ACCXY.c'); - %% Save output and convert to m and c code fragments fileName = strcat('SymbolicOutput',int2str(nStates),'.mat'); save(fileName); SaveScriptCode(nStates); ConvertToM(nStates); -ConvertToC(nStates); \ No newline at end of file +ConvertToC(nStates);