Add derivation for 3-state terrain height estimator

derivations/TerrainEstimators/C_code3.txt

@@ -0,0 +1,33 @@
+float F[3][3];
+F[0][0] = 1;
+F[1][0] = dt;
+F[1][1] = 1;
+F[2][2] = 1;
+float SPP[1][1];
+SPP[0] = P[1][0] + P[0][0]*dt;
+float nextP[3][3];
+nextP[0][0] = P[0][0] + R_AD*sq(dt);
+nextP[0][1] = P[0][1] + P[0][0]*dt;
+nextP[1][1] = P[1][1] + P[0][1]*dt + dt*SPP[0];
+nextP[0][2] = P[0][2];
+nextP[1][2] = P[1][2] + P[0][2]*dt;
+nextP[2][2] = P[2][2] + R_TPD;
+float H_HAGL[1][3];
+H_HAGL[0][1] = -1;
+H_HAGL[0][2] = 1;
+float SK_HAGL[1][1];
+SK_HAGL[0] = 1/(P[1][1] - P[1][2] - P[2][1] + P[2][2] + R_HAGL);
+float Kfusion[3][1];
+float Kfusion[1][1];
+Kfusion[0] = -SK_HAGL[0]*(P[0][1] - P[0][2]);
+Kfusion[1] = -SK_HAGL[0]*(P[1][1] - P[1][2]);
+Kfusion[2] = -SK_HAGL[0]*(P[2][1] - P[2][2]);
+float H_PD[1][3];
+H_PD[0][1] = 1;
+float SK_PD[1][1];
+SK_PD[0] = 1/(P[1][1] + R_PD);
+float Kfusion[3][1];
+float Kfusion[1][1];
+Kfusion[0] = P[0][1]*SK_PD[0];
+Kfusion[1] = P[1][1]*SK_PD[0];
+Kfusion[2] = P[2][1]*SK_PD[0];

derivations/TerrainEstimators/GenerateEquationsTerrainEstimator3.m

@@ -0,0 +1,115 @@
+% IMPORTANT - This script requires the Matlab symbolic toolbox
+% Derivation of EKF equations for estimation of terrain height offset
+% Author:  Paul Riseborough
+
+% State vector:
+
+% vehicle vertical velocity in local NED frame
+% vehicle vertical position in loal NED frame
+% terrain vertical position in local NED frame
+
+
+% Observations:
+
+% vehicle vertical position estimate from the nav system relative to local NED frame
+% Range measurement aligned with the Z body axis assuming a flat earth model
+
+% Control inputs parameters:
+
+% vertical rate of change of velocity in local NED frame
+
+clear all;
+
+%% define symbolic variables and constants
+% states
+syms VD real % down velocity : m/sec
+syms PD real % down position : m
+syms TPD real % down position of terrain : m
+% state noise
+syms R_TPD real % process noise variance of terrain position ; m^2
+% observations
+syms meaHAGL real; % height above ground measurement : m
+syms meaPD real; % vertical position measurement : m
+syms R_HAGL real % variance of range finder measurement : m^2
+syms R_PD real; % variance of vertical position measurement : m^2
+% control inputs
+syms AD real % downwards rate of change of velocity : m/sec^2
+syms R_AD real % noise variance of AD : (m/sec^2)^2
+% parameters
+syms dt real % time step : sec
+nStates = 3;
+
+%% define the process equations
+
+% define the state vector
+stateVector = [VD;PD;TPD];
+
+% define the process equations
+newVD = VD + dt*AD;
+newPD = PD + dt*VD;
+newTPD = TPD;
+processEqns = [newVD;newPD;newTPD];
+
+%% derive the state transition matrix
+
+% derive the state transition matrix
+F = jacobian(processEqns, stateVector);
+
+%% 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 vehicle
+% vertical velocity and position estimates are assumed to be driven by 
+% 'noise' in the vertical acceleration.
+distVector = [AD];
+
+% derive the control(disturbance) influence matrix
+G = jacobian(processEqns, distVector);
+
+% derive the state error matrix for control inputs
+imuNoise = diag([R_AD]);
+Q = G*imuNoise*transpose(G);
+
+% define a symbolic covariance matrix using strings to represent 
+% '_lp_' to represent '( '
+% '_c_' to represent ,
+% '_rp_' 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 + diag([0;0;R_TPD]);
+
+% Collect common expressions to optimise processing
+[PP,SPP]=OptimiseAlgebra(PP,'SPP');
+
+%% derive equations for fusion of height above ground
+meaHAGL = TPD - PD;
+H_HAGL = jacobian(meaHAGL,stateVector); % measurement Jacobian
+
+% calculate the Kalman gain matrix and optimise algebra
+K_HAGL = (P*transpose(H_HAGL))/(H_HAGL*P*transpose(H_HAGL) + R_HAGL);
+[K_HAGL,SK_HAGL]=OptimiseAlgebra(K_HAGL,'SK_HAGL');
+
+%% derive equations for fusion of vehice position measurements
+meaPD = PD;
+H_PD = jacobian(meaPD,stateVector); % measurement Jacobian
+
+% calculate the Kalman gain matrix and optimise algebra
+K_PD = (P*transpose(H_PD))/(H_PD*P*transpose(H_PD) + R_PD);
+[K_PD,SK_PD]=OptimiseAlgebra(K_PD,'SK_PD');
+
+%% Save output
+fileName = strcat('SymbolicOutput3.mat');
+save(fileName);
+
+%% Write equations to text and convert to m and c code fragments
+SaveScriptCodeTerrainEstimator3;
+ConvertToM(3);
+ConvertToC(3);

