%% ECE 602 Final Project: By David Murdoch
% Paper: Minimum-Landing-Error Powered-Descent Guidance for Mars Landing Using Convex Optimization


%% Problem description
% The problem at hand is to create a trajectory for a space craft to land
% on Mars with minimal landing error. Mars rovers make their final descent
% to the surface of the planet using solely thrusters, and if they are
% blown off course or into hazardous areas, they could be damaged or made
% unable to perform their mission. Therefore, it is imperative that the
% landing error be minimised during the final thruster stage. The claim
% made by the authors of the paper is that convex optimization can be used
% to not only provide these minimum error trajectories, but that they can
% be provided in a guaraneteed number of iterations.

%% Proposed Solution
% The proposed solution is a relaxed, discretized, second order cone
% program, which allows the rover to find a globally optimal solution. The
% SOCP is shown below.
%%
% 
% <<problemstatement.PNG>>
% 
% Parameters in the SOCP are defined on the first page of the paper. The
% SOCP is also subject to the following cone constraint.

%%
% 
% <<glideslope.PNG>>
% 
% S and C are defined as follows.
%%
% 
% <<sAndC.PNG>>
% 
% This allows for the definition of a glide angle constraint for the rover
% as well.

%% Data Sources
% All parameters used for this project were provided in the paper.
% Constraints and physical values for the rover are provided as follows:
%%
% 
% <<params.PNG>>
% 
%Initial position and velocity values are also provided as part of the
%paper.

%% System Parameters
%Mass of the lander with no fuel(kg)
mDry = 1505;
%Mass of the lander with a full fuel tank (kg)
mWet = 1905;

%Lower bound on thrust (N)
p1 = 4972;
%Upper bound on thrust (N)
p2 = 13260;

% Coefficient of proportoniality between thrust magnitude and mass rate
% (s/m)
alph = 0.000453;
% Acceleration due to gravity(m/s^2)
g = [-3.7114 0 0]';

% Initial position (m)
r0 = [1500 500 2000]';

%Operators defined for the conic program
e = [eye(3) zeros(3,4)];
f = [0 0 0 0 0 0 1];
eU = [eye(3) zeros(3,1)];
eV = [zeros(3,3) eye(3) zeros(3,1)];

% Vectors of zero with unity at index i
e1 = [1 0 0]';
e2 = [0 1 0]';
e3 = [0 0 1]';

%Defining gamma as the glide slope constraint angle
gamma = pi*(4/180); %Radians

%Matrix component of cone constraint, as defined by the paper
s = [ e2'; e3'];

%Vector component of cone constraint, as defined by the paper
c = (e1/tan(gamma))*gamma;

%As per the paper, 55 time discretization units were used.
N = 55;
%Ac and Bc are given by the paper referenced as [3] in the assigned paper,
%a previous work on a similar topic, as are the equations to transform them
%into A and B
Ac = [ zeros(3,3) eye(3) zeros(3,1); zeros(4,7)];
% Ac = 0     0     0     1     0     0     0
%      0     0     0     0     1     0     0
%      0     0     0     0     0     1     0
%      0     0     0     0     0     0     0
%      0     0     0     0     0     0     0
%      0     0     0     0     0     0     0
%      0     0     0     0     0     0     0
Bc =[ [zeros(3,3); eye(3); 0 0 0] [zeros(6,1); -1*alph] ];
% 
% Bc =     0         0         0         0
%          0         0         0         0
%          0         0         0         0
%     1.0000         0         0         0
%          0    1.0000         0         0
%          0         0    1.0000         0
%          0         0         0    -alph

A = expm(Ac*dt); %Discrete time dynamics A matrix
B = A*(eye(7)*dt - Ac*dt^2/2)*Bc; % Discrete time dynamics B matrix


%Construct the Lambda matrix required for the constraints.
lambda = zeros(7,4,N);
lambda(:,:,1) = B;
for k=2:N
   lambda(:,:,k) = A*lambda(:,:,k-1) +B; 
end

%Note: [3] in our paper shows the value of psi, but there seems to be
%a large typo in it; the matrix doesn't make sense as written. The matrix
%has been rederived as best as the author was capable.
psi = zeros(7, 4*(N+1), N);
for k=2:N
   psi(:,:,k) = A*psi(:,:,k-1);
   psi(:,4*(k-2)+1:4*(k-2)+4,k) = B;
end

z0 = log(mWet-alph*p2*dt*(0:N));

%% Case 1
% This case involves no initial y velocity, and should result in a feasible
% solution being found.



tf = 78.4; % s
% Initial velocity for Case 1(m/s) 
v0 = [-75 0 100]';
% Time step length for discretization
dt = tf/N;

y0 = [r0; v0; log(mWet)];

