clear all
%
% state dimension
nx=2;
% Random examples
A1=randn(nx);
A2=randn(nx);
%
B1=randn(2);
B2=randn(2,1)
Q1=[1 0;0 1];
Q2=[1 0;0 1];
%
R1=eye(2);
R2=1;
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% example 2 HSCC Automatica
% A1=[1 0;
% 1 -1];
% A2=[-2 1;
% 0 1];
% B1=[0;1];
% B2=[1;0];
% Q1=eye(2);
% Q2=Q1;
% R1=2;
% R2=1;
%
% example 1 HSCC
% A1=[-2.7 3.9
% 4.4 -12.6]
% A2=[-9.5 -5.1
% -7.5 -3.3]
% %
% B1=[0.1;0]
% B2=[4.6;0];
% Q1=eye(2);
% Q2=Q1;
% R1=1;
% R2=2;
%
%load datarandom_ABQR
% LQ Cost for each subsystems (if exist)
[P1,L,G,report] = care(A1,B1,Q1,R1);
if isempty(P1)
P1=zeros(2);
end
[P2,L,G,report] = care(A2,B2,Q2,R2);
if isempty(P2)
P2=zeros(2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Determination of Lambda^+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k=1;
P=zeros(nx,nx,1);
% Discretization step
dalpha=.001;
for alpha=0:dalpha:1
AA=alpha*A1+(1-alpha)*A2;
QQ=alpha*Q1+(1-alpha)*Q2;
if alpha==1
RR=R1;
BB=B1;
elseif alpha==0
RR=R2;
BB=B2;
else
RR=blkdiag(alpha*R1,(1-alpha)*R2);
BB=[alpha*B1 (1-alpha)*B2];
end
[Pc,L,G,REPORT]=care(AA,BB,QQ,RR);
if ~isempty(Pc)
P(:,:,k)=Pc;
k=k+1;
end
end
% number of elements in Lambda^+
nn=k-1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cholesky factorisation /Matrice representation of Vm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PPc=zeros(nx*nn,nx);
for k=1:nn
PPc((k-1)*nx+(1:nx),:)=chol(P(:,:,k));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matrice representation of Vm
% Vm(x)=min(S*z'.*z) where z=PPc*x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
B=spdiags(ones(nn,2),0:1,nn,nn+1);
S=spdiags(B,0:nn,nn,2*nn); %full
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% final time and step number
N=1000;
tf=10;
% Number of trajectories
pas=15;
%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
clf,
hold on
figure(2)
clf
hold on
% Radius of initial ball
rayon=5;
Cost=[];
for theta=0:pi/pas:pi-pi/pas
x=rayon*[cos(theta);sin(theta)];
% time step
dt=tf/N;
% initialisation
X=zeros(N+1,nx);
X(1,:)=x';
cost=0;
for k=1:N
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Feedback determination
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Z=zeros(1,nn);
% for kkk=1:nn
% Z(1,kkk)=x'*P(:,:,kkk)*x;
% %Z(1,kkk)=trace(Pi'*P(:,:,kkk)*Pi);
% end
z=PPc*x;
Z=S*(z.*z);
[~,i]=min(Z);
PP=P(:,:,i);
K1=R1^(-1)*B1'*PP;
K2=R2^(-1)*B2'*PP;
AA1=A1-B1*K1;
AA2=A2-B2*K2;
QQ1=Q1+K1'*R1*K1;
QQ2=Q2+K2'*R2*K2;
e1=x'*PP*AA1*x+.5*x'*QQ1*x;
e2=x'*PP*AA2*x+.5*x'*QQ2*x;
[~,mode] =min([e1,e2]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Integration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if mode==1
x1=expm((AA1)*dt)*x;
cost=cost+dt*(x'*(QQ1)*x+x1'*(QQ1)*x1)/2;
x=x1;
else
x1=expm((AA2)*dt)*x;
cost=cost+dt*(x'*(QQ2)*x+x1'*(QQ2)*x1)/2;
x=x1;
end
%%%%%%%%%%%
% Save
%%%%%%%%%%%
X(k+1,:)=x';
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
title('state space')
color='r';
plot(X(:,1),X(:,2),color,'LineWidth',1)
plot(-X(:,1),-X(:,2),color,'LineWidth',1)
Cost= [Cost cost/2];
end
Cost2= [];
Cost1= [];
Costlyap=[];
Costlyap2=[];
Costgeromel=[];
for theta=0:pi/pas:pi-pi/pas
x1=cos(theta);
x2=sin(theta);
x=rayon*[cos(theta);sin(theta)];
Cost1= [Cost1 .5*x'*P1*x];
Cost2= [Cost2 .5*x'*P2*x];
z=PPc*x;
Z=S*(z.*z);
[val,i]=min(Z)
Costlyap=[Costlyap .5*val];
end
figure(2)
title('Cost Comparison')
theta=0:pi/pas:pi-pi/pas;
color='g-*'
plot(theta,Cost,color)
%
plot(theta,Cost1,'g-d')
plot(theta,Cost2,'c-+')
plot(theta,Costlyap,'r-s')
grid on
save datarandom_ABQR A1 A2 B1 B2 R1 R2 Q1 Q2;