%% ECE 602 Final Project 
% Paper: Minimum-Landing-Error Powered-Descent Guidance for Mars Landing 
% Using Convex Optimization
%%
% Objective: Reproduce experimental results
%%
% Authors: Adam Gomes, Stanislav Bochkarev, and Ryan MacDonald
%%
% Date: March 30, 2016


%% Problem Formulation
% The main purpose of the paper "Minimum-Landing-Error Powered-Descent Guidance
% for Mars Landing Using Convex Optimization" is to generate the
% minimum-landing-error trajectory, which is the trajectory that minimizes
% the distance to prescribed target while using the available fuel
% optimally. By posing the problem as a convex optimization problem, the
% authors claim the global optimal solution of the problem can be solved
% onboard with known bounds on convergence time.

%% Proposed Solution
% By posing the Minimum-Landing-Error problem as a discrete time, relaxed
% convex optimization problem, a globally minimal solution can be found.
% The discretized relaxed minimum-landing-error guidance problem can be
% written as a second-order-cone program in the following way:
%%
% $$ \min_{N,\eta} \|E\mathbf{y}_N\|^2 $$
%%
% subject to
%%
% $$ \|E_u\Upsilon_k\eta\|\leq \mathbf{e}_4^T\Upsilon_k\eta $$
% $$ \ \ \ \ \ k=0,\ldots ,N $$
%%
% $$ \rho_1e^{-z_0(t_k)}[1-(F\mathbf{y}_k-z_0(t_k))+\frac{(F\mathbf{y}_k-z_0(t_k))^2}{2}]\leq
% \mathbf{e}_4^T\Upsilon_k\eta \leq
% \rho_2e^{-z_0(t_k)}[1-(F\mathbf{y}_k-z_0(t_k))] $$
%%
% $$ E\mathbf{y}_k \in \mathbf{X} \ \ \ \ \ k=1,\ldots, N $$
%%
% $$ F\mathbf{y}_N\geq \ln m_{dry} $$
%%
% $$ \mathbf{y}_N^T\mathbf{e}_1=0, \ \ \ \ \ E_v\mathbf{y}_N^T=\mathbf{0}
% $$
%%
% $$ \mathbf{y}_k=\Phi_k  [\mathbf{r}_0 \  \dot{\mathbf{r}}_0 \  \ln m_{wet}]^T + \Lambda_k [\mathbf{g} \ 0]^T +\Psi_k\eta \ \ \ \ \ k=1,\ldots,N$$
%%
% The definitions of the symbols from the above minimization problem can be found on the first
% page of the paper.
%% Data sources
% The authors specified the problem parameters in the paper. We
% used identical parameters in our simulations. These parameters are defined here.
%%
% $$ \mathbf{g}=[-3.7114m/s^2 \ 0 \ 0]^T $$ :gravitational vector 
%%
% $$ m_{dry}=1505kg $$ :dry mass 
%%
% $$ m_{wet}=1905kg $$ :wet mass 
%%
% $$ \alpha=4.53x10^{-4} s/m $$ :fuel conversion coefficient 
%%
% $$ \rho_1=4972N $$ :lower bound for thrust 
%%
% $$ \rho_2=13260N $$ :upper bound for thrust 
%%
% $$ \mathbf{r}_0=[1500m \ 500m \ 2000m]^T $$ :initial position of the lander 
%%
% $$ \dot{\mathbf{r}}_0^{(1)} = [-75m/s \ 0 \ 100m/s]^T $$ :initial velocity
% profile for lander in Case 1 
%%
% $$ \dot{\mathbf{r}}_0^{(2)} = [-75m/s \ 40m/s \ 100m/s]^T $$ :initial
% velocity profile for lander in Case 2 
%%
%
% Additionally, a number of matrices were defined to form the appropriate structure
% in the problem formulation. These matrices are defined here.
%%
% $$ E=[I_{3x3} \  0_{3x4}] $$
%%
% $$ F=[0_{1x6} \  1] $$
%%
% $$ E_u=[I_{3x3} \  0_{3x1}] $$
%%
% $$ E_v=[0_{3x3} \  I_{3x3} \ 0_{3x1}] $$


%% System parameters for Mars Landing Problem 
%constant acceleration due to gravity
gravity = [-3.1444 0 0]';
%dry mass of lander
mass_dry = 1505; %kg
%wet mass of lander
mass_wet = 1905; %kg
%constant of proportionality between thrust magnitude and mass consumption
alph = 4.53e-4; %s/m
%lower bound on thrust magnitude
rho_1 = 4972; %N
%upper bound on thrust magnitude
rho_2 = 13260; %N
%Convex Linear Operators
E = [eye(3) zeros(3,4)];
F = [0 0 0 0 0 0 1];
E_u = [eye(3) zeros(3,1)];
E_v = [zeros(3,3) eye(3) zeros(3,1)];
%vector of zeros with unity at index i
e_1 = [1 0 0]';
e_2 = [0 1 0]';
e_3 = [0 0 1]';
%glide-slope constraint angle
gamma = pi*(4/180); %rad
%matrix component of cone constraint 
S = [e_2';e_3'];
%vector component of cone constraint
c = e_1/tan(gamma);
%number of discrete time steps
N = 55;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Case 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Case 1: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 
%
%
%
%initial position of lander
position_0 = [1500 500 2000]'; %m
%initial velocity of lander
velocity_0 = [-75 0 100]'; %m/s
%optimal final time
tf_opt = 78.4; %s
%time increment
time_step = tf_opt/N;

