MATLAB code for solving PMP example

clearvars
close all

% Defining your example system
A = [-1 0.5; 0.3 -1];
B = [1; 1];
x0 = [1; 0];
xT = [0; 1];
T = 5;
rho = 100;

% Defining some intermediate constants
I = eye(2);
O = zeros(2);
Ah = [A -1/(2*rho)*(B*B'); -2*I -A'];
Bh = [O; 2*I];

% X = (expm(Ah * T) - I) * inv(Ah) * Bh can also be calculated with
% [expm(Ah * T) X; 0 I] = expm([Ah Bh; 0 0] * T)
% for more details see: https://en.wikipedia.org/wiki/Discretization
expAB = expm([Ah Bh; zeros(2, 6)] * T);
M11 = expAB(1:2,1:2);   % Upper left part of expm(Ah * T)
M12 = expAB(1:2,3:4);   % Upper right part of expm(Ah * T)
M21 = expAB(3:4,1:2);   % Lower left part of expm(Ah * T)
M22 = expAB(3:4,3:4);   % Lower right part of expm(Ah * T)
R1 = expAB(1:2,5:6);    % Upper part of X
R2 = expAB(3:4,5:6);    % Lower part of X

% Using equation (21) then lambda0 and lambdaT can be calculate with
% [lambda0; lambdaT] = [-M12 O; -M22 I] \ ([M11 R1-I; M21 R2] * [x0; xT])
% However we only need lambda0, which can be solved by only using the top
% half of (19), which can be simplifyfied down to:
% xT = M11 * x0 + M12 * lambda0 + R1 * xT
% Solving this for lambda0 gives:
lambda0 = M12 \ ((I - R1) * xT - M11 * x0);

% So all the initial conditions are known and pluging this into (18) should
% give use the trajectory as a function of time:
f = @(t) expm(Ah * t) * ([x0; lambda0] - Ah \ Bh * xT) + Ah \ Bh * xT;

%% Solution using f(t) obtained from (18)

N = 1e3;                % Number of samples used for the trajectory
t = linspace(0, T, N);  % Corresponding time vector
X = zeros(4, N);        % Allocate memory for the trajectory
U = zeros(1, N);        % Allocate memory for the input

% Calculate the state and input at each time instance
for k = 1 : N
    X(:,k) = f(t(k));
    U(k) = -(0.5 / rho) * B' * X(3:4,k);
end

% Plotting the results
figure
subplot(2,1,1)
plot(t, X(1:2,:))       % Since the full state consists out of [x; lambda]
legend('x_1', 'x_2', 'location', 'north')
subplot(2,1,2)
plot(t, U)
ylabel('u')
xlabel('t')

%% Time varying feedback control

% Calculate the top layer of expAB as a function of time so the optimal
% solution given the current state can be found:
Gam = @(t) [I O O] * expm([Ah Bh; zeros(2, 6)] * (T-t));
% Here Gam(t) = [M11(t) M12(t) R1(t)] using T-t instead of T, and since:
% lambda(t) = inv(M12(t)) * ((I - R1(t)) * xT - M11(t) * x(t))
% u(t) = -1 / (2 * rho) * B' * lambda(t)
% Combining these gives:
% u(t) = -1 / (2 * rho) * B' * inv(M12(t)) * ((I - R1(t)) * xT - M11(t) * x(t))
% I wanted to avoid creating too many temperary variables, so I used that
% M11(t) = Gam(t) * [I; O; O]
% M12(t) = Gam(t) * [O; I; O]
% R1(t)  = Gam(t) * [O; O; I]
% Since xT is constant, therefore the input is only a function t and x(t):
u = @(t,x) (0.5/rho)*B' * ((Gam(t)*[O;I;O]) \ (Gam(t)*[I;O;O] * x + (Gam(t)*[O;O;I]-I) * xT));

% Using this control law then the dynamics of the system can be written as:
dx = @(t,x) A * x + B * u(t,x);

% Since the dynamics is linear but timevarying I simulated it using ode45:
[t, x] = ode45(dx, [0 T], x0);

% Calculate the applied control input from the results
U = zeros(1, length(t));
for k = 1 : length(t)
    U(k) = u(t(k), x(k,:)');
end

% Plotting the results
figure
subplot(2,1,1)
plot(t, x)
legend('x_1', 'x_2', 'location', 'north')
subplot(2,1,2)
plot(t, U)
xlabel('t')
ylabel('u')