% minimum energy control for electrical system (Lecture 4, page 23)

clear all
close all

R1=1e3 %Ohm
C1=1e-3 %Farad

alpha=.5
R2=alpha*R1
C2=C1

% .
% x = Ax + Bu
%
% y = Cx + Du

%y=Ix

% state matrix
A=[-1/(R1*C1)   0;
    0           -1/(R2*C2)]

% input matrix
B=[ 1/(R1*C1); 
    1/(R2*C2)]

% output matrix
C=eye(2)

% input to output matrix
D=zeros(2,1)


% reachability matrix
R=[B A*B]

% or...
R=ctrb(A,B)



% rank check
rank(R)

el_sys=ss(A,B,C,D)

figure('Windowstyle','docked')
step(el_sys)

x0=[0;0] %initial state

T=10 % second

deltat=0.01;

% vector of time instants
tt=[0:deltat:T]

u=1*ones(1,length(tt))



figure('Windowstyle','docked')
lsim(el_sys,u,tt,x0)

x1=[3; -3] %final state

% compute the minimum energy control

%reachability Gramian [ Lecture 4, p29 formula (6) ] 

WT=gram(el_sys,'c','TimeIntervals',[0 T])

eta=inv(WT)*x1

%thus, the minimum energy control is computed as follows
u=[];
for t=tt
    u=[u B'*expm(A'*(T-t))*eta]; 
end

figure
plot(tt,u)
grid on

figure
lsim(el_sys,u,tt,x0)

x=[-1:.01:1];
y=[-1:.01:1];

[X,Y] = meshgrid(x,y);

invWT=inv(WT)

figure
% compute the quadratic form x'*inv(WT)*x
Z = X.^2*invWT(1,1) + 2*X.*Y*invWT(1,2) + Y.^2*invWT(2,2);

contour(X,Y,Z,[0:0.5:10])

axis equal
grid

% set alpha=.9 and check the difference