diff --git a/matlab/scripts/Inertial Nav EKF/ConvertToC.m b/matlab/scripts/Inertial Nav EKF/ConvertToC.m
new file mode 100644
index 0000000000..7c18441859
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/ConvertToC.m	
@@ -0,0 +1,213 @@
+function ConvertToC(nStates)
+%% Define file to read in
+fileName = strcat('M_code',int2str(nStates),'.txt');
+delimiter = '';
+
+%% Format string for each line of text:
+%   column1: text (%s)
+% For more information, see the TEXTSCAN documentation.
+formatSpec = '%s%[^\n\r]';
+
+%% Open the text file.
+fileID = fopen(fileName,'r');
+
+%% Read columns of data according to format string.
+% This call is based on the structure of the file used to generate this
+% code. If an error occurs for a different file, try regenerating the code
+% from the Import Tool.
+dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter,  'ReturnOnError', false, 'Bufsize', 65535);
+
+%% Close the text file.
+fclose(fileID);
+
+%% Create output variable
+SymbolicOutput = [dataArray{1:end-1}];
+
+%% Clear temporary variables
+clearvars filename delimiter formatSpec fileID dataArray ans;
+
+%% Convert indexing and replace brackets
+
+% replace 1-D indexes
+for arrayIndex = 1:99
+    strIndex = int2str(arrayIndex);
+    strRep = sprintf('[%d]',(arrayIndex-1));
+    strPat = strcat('\(',strIndex,'\)');
+    for lineIndex = 1:length(SymbolicOutput)
+        str = char(SymbolicOutput(lineIndex));
+        SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
+    end
+end
+
+% replace 2-D left indexes
+for arrayIndex = 1:99
+    strIndex = int2str(arrayIndex);
+    strRep = sprintf('[%d,',(arrayIndex-1));
+    strPat = strcat('\(',strIndex,'\,');
+    for lineIndex = 1:length(SymbolicOutput)
+        str = char(SymbolicOutput(lineIndex));
+        SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
+    end
+end
+
+% replace 2-D right indexes
+for arrayIndex = 1:99
+    strIndex = int2str(arrayIndex);
+    strRep = sprintf(',%d]',(arrayIndex-1));
+    strPat = strcat('\,',strIndex,'\)');
+    for lineIndex = 1:length(SymbolicOutput)
+        str = char(SymbolicOutput(lineIndex));
+        SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
+    end
+end
+
+% replace commas
+for lineIndex = 1:length(SymbolicOutput)
+    str = char(SymbolicOutput(lineIndex));
+    SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
+end
+
+%% Replace ^2
+
+% replace where adjacent to ) parenthesis
+for lineIndex = 1:length(SymbolicOutput)
+    idxsq = regexp(SymbolicOutput(lineIndex),'\)\^2');
+    if ~isempty(idxsq{1})
+        strIn = SymbolicOutput(lineIndex);
+        idxlp = regexp(strIn,'\(');
+        idxrp = regexp(strIn,'\)');
+        for pwrIndex = 1:length(idxsq{1})
+            counter = 1;
+            index = idxsq{1}(pwrIndex);
+            endIndex(pwrIndex) = index;
+            while (counter > 0 && index > 0)
+                index = index - 1;
+                counter = counter + ~isempty(find(idxrp{1} == index, 1));
+                counter = counter - ~isempty(find(idxlp{1} == index, 1));
+            end
+            startIndex(pwrIndex) = index;
+            %            strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
+            strRep = strcat('sq',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)));
+            %            cellStrPat(pwrIndex) = cellstr(strPat);
+            cellStrRep(pwrIndex) = cellstr(strRep);
+        end
+        for pwrIndex = 1:length(idxsq{1})
+            strRep = char(cellStrRep(pwrIndex));
+            strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+2) = strRep;
+        end
+        SymbolicOutput(lineIndex) = strIn;
+    end
+end
+
+% replace where adjacent to ] parenthesis
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    [match,idxsq1,idxsq2] = regexp(strIn,'\w*\[\w*\]\^2','match','start','end');
+    [idxsq3] = regexp(strIn,'\[\w*\]\^2','start');
+    if ~isempty(match)
+        for pwrIndex = 1:length(match)
+            strVar   = strIn(idxsq1(pwrIndex):idxsq3(pwrIndex)-1);
+            strIndex = strIn(idxsq3(pwrIndex)+1:idxsq2(pwrIndex)-3);
+            strPat = strcat(strVar,'\[',strIndex,'\]\^2');
+            strRep = strcat('sq(',strVar,'[',strIndex,']',')');
+            strIn = regexprep(strIn,strPat,strRep);
+        end
+        SymbolicOutput(lineIndex) = cellstr(strIn);
+    end
+end
+
+% replace where adjacent to alpanumeric characters
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    [match,idxsq1,idxsq2] = regexp(strIn,'\w*\^2','match','start','end');
+    [idxsq3] = regexp(strIn,'\^2','start');
+    if ~isempty(match)
+        for pwrIndex = 1:length(match)
+            strVar   = strIn(idxsq1(pwrIndex)+2*(pwrIndex-1):idxsq2(pwrIndex)-2+2*(pwrIndex-1));
+            strPat = strcat(strVar,'\^2');
+            strRep = strcat('sq(',strVar,')');
+            strIn = regexprep(strIn,strPat,strRep);
+        end
+        SymbolicOutput(lineIndex) = cellstr(strIn);
+    end
+end
+
+%% Replace ^(1/2)
+
+% replace where adjacent to ) parenthesis
+for lineIndex = 1:length(SymbolicOutput)
+    idxsq = regexp(SymbolicOutput(lineIndex),'\)\^\(1\/2\)');
+    if ~isempty(idxsq{1})
+        strIn = SymbolicOutput(lineIndex);
+        idxlp = regexp(strIn,'\(');
+        idxrp = regexp(strIn,'\)');
+        for pwrIndex = 1:length(idxsq{1})
+            counter = 1;
+            index = idxsq{1}(pwrIndex);
+            endIndex(pwrIndex) = index;
+            while (counter > 0 && index > 0)
+                index = index - 1;
+                counter = counter + ~isempty(find(idxrp{1} == index, 1));
+                counter = counter - ~isempty(find(idxlp{1} == index, 1));
+            end
+            startIndex(pwrIndex) = index;
+            %            strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
+            strRep = strcat('(sqrt',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),')');
+            %            cellStrPat(pwrIndex) = cellstr(strPat);
+            cellStrRep(pwrIndex) = cellstr(strRep);
+        end
+        for pwrIndex = 1:length(idxsq{1})
+            strRep = char(cellStrRep(pwrIndex));
+            strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+6) = strRep;
+        end
+        SymbolicOutput(lineIndex) = strIn;
+    end
+end
+
+%% Replace Divisions
+% Compiler looks after this type of optimisation for us
+% for lineIndex = 1:length(SymbolicOutput)
+%     strIn = char(SymbolicOutput(lineIndex));
+%     strIn = regexprep(strIn,'\/2','\*0\.5');
+%     strIn = regexprep(strIn,'\/4','\*0\.25');
+%     SymbolicOutput(lineIndex) = cellstr(strIn);
+% end
+
+%% Convert declarations
+for lineIndex = 1:length(SymbolicOutput)
+    str = char(SymbolicOutput(lineIndex));
+    if ~isempty(regexp(str,'zeros', 'once'))
+        index1 = regexp(str,' = zeros[','once')-1;
+        index2 = regexp(str,' = zeros[','end','once')+1;
+        index3 = regexp(str,'\]\[','once')-1;
+        index4 = index3 + 3;
+        index5 = max(regexp(str,'\]'))-1;
+        str1 = {'float '};
+        str2 = str(1:index1);
+        str3 = '[';
+        str4 = str(index2:index3);
+        str4 = num2str(str2num(str4)+1);
+        str5 = '][';
+        str6 = str(index4:index5);
+        str6 = num2str(str2num(str6)+1);
+        str7 = '];';
+        SymbolicOutput(lineIndex) = strcat(str1,str2,str3,str4,str5,str6,str7);
+    end
+end
+
+%% Change covariance matrix variable name to P
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    strIn = regexprep(strIn,'OP\[','P[');
+    SymbolicOutput(lineIndex) = cellstr(strIn);
+end
+
+%% Write to file
+fileName = strcat('C_code',int2str(nStates),'.txt');
+fid = fopen(fileName,'wt');
+for lineIndex = 1:length(SymbolicOutput)
+    fprintf(fid,char(SymbolicOutput(lineIndex)));
+    fprintf(fid,'\n');
+end
+fclose(fid);
+clear all;
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/ConvertToM.m b/matlab/scripts/Inertial Nav EKF/ConvertToM.m
new file mode 100644
index 0000000000..1e5f7f4388
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/ConvertToM.m	
@@ -0,0 +1,46 @@
+function ConvertToM(nStates)
+%% Initialize variables
+fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
+delimiter = '';
+
+%% Format string for each line of text:
+%   column1: text (%s)
+% For more information, see the TEXTSCAN documentation.
+formatSpec = '%s%[^\n\r]';
+
+%% Open the text file.
+fileID = fopen(fileName,'r');
+
+%% Read columns of data according to format string.
+% This call is based on the structure of the file used to generate this
+% code. If an error occurs for a different file, try regenerating the code
+% from the Import Tool.
+dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter,  'ReturnOnError', false,'Bufsize',65535);
+
+%% Close the text file.
+fclose(fileID);
+
+%% Create output variable
+SymbolicOutput = [dataArray{1:end-1}];
+
+%% Clear temporary variables
+clearvars filename delimiter formatSpec fileID dataArray ans;
+
+%% replace brackets and commas
+for lineIndex = 1:length(SymbolicOutput)
+    SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
+    SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
+    SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
+end
+
+%% Write to file
+fileName = strcat('M_code',int2str(nStates),'.txt');
+fid = fopen(fileName,'wt');
+for lineIndex = 1:length(SymbolicOutput)
+    fprintf(fid,char(SymbolicOutput(lineIndex)));
+    fprintf(fid,'\n');
+end
+fclose(fid);
+clear all;
+
+end
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/EulToQuat.m b/matlab/scripts/Inertial Nav EKF/EulToQuat.m
new file mode 100644
index 0000000000..9b57a20b5d
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/EulToQuat.m	
@@ -0,0 +1,23 @@
+function quaterion = EulToQuat(Euler)
+
+% Convert from a 321 Euler rotation sequence specified in radians to a
+% Quaternion
+
+quaterion = zeros(4,1);
+
+Euler    = Euler * 0.5;
+cosPhi   = cos(Euler(1));
+sinPhi   = sin(Euler(1));
+cosTheta = cos(Euler(2));
+sinTheta = sin(Euler(2));
+cosPsi   = cos(Euler(3));
+sinPsi   = sin(Euler(3));
+
+quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi);
+quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi);
+quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
+quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
+
+return;
+
+   
diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
new file mode 100644
index 0000000000..7af03eecca
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m	
@@ -0,0 +1,349 @@
+% 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 
+% XYZ body fixed frame
+% Sequential fusion of velocity and position measurements
+% Fusion of true airspeed
+% Sequential fusion of magnetic flux measurements
+% 24 state architecture.
+% IMU data is assumed to arrive at a constant rate with a time step of dt
+% IMU delta angle and velocity data are used as time varying parameters,
+% not observations
+
+% Author:  Paul Riseborough
+
+% 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), 
+% pp. 855-860.
+
+% State vector:
+% error rotation vector in body frame (X,Y,Z)
+% Velocity - m/sec (North, East, Down)
+% Position - m (North, East, Down)
+% Delta Angle bias - rad (X,Y,Z)
+% Delta Angle scale factor (X,Y,Z)
+% Delta Velocity bias - m/s (Z)
+% Earth Magnetic Field Vector - (North, East, Down)
+% Body Magnetic Field Vector - (X,Y,Z)
+% Wind Vector  - m/sec (North,East)
+
+% Observations:
+% NED velocity - m/s
+% NED position - m
+% True airspeed - m/s
+% angle of sideslip - rad
+% XYZ magnetic flux
+
+% Time varying parameters:
+% XYZ delta angle measurements in body axes - rad
+% XYZ delta velocity measurements in body axes - m/sec
+
+
+%% define symbolic variables and constants
+clear all;
+reset(symengine);
+syms dax day daz real % IMU delta angle measurements in body axes - rad
+syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
+syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
+syms vn ve vd real % NED velocity - m/sec
+syms pn pe pd real % NED position - m
+syms dax_b day_b daz_b real % delta angle bias - rad
+syms dax_s day_s daz_s real % delta angle scale factor
+syms dvz_b real % delta velocity bias - m/sec
+syms dt real % IMU time step - sec
+syms gravity real % gravity  - m/sec^2
+syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise
+syms vwn vwe real; % NE wind velocity - m/sec
+syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss
+syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
+syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2
+syms R_PN R_PE R_PD real % variances for NED position measurements - m^2
+syms R_TAS real  % variance for true airspeed measurement - (m/sec)^2
+syms R_MAG real  % variance for magnetic flux measurements - milligauss^2
+syms R_BETA real % variance of sidelsip measurements rad^2
+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_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)
+syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
+syms Kacc real % ratio of horizontal acceleration to top speed for a multirotor
+
+%% define the process equations
+
+% define the measured Delta angle and delta velocity vectors
+dAngMeas = [dax; day; daz];
+dVelMeas = [dvx; dvy; dvz];
+
+% define the IMU bias errors and scale factor
+dAngBias = [dax_b; day_b; daz_b];
+dAngScale = [dax_s; day_s; daz_s];
+dVelBias = [0;0;dvz_b];
+
+% define the quaternion rotation vector for the state estimate
+estQuat = [q0;q1;q2;q3];
+
+% define the attitude error rotation vector, where error = truth - estimate
+errRotVec = [rotErrX;rotErrY;rotErrZ];
+
+% define the attitude error quaternion using a first order linearisation
+errQuat = [1;0.5*errRotVec];
+
+% Define the truth quaternion as the estimate + error
+truthQuat = QuatMult(estQuat, errQuat);
+
+% derive the truth body to nav direction cosine matrix
+Tbn = Quat2Tbn(truthQuat);
+
+% define the truth delta angle
+% 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];
+
+% define the attitude update equations
+% use a first order expansion of rotation to calculate the quaternion increment
+% acceptable for propagation of covariances
+deltaQuat = [1;
+    0.5*dAngTruth(1);
+    0.5*dAngTruth(2);
+    0.5*dAngTruth(3);
+    ];
+truthQuatNew = QuatMult(truthQuat,deltaQuat);
+% calculate the updated attitude error quaternion with respect to the previous estimate
+errQuatNew = QuatDivide(truthQuatNew,estQuat);
+% change to a rotaton vector - this is the error rotation vector updated state
+errRotNew = 2 * [errQuatNew(2);errQuatNew(3);errQuatNew(4)];
+
+% Define the truth delta velocity -ignore sculling and transport rate
+% corrections as these negligible are in terms of covariance growth for our
+% application and grade of sensor
+dVelTruth = dVelMeas - dVelBias - [dvxNoise;dvyNoise;dvzNoise];
+
+% define the velocity update equations
+% ignore coriolis terms for linearisation purposes
+vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
+
+% define the position update equations
+pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
+
+% define the IMU error update equations
+dabNew = [dax_b; day_b; daz_b];
+dasNew = [dax_s; day_s; daz_s];
+dvbNew = dvz_b;
+
+% define the wind velocity update equations
+vwnNew = vwn;
+vweNew = vwe;
+
+% define the earth magnetic field update equations
+magNnew = magN;
+magEnew = magE;
+magDnew = magD;
+
+% define the body magnetic field update equations
+magXnew = magX;
+magYnew = magY;
+magZnew = magZ;
+
+% Define the state vector & number of states
+stateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b;magN;magE;magD;magX;magY;magZ;vwn;vwe];
+nStates=numel(stateVector);
+
+% Define vector of process equations
+newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
+
+%% derive the covariance prediction equation
+% This reduces the number of floating point operations by a factor of 6 or
+% more compared to using the standard matrix operations in code
+
+% Define the control (disturbance) vector. Error growth in the inertial
+% solution is assumed to be driven by 'noise' in the delta angles and
+% velocities, after bias effects have been removed. This is OK becasue we
+% have sensor bias accounted for in the state equations.
+distVector = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise];
+
+% derive the control(disturbance) influence matrix
+G = jacobian(newStateVector, distVector);
+G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+[G,SG]=OptimiseAlgebra(G,'SG');
+
+% derive the state error matrix
+distMatrix = diag(distVector);
+Q = G*distMatrix*transpose(G);
+[Q,SQ]=OptimiseAlgebra(Q,'SQ');
+
+% remove the disturbance noise from the process equations as it is only
+% needed when calculating the disturbance influence matrix
+vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
+errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
+
+% derive the state transition matrix
+F = jacobian(newStateVector, stateVector);
+% set the rotation error states to zero
+F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+[F,SF]=OptimiseAlgebra(F,'SF');
+
+% 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:nStates
+    for colIndex = 1:nStates
+        eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
+        eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
+    end
+end
+
+% Derive the predicted covariance matrix using the standard equation
+PP = F*P*transpose(F) + Q;
+
+% Collect common expressions to optimise processing
+[PP,SPP]=OptimiseAlgebra(PP,'SPP');
+
+%% derive equations for fusion of true airspeed measurements
+VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
+H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
+H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
+K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
+
+%% derive equations for fusion of angle of sideslip measurements
+% calculate wind relative velocities in nav frame and rotate into body frame
+Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd];
+% calculate predicted angle of sideslip using small angle assumption
+BetaPred = Vbw(2)/Vbw(1);
+H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
+H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
+K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
+
+%% derive equations for fusion of magnetic field measurement
+magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
+H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
+H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+[H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');
+
+K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
+[K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX');
+K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector
+[K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY');
+K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
+[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
+
+%% derive equations for sequential fusion of optical flow measurements
+
+% calculate range from plane to centre of sensor fov assuming flat earth
+% and camera axes aligned with body axes
+range = ((ptd - pd)/Tbn(3,3));
+% calculate relative velocity in body frame
+relVelBody = transpose(Tbn)*[vn;ve;vd];
+% divide by range to get predicted angular LOS rates relative to X and Y
+% axes. Note these are body angular rate motion compensated optical flow rates
+losRateX = +relVelBody(2)/range;
+losRateY = -relVelBody(1)/range;
+
+H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian
+H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+H_LOS = simplify(H_LOS);
+[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS');
+
+% combine into a single K matrix to enable common expressions to be found
+% note this matrix cannot be used in a single step fusion
+K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector
+K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+K_LOSY = (P*transpose(H_LOS(2,:)))/(H_LOS(2,:)*P*transpose(H_LOS(2,:)) + R_LOS); % Kalman gain vector
+K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+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 
+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
+
+% 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
+%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');
+
+%% derive equations for fusion of synthetic deviation measurement
+% used to keep correct heading when operating without absolute position or
+% velocity measurements - eg when using optical flow
+% rotate magnetic field into earth axes
+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));
+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');
+% 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');
+
+%% derive equations for fusion of lateral body acceleration (multirotors only)
+
+% use relationship between airspeed along the X and Y body axis and the
+% drag to predict the lateral acceleration for a multirotor vehicle type
+% where propulsion forces are generated primarily along the Z body axis
+
+vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
+
+% calculate drag assuming flight along axis in positive direction
+% sign change will be looked after in implementation rather than by adding
+% sign functions to symbolic derivation which genererates output with dirac
+% functions
+% accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis
+% 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 
+% speed range
+% The nonlinear equation will be used to calculate the predicted
+% measurement in implementation
+accXpred = -Kacc*vrel(1); % predicted acceleration measured along X body axis
+accYpred = -Kacc*vrel(2); % predicted acceleration measured along Y body axis
+
+% Derive observation Jacobian and Kalman gain matrix for X accel fusion
+H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
+H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
+K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
+ccode([H_ACCX',K_ACCX],'file','calcACCX.c');
+[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
+
+% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
+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);
+[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
diff --git a/matlab/scripts/Inertial Nav EKF/NormQuat.m b/matlab/scripts/Inertial Nav EKF/NormQuat.m
new file mode 100644
index 0000000000..7d1913efda
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/NormQuat.m	
@@ -0,0 +1,5 @@
+% normalise the quaternion
+function quaternion = normQuat(quaternion)
+
+quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
+quaternion(1:4) = quaternion / quatMag;
diff --git a/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m b/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m
new file mode 100644
index 0000000000..16d4c353ca
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m	
@@ -0,0 +1,29 @@
+function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
+
+% Loop through symbolic expression, identifying repeated expressions and
+% bringing them out as shared expression or sub expressions
+% do this until no further repeated expressions found
+% This can significantly reduce computations
+
+syms SubExpIn SubExpArray ;
+
+SubExpArray(1,1) = 'invalid';
+index = 0;
+f_complete = 0;
+while f_complete==0
+    index = index + 1;
+    SubExpIn = [SubExpName,'(',num2str(index),')'];
+    SubExpInStore{index} = SubExpIn;
+    [SymExpOut,SubExpOut]=subexpr(SymExpIn,SubExpIn);
+        for k = 1:index
+            if SubExpOut == SubExpInStore{k}
+                f_complete = 1;
+            end
+        end
+    if f_complete || index > 100
+        SymExpOut = SymExpIn;
+    else
+        SubExpArray(index,1) = SubExpOut;
+        SymExpIn = SymExpOut;
+    end
+end
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m b/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m
new file mode 100644
index 0000000000..663d19cde8
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m	
@@ -0,0 +1,14 @@
+function Tbn = Quat2Tbn(quat)
+
+% Convert from quaternions defining the flight vehicles rotation to
+% the direction cosine matrix defining the rotation from  body to navigation
+% coordinates
+
+q0 = quat(1);
+q1 = quat(2);
+q2 = quat(3);
+q3 = quat(4);
+  
+Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
+     2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
+     2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/QuatDivide.m b/matlab/scripts/Inertial Nav EKF/QuatDivide.m
new file mode 100644
index 0000000000..20e789dcfb
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/QuatDivide.m	
@@ -0,0 +1,16 @@
+function q_out = QuatDivide(qin1,qin2)
+
+q0 = qin1(1);
+q1 = qin1(2);
+q2 = qin1(3);
+q3 = qin1(4);
+
+r0 = qin2(1);
+r1 = qin2(2);
+r2 = qin2(3);
+r3 = qin2(4);
+
+q_out(1,1) = (qin2(1)*qin1(1) + qin2(2)*qin1(2) + qin2(3)*qin1(3) + qin2(4)*qin1(4));
+q_out(2,1) = (r0*q1 - r1*q0 - r2*q3 + r3*q2);
+q_out(3,1) = (r0*q2 + r1*q3 - r2*q0 - r3*q1);
+q_out(4,1) = (r0*q3 - r1*q2 + r2*q1 - r3*q0);
diff --git a/matlab/scripts/Inertial Nav EKF/QuatMult.m b/matlab/scripts/Inertial Nav EKF/QuatMult.m
new file mode 100644
index 0000000000..357c545d22
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/QuatMult.m	
@@ -0,0 +1,5 @@
+function quatOut = QuatMult(quatA,quatB)
+% Calculate the following quaternion product quatA * quatB using the 
+% standard identity 
+
+quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))];
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/QuatToEul.m b/matlab/scripts/Inertial Nav EKF/QuatToEul.m
new file mode 100644
index 0000000000..09b1505b6f
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/QuatToEul.m	
@@ -0,0 +1,9 @@
+% Convert from a quaternion to a 321 Euler rotation sequence in radians
+
+function Euler = QuatToEul(quat)
+
+Euler = zeros(3,1);
+
+Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)),  quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4));
+Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
+Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)),  quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4));
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/RotToQuat.m b/matlab/scripts/Inertial Nav EKF/RotToQuat.m
new file mode 100644
index 0000000000..3c9777acd5
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/RotToQuat.m	
@@ -0,0 +1,10 @@
+% convert froma rotation vector in radians to a quaternion
+function quaternion = RotToQuat(rotVec)
+
+vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
+
+if vecLength < 1e-6
+  quaternion = [1;0;0;0];
+else
+  quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
+end
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m b/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m
new file mode 100644
index 0000000000..a1f755136c
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m	
@@ -0,0 +1,647 @@
+function SaveScriptCode(nStates)
+%% Load Data
+fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
+load(fileName);
+
+%% Open output file
+fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
+fid = fopen(fileName,'wt');
+
+%% Write equation for state transition matrix
+if exist('SF','var')
+    
+    fprintf(fid,'SF = zeros(%d,1);\n',numel(SF));
+    for rowIndex = 1:numel(SF)
+        string = char(SF(rowIndex,1));
+        fprintf(fid,'SF(%d) = %s;\n',rowIndex,string);
+    end
+    
+    % fprintf(fid,'\n');
+    % fprintf(fid,'F = zeros(%d,%d);\n',nStates,nStates);
+    % for rowIndex = 1:nStates
+    %     for colIndex = 1:nStates
+    %         string = char(F(rowIndex,colIndex));
+    %         % don't write out a zero-assignment
+    %         if ~strcmpi(string,'0')
+    %             fprintf(fid,'F(%d,%d) = %s;\n',rowIndex,colIndex,string);
+    %         end
+    %     end
+    % end
+    % fprintf(fid,'\n');
+    
+end
+%% Write equations for control influence (disturbance) matrix
+if exist('SG','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SG = zeros(%d,1);\n',numel(SG));
+    for rowIndex = 1:numel(SG)
+        string = char(SG(rowIndex,1));
+        fprintf(fid,'SG(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    % fprintf(fid,'\n');
+    % fprintf(fid,'G = zeros(%d,%d);\n',nStates,numel([da;dv]));
+    % for rowIndex = 1:nStates
+    %     for colIndex = 1:numel([da;dv])
+    %         string = char(G(rowIndex,colIndex));
+    %         % don't write out a zero-assignment
+    %         if ~strcmpi(string,'0')
+    %             fprintf(fid,'G(%d,%d) = %s;\n',rowIndex,colIndex,string);
+    %         end
+    %     end
+    % end
+    % fprintf(fid,'\n');
+    
+end
+%% Write equations for state error matrix
+if exist('SQ','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SQ = zeros(%d,1);\n',numel(SQ));
+    for rowIndex = 1:numel(SQ)
+        string = char(SQ(rowIndex,1));
+        fprintf(fid,'SQ(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    % fprintf(fid,'\n');
+    % fprintf(fid,'Q = zeros(%d,%d);\n',nStates,nStates);
+    % for rowIndex = 1:nStates
+    %     for colIndex = 1:nStates
+    %         string = char(Q(rowIndex,colIndex));
+    %         % don't write out a zero-assignment
+    %         if ~strcmpi(string,'0')
+    %             fprintf(fid,'Q(%d,%d) = %s;\n',rowIndex,colIndex,string);
+    %         end
+    %     end
+    % end
+    % fprintf(fid,'\n');
+    
+end
+%% Write equations for covariance prediction
+% Only write out upper diagonal (matrix is symmetric)
+if exist('SPP','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SPP = zeros(%d,1);\n',numel(SPP));
+    for rowIndex = 1:numel(SPP)
+        string = char(SPP(rowIndex,1));
+        fprintf(fid,'SPP(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+end
+
+if exist('PP','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'nextP = zeros(%d,%d);\n',nStates,nStates);
+    for colIndex = 1:nStates
+        for rowIndex = 1:colIndex
+            string = char(PP(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'nextP(%d,%d) = %s;\n',rowIndex,colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+
+%% Write equations for velocity and position data fusion
+if exist('H_VP','var')
+    
+    [nRow,nCol] = size(H_VP);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_VP = zeros(%d,%d);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_VP(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_VP(%d,%d) = %s;\n',rowIndex,colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(SK_VP);
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_VP = zeros(%d,%d);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(SK_VP(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'SK_VP(%d,%d) = %s;\n',rowIndex,colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_VP);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,%d);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(K_VP(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'Kfusion(%d,%d) = %s;\n',rowIndex,colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+%% Write equations for true airspeed data fusion
+if exist('SH_TAS','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_TAS = zeros(%d,1);\n',numel(SH_TAS));
+    for rowIndex = 1:numel(SH_TAS)
+        string = char(SH_TAS(rowIndex,1));
+        fprintf(fid,'SH_TAS(%d) = %s;\n',rowIndex,string);
+    end
+    
+    [nRow,nCol] = size(H_TAS);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_TAS = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_TAS(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_TAS(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_TAS = zeros(%d,1);\n',numel(SK_TAS));
+    for rowIndex = 1:numel(SK_TAS)
+        string = char(SK_TAS(rowIndex,1));
+        fprintf(fid,'SK_TAS(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_TAS);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_TAS(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+%% Write equations for sideslip data fusion
+if exist('SH_BETA','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_BETA = zeros(%d,1);\n',numel(SH_BETA));
+    for rowIndex = 1:numel(SH_BETA)
+        string = char(SH_BETA(rowIndex,1));
+        fprintf(fid,'SH_BETA(%d) = %s;\n',rowIndex,string);
+    end
+    
+    [nRow,nCol] = size(H_BETA);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_BETA = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_BETA(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_BETA(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_BETA = zeros(%d,1);\n',numel(SK_BETA));
+    for rowIndex = 1:numel(SK_BETA)
+        string = char(SK_BETA(rowIndex,1));
+        fprintf(fid,'SK_BETA(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_BETA);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_BETA(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+%% Write equations for magnetometer data fusion
+if exist('SH_MAG','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_MAG = zeros(%d,1);\n',numel(SH_MAG));
+    for rowIndex = 1:numel(SH_MAG)
+        string = char(SH_MAG(rowIndex,1));
+        fprintf(fid,'SH_MAG(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(H_MAG);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
+    for colIndex = 1:nCol
+        string = char(H_MAG(1,colIndex));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_MX = zeros(%d,1);\n',numel(SK_MX));
+    for rowIndex = 1:numel(SK_MX)
+        string = char(SK_MX(rowIndex,1));
+        fprintf(fid,'SK_MX(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_MX);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_MX(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(H_MAG);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
+    for colIndex = 1:nCol
+        string = char(H_MAG(2,colIndex));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_MY = zeros(%d,1);\n',numel(SK_MY));
+    for rowIndex = 1:numel(SK_MY)
+        string = char(SK_MY(rowIndex,1));
+        fprintf(fid,'SK_MY(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_MY);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_MY(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(H_MAG);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
+    for colIndex = 1:nCol
+        string = char(H_MAG(3,colIndex));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_MZ = zeros(%d,1);\n',numel(SK_MZ));
+    for rowIndex = 1:numel(SK_MZ)
+        string = char(SK_MZ(rowIndex,1));
+        fprintf(fid,'SK_MZ(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_MZ);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_MZ(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+%% Write equations for optical flow sensor angular LOS data fusion
+if exist('SH_LOS','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_LOS = zeros(%d,1);\n',numel(SH_LOS));
+    for rowIndex = 1:numel(SH_LOS)
+        string = char(SH_LOS(rowIndex,1));
+        fprintf(fid,'SH_LOS(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    [nRow,nCol] = size(H_LOS);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol);
+    for colIndex = 1:nCol
+        string = char(H_LOS(1,colIndex));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    [nRow,nCol] = size(H_LOS);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol);
+    for colIndex = 1:nCol
+        string = char(H_LOS(2,colIndex));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string);
+        end
+    end
+    
+%     fprintf(fid,'\n');
+%     fprintf(fid,'SKK_LOS = zeros(%d,1);\n',numel(SKK_LOS));
+%     for rowIndex = 1:numel(SKK_LOS)
+%         string = char(SKK_LOS(rowIndex,1));
+%         fprintf(fid,'SKK_LOS(%d) = %s;\n',rowIndex,string);
+%     end
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_LOS = zeros(%d,1);\n',numel(SK_LOS));
+    for rowIndex = 1:numel(SK_LOS)
+        string = char(SK_LOS(rowIndex,1));
+        fprintf(fid,'SK_LOS(%d) = %s;\n',rowIndex,string);
+    end
+    
+    [nRow,nCol] = size(K_LOSX);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_LOS(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_LOSY);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_LOS(rowIndex,2));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    
+end
+%% Write observation fusion equations for optical flow sensor scale factor error estimation
+if exist('SH_OPT','var')
+    
+    fprintf(fid,'\n');
+    for rowIndex = 1:numel(SH_OPT)
+        string = char(SH_OPT(rowIndex,1));
+        fprintf(fid,'SH_OPT(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+        string = char(H_OPT(1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_OPT(1) = %s;\n',1,string);
+        end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+        string = char(H_OPT(2));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_OPT(2) = %s;\n',1,string);
+        end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    for rowIndex = 1:numel(SK_OPT)
+        string = char(SK_OPT(rowIndex,1));
+        fprintf(fid,'SK_OPT(%d) = %s;\n',rowIndex,string);
+    end
+    
+    fprintf(fid,'\n');
+        string = char(K_OPT(1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'K_OPT(1) = %s;\n',1,string);
+        end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+        string = char(K_OPT(2));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'K_OPT(2) = %s;\n',1,string);
+        end
+    fprintf(fid,'\n');
+    
+end
+%% Write equations for laser range finder data fusion
+if exist('SH_RNG','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_RNG = zeros(%d,1);\n',numel(SH_RNG));
+    for rowIndex = 1:numel(SH_RNG)
+        string = char(SH_RNG(rowIndex,1));
+        fprintf(fid,'SH_RNG(%d) = %s;\n',rowIndex,string);
+    end
+    
+    [nRow,nCol] = size(H_RNG);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_RNG = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_RNG(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_RNG(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_RNG = zeros(%d,1);\n',numel(SK_RNG));
+    for rowIndex = 1:numel(SK_RNG)
+        string = char(SK_RNG(rowIndex,1));
+        fprintf(fid,'SK_RNG(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_RNG);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_RNG(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+
+%% Write equations for simple magnetomter data fusion
+if exist('SH_MAGS','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_MAGS = zeros(%d,1);\n',numel(SH_MAGS));
+    for rowIndex = 1:numel(SH_MAGS)
+        string = char(SH_MAGS(rowIndex,1));
+        fprintf(fid,'SH_MAGS(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(H_MAGS);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_MAGS = zeros(1,%d);\n',nCol);
+    for colIndex = 1:nCol
+        string = char(H_MAGS(1,colIndex));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'H_MAGS(%d) = %s;\n',colIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+        
+end
+
+%% Write equations for X accel fusion
+if exist('SH_ACCX','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_ACCX = zeros(%d,1);\n',numel(SH_ACCX));
+    for rowIndex = 1:numel(SH_ACCX)
+        string = char(SH_ACCX(rowIndex,1));
+        fprintf(fid,'SH_ACCX(%d) = %s;\n',rowIndex,string);
+    end
+    
+    [nRow,nCol] = size(H_ACCX);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_ACCX = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_ACCX(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_ACCX(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_ACCX = zeros(%d,1);\n',numel(SK_ACCX));
+    for rowIndex = 1:numel(SK_ACCX)
+        string = char(SK_ACCX(rowIndex,1));
+        fprintf(fid,'SK_ACCX(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_ACCX);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_ACCX(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+
+%% Write equations for Y accel fusion
+if exist('SH_ACCY','var')
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SH_ACCY = zeros(%d,1);\n',numel(SH_ACCY));
+    for rowIndex = 1:numel(SH_ACCY)
+        string = char(SH_ACCY(rowIndex,1));
+        fprintf(fid,'SH_ACCY(%d) = %s;\n',rowIndex,string);
+    end
+    
+    [nRow,nCol] = size(H_ACCY);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_ACCY = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_ACCY(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_ACCY(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+    
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_ACCY = zeros(%d,1);\n',numel(SK_ACCY));
+    for rowIndex = 1:numel(SK_ACCY)
+        string = char(SK_ACCY(rowIndex,1));
+        fprintf(fid,'SK_ACCY(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+    
+    [nRow,nCol] = size(K_ACCY);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_ACCY(rowIndex,1));
+        % don't write out a zero-assignment
+        if ~strcmpi(string,'0')
+            fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
+        end
+    end
+    fprintf(fid,'\n');
+    
+end
+
+%% Close output file
+fclose(fid);
+
+end