%% LQ-optimal control on a fixed and finite time interval and with a fixed final state approached as a two-point boundary value problem
% We start by generating some random model and the weights
%% Models and weights

n = 2;                  % order of the system
A = rand(n,n);
B = rand(n,1);
Q = diag(rand(n,1));    % weighting matrices for the quadratic cost function
R = rand(1,1);

% x0 = rand(n,1);
% xf = rand(n,1);
x0 = [1;2];             % initial states
xf = [0;3];             % final (desired) states

tf = 1;                 % final time

%
%% Building Hamiltonian system and solving for the missing boundary conditions
% using a state-transition matrix

H = [A, B/R*B'; Q -A']; % the combined matrix for the Hamiltonian cannonical equations
P = expm(H*tf);         % "state"-transition matrix

P11 = P(1:n,1:n);
P12 = P(1:n,n+1:end);
P21 = P(n+1:end,1:n);
P22 = P(n+1:end,n+1:end);

lambda0 = P12\(xf-P11*x0);  % solving for the initial value of costate

%
%% Solving for the states and costates during the time interval

G = ss(H,ones(2*n,1),eye(2*n,2*n),zeros(2*n,1));% auxiliary system comprising the states and costates

[y,t,w] = initial(G,[x0;lambda0],tf);           % response to nonzero initial values for the states and costates

x = w(:,1:n);           % state
lambda = w(:,n+1:end);  % costate
u = (R\B'*(lambda'))';  % optimal control from the stationarity equation

%
%% Plotting the responses

plot(t,x)
xlabel('t')
ylabel('x(t)')
plot(t,lambda)
xlabel('t')
ylabel('\lambda(t)')
plot(t,u)
xlabel('t')
ylabel('u(t)')