diff --git a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt index 8e1d146575..c1455008a5 100644 --- a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt @@ -9,62 +9,56 @@ declinationAngle is the estimated declination as that location This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements (e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors can cause the magnetic declination of the earth field estimates to slowly rotate. + +Divide by zero protection and protection against a badly conditioned covariance matrix must be included. */ // Calculate intermediate variable -float t2 = 1.0f/magN; -float t4 = magE*t2; -float t3 = tanf(t4); -float t5 = t3*t3; -float t6 = t5+1.0f; -float t25 = t2*t6; -float t7 = 1.0f/(magN*magN); -float t26 = magE*t6*t7; -float t8 = P[17][17]*t25; -float t15 = P[16][17]*t26; -float t9 = t8-t15; -float t10 = t25*t9; -float t11 = P[17][16]*t25; -float t16 = P[16][16]*t26; -float t12 = t11-t16; -float t17 = t26*t12; -float t13 = R_DECL+t10-t17; -float t14 = 1.0f/t13; -float t18 = magE; -float t19 = magN; -float t21 = 1.0f/t19; -float t22 = t18*t21; -float t20 = tanf(t22); -float t23 = t20*t20; -float t24 = t23+1.0f; +float t2 = magE*magE; +float t3 = magN*magN; +float t4 = t2+t3; +float t5 = 1.0f/t4; +float t22 = magE*t5; +float t23 = magN*t5; +float t6 = P[16][16]*t22; +float t13 = P[17][16]*t23; +float t7 = t6-t13; +float t8 = t22*t7; +float t9 = P[16][17]*t22; +float t14 = P[17][17]*t23; +float t10 = t9-t14; +float t15 = t23*t10; +float t11 = R_DECL+t8-t15; // innovation variance +float t12 = 1.0f/t11; // Calculate the observation Jacobian // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost -H_MAG[16] = -t18*1.0f/(t19*t19)*t24; -H_MAG[17] = t21*t24; +H_DECL[16] = -magE*t5; +H_DECL[17] = magN*t5; // Calculate the Kalman gains -Kfusion[0] = t14*(P[0][17]*t25-P[0][16]*t26); -Kfusion[1] = t14*(P[1][17]*t25-P[1][16]*t26); -Kfusion[2] = t14*(P[2][17]*t25-P[2][16]*t26); -Kfusion[3] = t14*(P[3][17]*t25-P[3][16]*t26); -Kfusion[4] = t14*(P[4][17]*t25-P[4][16]*t26); -Kfusion[5] = t14*(P[5][17]*t25-P[5][16]*t26); -Kfusion[6] = t14*(P[6][17]*t25-P[6][16]*t26); -Kfusion[7] = t14*(P[7][17]*t25-P[7][16]*t26); -Kfusion[8] = t14*(P[8][17]*t25-P[8][16]*t26); -Kfusion[9] = t14*(P[9][17]*t25-P[9][16]*t26); -Kfusion[10] = t14*(P[10][17]*t25-P[10][16]*t26); -Kfusion[11] = t14*(P[11][17]*t25-P[11][16]*t26); -Kfusion[12] = t14*(P[12][17]*t25-P[12][16]*t26); -Kfusion[13] = t14*(P[13][17]*t25-P[13][16]*t26); -Kfusion[14] = t14*(P[14][17]*t25-P[14][16]*t26); -Kfusion[15] = t14*(P[15][17]*t25-P[15][16]*t26); -Kfusion[16] = -t14*(t16-P[16][17]*t25); -Kfusion[17] = t14*(t8-P[17][16]*t26); -Kfusion[18] = t14*(P[18][17]*t25-P[18][16]*t26); -Kfusion[19] = t14*(P[19][17]*t25-P[19][16]*t26); -Kfusion[20] = t14*(P[20][17]*t25-P[20][16]*t26); -Kfusion[21] = t14*(P[21][17]*t25-P[21][16]*t26); -Kfusion[22] = t14*(P[22][17]*t25-P[22][16]*t26); -Kfusion[23] = t14*(P[23][17]*t25-P[23][16]*t26); +Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23); +Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23); +Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23); +Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23); +Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23); +Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23); +Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23); +Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23); +Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23); +Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23); +Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23); +Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23); +Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23); +Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23); +Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23); +Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23); +Kfusion[16] = -t12*(t6-P[16][17]*t23); +Kfusion[17] = t12*(t14-P[17][16]*t22); +Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23); +Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23); +Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23); +Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23); +Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23); +Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23); + diff --git a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt index 606c51d51b..ea4a5ba1d3 100644 --- a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt @@ -5,6 +5,8 @@ innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationA magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame. declinationAngle is the estimated declination as that location + +Protection against /0 and covariance matrix errors will need to be added. */ // intermediate variables @@ -12,36 +14,19 @@ float t2 = q0*q0; float t3 = q1*q1; float t4 = q2*q2; float t5 = q3*q3; -float t6 = q0*q2*2.0f; -float t7 = q1*q3*2.0f; -float t8 = t6+t7; -float t9 = q0*q3*2.0f; -float t13 = q1*q2*2.0f; -float t10 = t9-t13; -float t11 = t2+t3-t4-t5; -float t12 = magX*t11; -float t14 = magZ*t8; -float t19 = magY*t10; -float t15 = t12+t14-t19; -float t16 = t2-t3+t4-t5; -float t17 = q0*q1*2.0f; -float t24 = q2*q3*2.0f; -float t18 = t17-t24; -float t20 = 1.0f/t15; -float t21 = magY*t16; -float t22 = t9+t13; -float t23 = magX*t22; -float t28 = magZ*t18; -float t25 = t21+t23-t28; -float t29 = t20*t25; -float t26 = tan(t29); -float t27 = 1.0f/(t15*t15); -float t30 = t26*t26; -float t31 = t30+1.0f; +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_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10)); -H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11)); -H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11)); +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 diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index 7af03eecca..1a200935e8 100644 --- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -68,7 +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_MAGS real; % variance for magnetic deviation measurement +syms R_YAW real; % variance for magnetic deviation measurement syms R_DECL real; % variance of supplied declination 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) @@ -269,19 +269,22 @@ 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 magnetic heading measurement +%% derive equations for fusion of direct yaw measurement -% rotate magnetic field into earth axes -magMeasNED = Tbn*[magX;magY;magZ]; -% the predicted measurement is the angle wrt magnetic north of the horizontal -% component of the measured field -angMeas = tan(magMeasNED(2)/magMeasNED(1)) - decl; -H_MAGS = jacobian(angMeas,errRotVec); % measurement Jacobian +% rotate X body axis into earth axes +yawVec = Tbn*[1;0;0]; +% Calculate the yaw angle of the projection +angMeas = atan(yawVec(2)/yawVec(1)); +H_YAW = jacobian(angMeas,stateVector); % 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_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_YAW = simplify(H_YAW); %[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS'); -ccode(H_MAGS,'file','calcH_MAGS.c'); +ccode(H_YAW,'file','calcH_YAW.c'); +% Calculate Kalman gain vector +K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW); +%K_MAGS = simplify(K_MAGS); +ccode(K_YAW,'file','calcK_YAW.c'); %% derive equations for fusion of synthetic deviation measurement % used to keep correct heading when operating without absolute position or @@ -290,7 +293,7 @@ ccode(H_MAGS,'file','calcH_MAGS.c'); magMeasNED = [magN;magE;magD]; % the predicted measurement is the angle wrt magnetic north of the horizontal % component of the measured field -angMeas = tan(magMeasNED(2)/magMeasNED(1)); +angMeas = atan(magMeasNED(2)/magMeasNED(1)); H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_MAGD = simplify(H_MAGD);