Reentry

% from Julier and Uhlmann's UKF article

function Reentry

b0 = -0.59783;
H0 = 13.406;
Gm0 = 3.9860e5;
R0 = 6374;

iter = 2000;
dt = 0.1;

Q = diag([2.4064e-5, 2.4064e-5, 1e-6]);
A = zeros(5,3); % noise term is given by w = A * randn(3,1);
A(3:5, 1:3) = sqrt(Q);

% true initial state
m0 = [6500.4 ; 349.14 ; -1.8093 ; -6.7967 ; 0.6932];
P0 = diag([1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 0]);
trueState = m0;  % FIXME: plus randomness coming from P0

% prior distribution assumed by filter
mu0 = [6500.4 ; 349.14 ; -1.8093 ; -6.7967 ; 0];
K0 = diag([1e-6, 1e-6, 1e-6, 1e-6, 1]);
%K0 = diag([1 1 1 1 1]);

% FIXME: This is cartesian.  Should try J-U polar coords.
H = [1 0 0 0 0; 0 1 0 0 0];


%%%%%

ekfSysModel = EKFContinuousSystemModel(@f, A, @J_f);
ukfSysModel = UnscentedContinuousSystemModel(@f, A);

% FIXME: UKF fails if there isn't noise in the observer model,
% because we know the exact position of some of the variables, so
% covariance has zero eigenvals, so cholesky factorization is singular.
%obsModel = LinearObserverModel(H, zeros(2));
obsModel = LinearObserverModel(H, [2 0; 0 2]);

%obsModel = UnscentedObserverModel(obsModel);

ekfFilt = BLUEFilter(ekfSysModel, obsModel);
ukfFilt = BLUEFilter(ukfSysModel, obsModel);

ekfPrediction = zeros(5,iter);
ekfPrediction(:,1) = mu0;
ukfPrediction = zeros(5,iter);
ukfPrediction(:,1) = mu0;

stateVec = zeros([5 iter]); % true state
stateVec(:,1) = trueState;

measVec = zeros([2 iter]); % measurements

% first simulate time evolution and measurements, so we can reuse the
% same data for EKF and UKF.
for i = 2:iter
    % simulate time evolution and measurement
    trueState = ukfSysModel.simulate(trueState, dt);
    observation = obsModel.simulate(trueState, dt);

    stateVec(:,i) = trueState;
    measVec(:,i) = observation;
end

%sysModelEKF.nSteps = 1;
%sysModelUKF.nSteps = 1;

% covariances for each step of filter
ekfCov = K0;
ukfCov = K0; 

for k = 2:iter
    % filter the simulated observation
    [ekfPrediction(:,k), ekfCov] = ekfFilt.filter(ekfPrediction(:,k-1), ...
                                                  ekfCov, measVec(:,k), ...
                                                  dt);
    [ukfPrediction(:,k), ukfCov] = ukfFilt.filter(ukfPrediction(:,k-1), ...
                                                  ukfCov, measVec(:,k), ...
                                                  dt);
end

times = dt * 1:iter;

figure
theta = -0.03:0.001:0.05;
arcx = R0 * cos(theta);
arcy = R0 * sin(theta);
plot(arcx, arcy, 'r-');
hold on;
plot(ekfPrediction(1,:), ekfPrediction(2,:), 'b-');
plot(ukfPrediction(1,:), ukfPrediction(2,:), 'r:');
hold off

figure
for k=1:5
    subplot(1,5,k)
    hold on;
    plot(times, ekfPrediction(k,:) - stateVec(k,:), 'b-');
    plot(times, ukfPrediction(k,:) - stateVec(k,:), 'r:');
    hold off;
    
    fprintf('RMS error in variable %d with EKF = %f\n', k, ...
            std(ekfPrediction(k,:) - stateVec(k,:)));
    fprintf('RMS error in variable %d with UKF = %f\n', k, ...
            std(ukfPrediction(k,:) - stateVec(k,:)));
    fprintf('\n');
end



function xdot = f(x,t)
% used in xdot = f(x) + w
    R = sqrt(x(1,:).^2 + x(2,:).^2);
    V = sqrt(x(3,:).^2 + x(4,:).^2);
    b = b0 * exp(x(5,:));
    D = b .* exp((R0 - R) / H0) .* V;
    G = -Gm0 ./ R.^3;
    
    xdot(1,:) = x(3,:);
    xdot(2,:) = x(4,:);
    xdot(3,:) = D .* x(3,:) + G .* x(1,:);
    xdot(4,:) = D .* x(4,:) + G .* x(2,:);
    xdot(5,:) = 0;
end

function J = J_f(x,t)
    R = sqrt(x(1,:).^2 + x(2,:).^2);
    V = sqrt(x(3,:).^2 + x(4,:).^2);
    b = b0 * exp(x(5,:));
    D = b .* exp((R0 - R) / H0) .* V;
    G = -Gm0 ./ R.^3;
    
    dG_dx =  3 * Gm0 * R^(-5) * x([1 2]);
    dD_dx([1 2]) = b * (-1/H0) *(1/R) * exp((R0 - R)/H0) * V * x([1 2]);
    dD_dx([3 4]) = b * exp((R0-R)/H0) * (1/V) * x([3 4]);
    dD_dx(5) = D;
    
    J = zeros(5,5);
    J(1,3) = 1;
    J(2,4) = 1;

    %J(3,:) = dD_dx * x(3) + [dG_dx(1) * x(1)+G, dG_dx(2) * x(1), D, 0, 0]
    
    J(3,1) = dD_dx(1) * x(3) + dG_dx(1) * x(1) + G;
    J(3,2) = dD_dx(2) * x(3) + dG_dx(2) * x(1);
    J(3,3) = dD_dx(3) * x(3) + D;
    J(3,4) = dD_dx(4) * x(3);
    J(3,5) = dD_dx(5) * x(3);
    
    J(4,1) = dD_dx(1) * x(4) + dG_dx(1) * x(2);
    J(4,2) = dD_dx(2) * x(4) + dG_dx(2) * x(2) + G;
    J(4,3) = dD_dx(3) * x(4);
    J(4,4) = dD_dx(4) * x(4) + D;
    J(4,5) = dD_dx(5) * x(4);
end


end