derivations/TerrainEstimators/M_code3.txt

@@ -0,0 +1,33 @@
+F = zeros(3,3);
+F(1,1) = 1;
+F(2,1) = dt;
+F(2,2) = 1;
+F(3,3) = 1;
+SPP = zeros(1,1);
+SPP(1) = OP(2,1) + OP(1,1)*dt;
+nextP = zeros(3,3);
+nextP(1,1) = OP(1,1) + R_AD*dt^2;
+nextP(1,2) = OP(1,2) + OP(1,1)*dt;
+nextP(2,2) = OP(2,2) + OP(1,2)*dt + dt*SPP(1);
+nextP(1,3) = OP(1,3);
+nextP(2,3) = OP(2,3) + OP(1,3)*dt;
+nextP(3,3) = OP(3,3) + R_TPD;
+H_HAGL = zeros(1,3);
+H_HAGL(1,2) = -1;
+H_HAGL(1,3) = 1;
+SK_HAGL = zeros(1,1);
+SK_HAGL(1) = 1/(OP(2,2) - OP(2,3) - OP(3,2) + OP(3,3) + R_HAGL);
+Kfusion = zeros(3,1);
+Kfusion = zeros(1,1);
+Kfusion(1) = -SK_HAGL(1)*(OP(1,2) - OP(1,3));
+Kfusion(2) = -SK_HAGL(1)*(OP(2,2) - OP(2,3));
+Kfusion(3) = -SK_HAGL(1)*(OP(3,2) - OP(3,3));
+H_PD = zeros(1,3);
+H_PD(1,2) = 1;
+SK_PD = zeros(1,1);
+SK_PD(1) = 1/(OP(2,2) + R_PD);
+Kfusion = zeros(3,1);
+Kfusion = zeros(1,1);
+Kfusion(1) = OP(1,2)*SK_PD(1);
+Kfusion(2) = OP(2,2)*SK_PD(1);
+Kfusion(3) = OP(3,2)*SK_PD(1);

derivations/TerrainEstimators/SaveScriptCodeTerrainEstimator3.m

