% reachability
% x(k+1)=Ax(xk)+Bu(k)

clear all
close all

A = [  1 1 ;  %#ok
       0 1]

B = [ .5; %#ok
     1]   

R = [B, A*B]

% or
R=ctrb(A,B) %#ok

rank(R)

x1=[5;2];

x0=[0;0];

% want reach x1 in two steps from 0
%
% need to solve x1=R*U where U=[u(1); u(0)], notice the inputs in **reverse** order

U=inv(R)*x1 ; %#ok

U=flipud(U) % flip order
X=x0
for ii=1:size(U,1) 
 X=[X A*X(:,end)+B*U(ii)];
end
%figure('WindowStyle','docked')
plot(X(1,:),X(2,:),'o-')
axis([-10 10 -10 10])
grid



% take a nonzero initial state

x0=[-2;-2]

% want reach x1 in two steps from x0
% LEFT AS EXERCISE...


% need to solve x1-A^2x0 = R*U where U=[u(1); u(0)]

U=inv(R)*(x1-A^2*x0) 


U=flipud(U) %flip order
X=x0
for ii=1:length(U) 
 X=[X A*X(:,ii)+B*U(ii)];
end
hold on
plot(X(1,:),X(2,:),'o-')


% more than n=2 steps
K=8

% want reach x1 in K steps from x0

% need to solve x1-A^Kx0 = R_K*U where U=[u(K-1); .... u(0)] (see formula (14),
% Lecture 4, p50)


% build R_K

R_K=[];
for ii=0:K-1
    R_K=[R_K A^ii*B] %#ok
end

%reachability gramian
W_K=R_K*R_K';

% find the minimum energy input sequence
U=R_K'*inv(W_K)*(x1-A^K*x0) % 

% which is the same as...
U=pinv(R_K)*(x1-A^K*x0)

U=flipud(U) 
X=x0
for ii=1:length(U) 
 X=[X A*X(:,ii)+B*U(ii)];
end
hold on
plot(X(1,:),X(2,:),'o-')

%.. but is different from the one obtained via mldivide
% 

U_=R_K\(x1-A^K*x0)  %[equivalent to U_=mldivide(R_K,x1-A^K*x0)]

% .. which IS NOT MINIMUM NORM, because the solution using pinv() minimizes ||x||_2 
% while mldivide(A,b) minimizes ||Ax-b||
% and in the case of underdetermined systems, picks up a solution having
% at most rank(A) nonzero elements doc(mldivide)

U_=flipud(U_) % flip order
X=x0
for ii=1:length(U_) 
   X=[X A*X(:,ii)+B*U_(ii)];
end
hold on
plot(X(1,:),X(2,:),'o-')

% indeed...

norm(U)
norm(U_)