You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
91 lines
3.1 KiB
91 lines
3.1 KiB
% 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');
|
|
|