@@ -0,0 +1,140 @@
+function SaveScriptCodeTerrainEstimator3
+
+%% Load Data
+fileName = strcat('SymbolicOutput3.mat');
+load(fileName);
+
+%% Open output file
+fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
+fid = fopen(fileName,'wt');
+
+%% Write equation for state transition matrix
+if exist('F','var')
+
+    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 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 HAGL fusion
+if exist('SK_HAGL','var')
+
+    [nRow,nCol] = size(H_HAGL);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_HAGL = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_HAGL(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_HAGL(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_HAGL = zeros(%d,1);\n',numel(SK_HAGL));
+    for rowIndex = 1:numel(SK_HAGL)
+        string = char(SK_HAGL(rowIndex,1));
+        fprintf(fid,'SK_HAGL(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+
+    [nRow,nCol] = size(K_HAGL);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_HAGL(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 vertical position fusion
+if exist('SK_PD','var')
+
+    [nRow,nCol] = size(H_HAGL);
+    fprintf(fid,'\n');
+    fprintf(fid,'H_PD = zeros(1,%d);\n',nCol);
+    for rowIndex = 1:nRow
+        for colIndex = 1:nCol
+            string = char(H_PD(rowIndex,colIndex));
+            % don't write out a zero-assignment
+            if ~strcmpi(string,'0')
+                fprintf(fid,'H_PD(1,%d) = %s;\n',colIndex,string);
+            end
+        end
+    end
+    fprintf(fid,'\n');
+
+    fprintf(fid,'\n');
+    fprintf(fid,'SK_PD = zeros(%d,1);\n',numel(SK_PD));
+    for rowIndex = 1:numel(SK_PD)
+        string = char(SK_PD(rowIndex,1));
+        fprintf(fid,'SK_PD(%d) = %s;\n',rowIndex,string);
+    end
+    fprintf(fid,'\n');
+
+    [nRow,nCol] = size(K_PD);
+    fprintf(fid,'\n');
+    fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
+    for rowIndex = 1:nRow
+        string = char(K_PD(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

derivations/TerrainEstimators/SymbolicOutput3.txt

@@ -0,0 +1,51 @@
+
+F = zeros(3,3);
+F(1,1) = 1;
+F(2,1) = dt;
+F(2,2) = 1;
+F(3,3) = 1;
+
+
+SPP = zeros(1,1);
+SPP(1) = OP_l_2_c_1_r_ + OP_l_1_c_1_r_*dt;
+
+
+nextP = zeros(3,3);
+nextP(1,1) = OP_l_1_c_1_r_ + R_AD*dt^2;
+nextP(1,2) = OP_l_1_c_2_r_ + OP_l_1_c_1_r_*dt;
+nextP(2,2) = OP_l_2_c_2_r_ + OP_l_1_c_2_r_*dt + dt*SPP(1);
+nextP(1,3) = OP_l_1_c_3_r_;
+nextP(2,3) = OP_l_2_c_3_r_ + OP_l_1_c_3_r_*dt;
+nextP(3,3) = OP_l_3_c_3_r_ + R_TPD;
+
+
+H_HAGL = zeros(1,3);
+H_HAGL(1,2) = -1;
+H_HAGL(1,3) = 1;
+
+
+SK_HAGL = zeros(1,1);
+SK_HAGL(1) = 1/(OP_l_2_c_2_r_ - OP_l_2_c_3_r_ - OP_l_3_c_2_r_ + OP_l_3_c_3_r_ + R_HAGL);
+
+
+Kfusion = zeros(3,1);
+Kfusion = zeros(1,1);
+Kfusion(1) = -SK_HAGL(1)*(OP_l_1_c_2_r_ - OP_l_1_c_3_r_);
+Kfusion(2) = -SK_HAGL(1)*(OP_l_2_c_2_r_ - OP_l_2_c_3_r_);
+Kfusion(3) = -SK_HAGL(1)*(OP_l_3_c_2_r_ - OP_l_3_c_3_r_);
+
+
+H_PD = zeros(1,3);
+H_PD(1,2) = 1;
+
+
+SK_PD = zeros(1,1);
+SK_PD(1) = 1/(OP_l_2_c_2_r_ + R_PD);
+
+
+Kfusion = zeros(3,1);
+Kfusion = zeros(1,1);
+Kfusion(1) = OP_l_1_c_2_r_*SK_PD(1);
+Kfusion(2) = OP_l_2_c_2_r_*SK_PD(1);
+Kfusion(3) = OP_l_3_c_2_r_*SK_PD(1);
+