From 233c4b49c546b65eec8d0fbc1394d906f4b09d9a Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 20 Jun 2017 16:01:30 +1000
Subject: [PATCH] matlab: Add derivation of observation Jacobians used by the
 terrain estimator

---
 .../GenerateEquationsTerrainEstimator.m       | 91 +++++++++++++++++++
 matlab/scripts/Terrain Estimator/H_OPT.c      | 10 ++
 matlab/scripts/Terrain Estimator/H_RNG.c      |  1 +
 3 files changed, 102 insertions(+)
 create mode 100644 matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m
 create mode 100644 matlab/scripts/Terrain Estimator/H_OPT.c
 create mode 100644 matlab/scripts/Terrain Estimator/H_RNG.c

diff --git a/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m b/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m
new file mode 100644
index 0000000000..4e071ecf01
--- /dev/null
+++ b/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m	
@@ -0,0 +1,91 @@
+% IMPORTANT - This script requires the Matlab symbolic toolbox
+% Derivation of EKF equations for estimation of terrain height offset
+% Author:  Paul Riseborough
+% Last Modified: 20 June 2017
+
+% State vector:
+
+% terrain vertical position (ptd)
+
+% Observations:
+
+% line of sight (LOS) angular rate measurements (rel to sensor frame)
+% from a downwards looking optical flow sensor measured in rad/sec about
+% the X and Y sensor axes. These rates are motion compensated.
+
+% A positive LOS X rate is a RH rotation of the image about the X sensor
+% axis, and is produced by either a positive ground relative velocity in 
+% the direction of the Y axis.
+
+% A positive LOS Y rate is a RH rotation of the image about the Y sensor
+% axis, and is produced by either a negative ground relative velocity in 
+% the direction of the X axis.
+
+% Range measurement aligned with the Z body axis (flat earth model assumed)
+
+% Time varying parameters:
+
+% quaternion parameters defining the rotation from navigation to body axes
+% NED flight vehicle velocities
+% vehicle vertical position
+
+clear all;
+
+%% define symbolic variables and constants
+syms vel_x vel_y vel_z real % NED velocity : m/sec
+syms R_OPT real % variance of LOS angular rate mesurements : (rad/sec)^2
+syms R_RNG real % variance of range finder measurement : m^2
+syms stateNoiseVar real % state process noise variance
+syms pd real % position of vehicle in down axis : (m)
+syms ptd real % position of terrain in down axis : (m)
+syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
+syms Popt real % state variance
+nStates = 1;
+
+
+%% derive Jacobians for fusion of optical flow measurements
+syms vel_x vel_y vel_z real % NED velocity : m/sec
+syms pd real % position of vehicle in down axis : (m)
+syms ptd real % position of terrain in down axis : (m)
+syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
+
+% derive the body to nav direction cosine matrix
+Tbn = Quat2Tbn([q0,q1,q2,q3]);
+
+% calculate relative velocity in sensor frame
+relVelSensor = transpose(Tbn)*[vel_x;vel_y;vel_z];
+
+% calculate range to centre of flow sensor fov assuming flat earth
+range = ((ptd - pd)/Tbn(3,3));
+
+% divide velocity by range to get predicted motion compensated flow rates
+optRateX =  relVelSensor(2)/range;
+optRateY = -relVelSensor(1)/range;
+
+% calculate the observation jacobians
+H_OPTX = jacobian(optRateX,ptd);
+H_OPTY = jacobian(optRateY,ptd);
+H_OPT = [H_OPTX;H_OPTY];
+ccode(H_OPT,'file','H_OPT.c');
+fix_c_code('H_OPT.c');
+
+clear all;
+reset(symengine)
+
+%% derive Jacobian for fusion of range finder measurements
+syms vel_x vel_y vel_z real % NED velocity : m/sec
+syms R_RNG real % variance of range finder measurement : m^2
+syms pd real % position of vehicle in down axis : (m)
+syms ptd real % position of terrain in down axis : (m)
+syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
+
+% derive the body to nav direction cosine matrix
+Tbn = Quat2Tbn([q0,q1,q2,q3]);
+
+% calculate range assuming flat earth
+range = ((ptd - pd)/Tbn(3,3));
+
+% calculate range observation Jacobian
+H_RNG = jacobian(range,ptd); % measurement Jacobian
+ccode(H_RNG,'file','H_RNG.c');
+fix_c_code('H_RNG.c');
diff --git a/matlab/scripts/Terrain Estimator/H_OPT.c b/matlab/scripts/Terrain Estimator/H_OPT.c
new file mode 100644
index 0000000000..c16eaf2010
--- /dev/null
+++ b/matlab/scripts/Terrain Estimator/H_OPT.c	
@@ -0,0 +1,10 @@
+t2 = q0*q0;
+t3 = q1*q1;
+t4 = q2*q2;
+t5 = q3*q3;
+t6 = pd-ptd;
+t7 = 1.0/(t6*t6);
+t8 = q0*q3*2.0;
+t9 = t2-t3-t4+t5;
+A0[0][0] = -t7*t9*(vel_z*(q0*q1*2.0+q2*q3*2.0)+vel_y*(t2-t3+t4-t5)-vel_x*(t8-q1*q2*2.0));
+A0[1][0] = t7*t9*(-vel_z*(q0*q2*2.0-q1*q3*2.0)+vel_x*(t2+t3-t4-t5)+vel_y*(t8+q1*q2*2.0));
diff --git a/matlab/scripts/Terrain Estimator/H_RNG.c b/matlab/scripts/Terrain Estimator/H_RNG.c
new file mode 100644
index 0000000000..9a195c0901
--- /dev/null
+++ b/matlab/scripts/Terrain Estimator/H_RNG.c	
@@ -0,0 +1 @@
+A0[0][0] = 1.0/(q0*q0-q1*q1-q2*q2+q3*q3);