%Define the State transition matrices (Reference [3] of paper)
A_c = [zeros(3,3) eye(3) zeros(3,1);zeros(4,7)];
B_c = [[zeros(3,3);eye(3);0 0 0] [0 0 0 0 0 0 -alph]'];

A = expm(A_c*time_step); %continuous time A matrix

B = A*(eye(7)*time_step - A_c*time_step^2/2)*B_c; %continuous time B matrix

Lambda_k = zeros(N*7,4);
Lambda_k(1:7,1:4) = B;
for k = 2:N
    %Next time step of gravities effect is dependent on the state 
    %transition matrix and the previous time step 
    Lambda_k((k-1)*7+1:k*7,:) = A*Lambda_k((k-2)*7+1:(k-1)*7,:) + B;
end

Psi_k = zeros(N*7,4*(N+1));
for k = 2:(N)
    %Next time step of gravities effect is dependent on the state
    %transition matrix and the previous time step 
    Psi_k((k-1)*7+1:k*7,:) = A*Psi_k((k-2)*7+1:(k-1)*7,:);
    Psi_k((k-1)*7+1:k*7,((k*4-7):(4*k-4))) = B;
end

% Mass after the change of variables
z0 = log(mass_wet-alph*rho_2*time_step*(0:N)');

% Initial state vector
y0 = [position_0; velocity_0; log(mass_wet)];

s(1:N,7) = 0;
for i = 1:N
    s(i,:) = (7*i-6):(7*i);
end

cvx_begin
    variable eta((N+1)*4)
    variable y(N*7)
 
    % Objective function
    minimize(norm(y(end-6:end-4),2))
 
    subject to
 
    % Convexified thrust constraint
    for k = 0:N
        norm(E_u*eta(4*k+1:4*k+4), 2) <= eta(4*k+4);
    end
 
    % Thrust constraint 1
    eta(4) <= rho_2*exp(-z0(1)).*(1-(F*y0-z0(1)));
    rho_1*exp(-z0(1))*(1-(F*y0-z0(1))+0.5*(F*y0-z0(1)).^2) <= eta(4);
 
    for k = 1:N
        % Cone constraints
        norm(S*E*(y(s(k, :))-y(s(N, :))), 2)-c'*(E*(y(s(k, :)))) <= 0;

        % Thrust constraints
        eta(4*(k)+4) <= rho_2*exp(-z0(k+1)).*(1-(F*y(s(k, :))-z0(k+1)));

        rho_1*exp(-z0(k+1))*(1-(F*y(s(k, :))-z0(k+1))+...
            0.5*(F*y(s(k, :))-z0(k+1)).^2) <= eta(4*k+4);

        % System dynamics constraint
        y(s(k, :)) == A^k*y0+Lambda_k(s(k, :), :)*[gravity; 0]+...
                        Psi_k(s(k, :), :)*eta;
     end

    % Fuel mass constraint
    y(end) >= log(mass_dry);

    % Final height is 0 constraint
    y(end-6) == 0;

    % Final velocity constraint
    for i = 1:3
        y(end-i) == 0;
    end
cvx_end

% Converting output into manageable format
dist(1:3, N+1) = 0;
dist(1:3, 1) = position_0;
mass(1) = mass_wet;
for i = 1:N
    dist(1:3, i+1) = y((7*i-6):(7*i-4));
    mass(i+1) = y(7*i);
end

%Graphing
close all

conenum = [0:100:1500];
radii = conenum./tan(gamma);
[Z, Y, X] = cylinder(radii);
m = surf(Z, Y, 1500*X);

alpha(0.4)
colormap(gray(256));
set(m, 'edgecolor', 'none');

hold on
plot3(dist(3, :), dist(2, :), dist(1, :), '.');
xlabel('z (meters)')
ylabel('y (meters)')
zlabel('x (meters)')
title('Case 1: 3D Trajectory')
figure

plot(0:time_step:tf_opt,dist)
legend('x','y','z');
xlabel('Time (seconds)')
ylabel('Position (meters)')
title('Case 1: Trajectory vs. Time')
figure

plot(0:time_step:tf_opt,exp(mass))
xlabel('Time (seconds)')
ylabel('Mass (kg)')
title('Case 1: Mass vs. Time')
%% Case 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Case 2: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 
%
%
%
%initial position of lander
position_0 = [1500 500 2000]'; %m
%initial velocity of lander
velocity_0 = [-75 40 100]'; %m/s
%optimal final time
tf_opt = 77.7; %s
%time increment
time_step = tf_opt/N;

%Define the State transition matrices (Reference [3] of paper)
A_c = [zeros(3,3) eye(3) zeros(3,1);zeros(4,7)];
B_c = [[zeros(3,3);eye(3);0 0 0] [0 0 0 0 0 0 -alph]'];

A = expm(A_c*time_step); %continuous time A matrix

B = A*(eye(7)*time_step - A_c*time_step^2/2)*B_c; %continuous time B matrix

Lambda_k = zeros(N*7,4);
Lambda_k(1:7,1:4) = B;
for k = 2:N
    %Next time step of gravities effect is dependent on the state 
    %transition matrix and the previous time step 
    Lambda_k((k-1)*7+1:k*7,:) = A*Lambda_k((k-2)*7+1:(k-1)*7,:) + B;
end

Psi_k = zeros(N*7,4*(N+1));
for k = 2:(N)
    %Next time step of gravities effect is dependent on the state
    %transition matrix and the previous time step 
    Psi_k((k-1)*7+1:k*7,:) = A*Psi_k((k-2)*7+1:(k-1)*7,:);
    Psi_k((k-1)*7+1:k*7,((k*4-7):(4*k-4))) = B;
end

% State vector after change in variables
z0 = log(mass_wet-alph*rho_2*time_step*(0:N)');

% Initial state vector
y0 = [position_0; velocity_0; log(mass_wet)];

s(1:N,7) = 0;
for i = 1:N
    s(i,:) = (7*i-6):(7*i);
end

cvx_begin
    variable eta((N+1)*4)
    variable y(N*7)
 
    % Objective function
    minimize(norm(y(end-6:end-4),2))
 
    subject to
 
    % Convexified thrust constraint
    for k = 0:N
        norm(E_u*eta(4*k+1:4*k+4), 2) <= eta(4*k+4);
    end
 
    % Thrust constraint 1
    eta(4) <= rho_2*exp(-z0(1)).*(1-(F*y0-z0(1)));
    rho_1*exp(-z0(1))*(1-(F*y0-z0(1))+0.5*(F*y0-z0(1)).^2) <= eta(4);
 
    for k = 1:N
        % Cone constraints
        norm(S*E*(y(s(k, :))-y(s(N, :))), 2)-c'*(E*(y(s(k, :)))) <= 0;

        % Thrust constraints
        eta(4*(k)+4) <= rho_2*exp(-z0(k+1)).*(1-(F*y(s(k, :))-z0(k+1)));

        rho_1*exp(-z0(k+1))*(1-(F*y(s(k, :))-z0(k+1))+...
            0.5*(F*y(s(k, :))-z0(k+1)).^2) <= eta(4*k+4);

        % System dynamics constraint
        y(s(k, :)) == A^k*y0+Lambda_k(s(k, :), :)*[gravity; 0]+...
                        Psi_k(s(k, :), :)*eta;
     end

    % Fuel mass constraint
    y(end) >= log(mass_dry);

    % Final height is 0 constraint
    y(end-6) == 0;

    % Final velocity constraint
    for i = 1:3
        y(end-i) == 0;
    end
cvx_end

% Converting output into manageable format
dist(1:3, N+1) = 0;
dist(1:3, 1) = position_0;
mass(1) = mass_wet;
for i = 1:N
    dist(1:3, i+1) = y((7*i-6):(7*i-4));
    mass(i+1) = y(7*i);
end

% Graphing
conenum = [0:100:1500];
radii = conenum./tan(gamma);
[Z, Y, X] = cylinder(radii);
m = surf(Z, Y, 1500*X);

alpha(0.4)
colormap(gray(256));
set(m, 'edgecolor', 'none');

hold on
plot3(dist(3, :), dist(2, :), dist(1, :), '.');
xlabel('z (meters)')
ylabel('y (meters)')
zlabel('x (meters)')
title('Case 2: 3D Trajectory')
figure

plot(0:time_step:tf_opt,dist)
legend('x','y','z');
xlabel('Time (seconds)')
ylabel('Position (meters)')
title('Case 2: Trajectory vs. Time')
figure

plot(0:time_step:tf_opt,exp(mass))
xlabel('Time (seconds)')
ylabel('Mass (kg)')
title('Case 2: Mass vs. Time')
%% Conclusion
% We have replicated the approach presented in the paper and performed
% simulations on the data set provided in the paper. 
%%
% In case 1, our simulation matches the results in the paper.
% Case 2 presents noticeable differences in the results. From our
% simulations, the lander is able reach the landing site; whereas the result
% in the paper indicate that the lander does not have enough fuel to do
% that. It is difficult to pin point the reason for this discrepancy.
% One possible explanation relies on the state space equations used to
% describe the dynamics of the lander. It should be noted that there was
% a significant typo in the paper regarding the derivation of a matrix
% Psi. We have fixed this derivation by deriving in the manner that is
% correct to be the best of our knowledge. It is difficult to tell if
% author's intentions were the same.