cvx_begin
    %Eta must be a 4*N vector instead of a matrix, otherwise the dimensions
    %with the psi matrix don't work out
    variable eta(4*(N+1))
    variable y(7,N)

    %Objective function
    minimize(norm(y(1:3,N),2))

    subject to
    
    %Convexified thrust constraints
    for k=0:N
       norm(eU*(eta(4*k+1:4*k+4)), 2) <= eta(4*k+4);
    end
    
    for k=1:N
       %Cone constraints
       norm(s*e*(y(:,k)-y(:,k)), 2)- c'*((e*y(:,k))) <= 0; 
       
       %Thrust constraints
       eta(4*k+4) <= p2*exp(-z0(k))*(1-(f*y(:,k) - z0(k)));
       
       p1*exp(-z0(k))*(1-(f*y(:,k) - z0(k)) + ((f*y(:,k) - z0(k)).^2)/2) <= eta(4*k+4);
       
       
       %System dynamic constraint
       y(:,k) == A^k*y0+lambda(:,:,k)*[g;0] + psi(:,:,k)*eta;
    end
    %Fuel mass constraint: Can't use more than available fuel
    y(7,N) >= log(mDry);

    %Final height zero: have to reach the ground
    y(1,N) == 0;

    %Velocity constraints: The lander can no longer be moving
    y(4,N) == 0;
    y(5,N) == 0;
    y(6,N) == 0;

cvx_end

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


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

hold on
plot3(y(1,:), y(2,:), y(3,:))
xlabel('z(m)')
ylabel('y(m)')
zlabel('x(m)')
title('Case 1: 3D trajectory with glide error constraint shown')

figure
plot(0:dt:tf, [y0(1:3) y(1:3,:)])
legend('x','y','z');
xlabel('Time (seconds)')
ylabel('Position (meters)')
title('Case 1: Trajectory vs. Time')

figure
plot(0:dt:tf, [exp(y0(7)) exp(y(7,:))])
legend('x','y','z');
xlabel('Time (seconds)')
ylabel('Mass (kg)')
title('Case 1: Mass vs. Time')

%% Case 2
% This case has an initial y velocity that should make a feasible solution
% impossible.

tf = 77.7; % s
% Initial velocity for Case 1(m/s) 
v0 = [-75 40 100]';
% Time step length for discretization
dt = tf/N;

y0 = [r0; v0; log(mWet)];

cvx_begin
    %Eta must be a 4*N vector instead of a matrix, otherwise the dimensions
    %with the psi matrix don't work out
    variable eta(4*(N+1))
    variable y(7,N)

    %Objective function
    minimize(norm(y(1:3,N),2))

    subject to
    
    %Convexified thrust constraints
    for k=0:N
       norm(eU*(eta(4*k+1:4*k+4)), 2) <= eta(4*k+4);
    end
    
    for k=1:N
       %Cone constraints
       norm(s*e*(y(:,k)-y(:,k)), 2)- c'*((e*y(:,k))) <= 0; 
       
       %Thrust constraints
       eta(4*k+4) <= p2*exp(-z0(k))*(1-(f*y(:,k) - z0(k)));
       
       p1*exp(-z0(k))*(1-(f*y(:,k) - z0(k)) + ((f*y(:,k) - z0(k)).^2)/2) <= eta(4*k+4);
       
       
       %System dynamic constraint
       y(:,k) == A^k*y0+lambda(:,:,k)*[g;0] + psi(:,:,k)*eta;
    end
    %Fuel mass constraint: Can't use more than available fuel
    y(7,N) >= log(mDry);

    %Final height zero: have to reach the ground
    y(1,N) == 0;

    %Velocity constraints: The lander can no longer be moving
    y(4,N) == 0;
    y(5,N) == 0;
    y(6,N) == 0;

cvx_end

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


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

hold on
plot3(y(1,:), y(2,:), y(3,:))
xlabel('z(m)')
ylabel('y(m)')
zlabel('x(m)')
title('Case 2: 3D trajectory with glide error constraint shown')

figure
plot(0:dt:tf, [y0(1:3) y(1:3,:)])
legend('x','y','z');
xlabel('Time (seconds)')
ylabel('Position (meters)')
title('Case 2: Trajectory vs. Time')

figure
plot(0:dt:tf, [exp(y0(7)) exp(y(7,:))])
legend('x','y','z');
xlabel('Time (seconds)')
ylabel('Mass (kg)')
title('Case 2: Mass vs. Time')

%% Conclusion
% The algorithm was implemented as in the paper, and simulation was
% performed on it. For case 1, there was minimal error compared to the
% implementation in the paper. However, in case 2, there was a feasible
% solution found when one should not have been. It's unclear what the
% problem was, but it likely relates to the obvious typo in the psi matrix,
% mentioned in the definition of the psi matrix above. While I attempted to
% re-derive the matrix to the best of my ability, it is likely, given the
% results, that either that matrix or one of the others involved is not
% correct. The most likely culprit for a faulty feasible solution in this
% case is the system dynamics. A precipitous drop can also be seen in the
% initial stages of the mass/time curve, which indicates that something
% else is likely amiss as well, as the mass is solely set via the dynamics
% constraints.
