A1=[-0.1778 0 0 0 0 -9.7807 -9.7807 0 0 0 0];
A2=[0 -0.3104 0 0 9.7807 0 0 9.7807 0 0 0];
A3=[-0.3326 -0.5353 0 0 0 0 75.764 343.86 0 0 0];
A4=[-0.1903 -0.2490 0 0 0 0 172.62 -59.958 0 0 0];
A5=[0 0 1 0 0 0 0 0 0 0 0];
A6=[0 0 0 1 0 0 0 0 0 0 0];
A7=[0 0 0 -1 0 0 -8.1222 4.6535 0 0 0];
A8=[0 0 -1 0 0 0 -0.0921 -8.1222 0 0 0];
A9=[0 0 0 0 0 0 0 0 -0.6821 -0.0535 0];
A10=[0 0 0 0 0 0 0 0 -0.2892 -5.5561 -36.674];
A11=[0 0 0 0 0 0 0 0 0 2.7492 -11.112];
A=[A1;A2;A3;A4;A5;A6;A7;A8;A9;A10;A11];
B = [ 0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0; .0496 2.6224 0 0; 2.4928 0.1741 0 0; 0 0 7.8246 0; 0 0 1.6349 -58.4053; 0 0 0 0];
C = [ 1 0 0 0 0 0 0 0 0 0 0; 0 1 0 0 0 0 0 0 0 0 0; 0 0 1 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0 0 0; 0 0 0 0 1 0 0 0 0 0 0; 0 0 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 0 0 1 0 0; 0 0 0 0 0 0 0 0 0 1 0];
D = 0;
Q = eye(11);
R = eye(4);
%Q=[1 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0];
SYS = ss(A,B,C,D);
[F,P,E]=lqr(SYS,Q,R)
x0=[1 0 0 0 0 -0.1 0 0 0 0 0]';
Ac=A-B*F;
Bc= zeros(size(B));
sys = ss(Ac,Bc,C,D);
time=0:0.01:5;%simulation time
[y,t,x]=initial(sys,x0,time);
x1=[1 0 0 0 0 0 0 0 0 0 0]*x';
x2=[0 1 0 0 0 0 0 0 0 0 0]*x';
x3=[0 0 1 0 0 0 0 0 0 0 0]*x';
x4=[0 0 0 1 0 0 0 0 0 0 0]*x';
x5=[0 0 0 0 1 0 0 0 0 0 0]*x';
x6=[0 0 0 0 0 1 0 0 0 0 0]*x';
x7=[0 0 0 0 0 0 1 0 0 0 0]*x';
x8=[0 0 0 0 0 0 0 1 0 0 0]*x';
x9=[0 0 0 0 0 0 0 0 1 0 0]*x';
x10=[0 0 0 0 0 0 0 0 0 1 0]*x';
x11=[0 0 0 0 0 0 0 0 0 0 1]*x';
figure
subplot(2,2,1);hold on;plot(time,x1);grid
xlabel('t(sec)');ylabel('x1')
subplot(2,2,2);hold on;plot(time,x2);grid
xlabel('t(sec)');ylabel('x2')
subplot(2,2,3);hold on;plot(time,x3);grid
xlabel('t(sec)');ylabel('x3')
subplot(2,2,4);hold on;plot(time,x4);grid
xlabel('t(sec)');ylabel('x4')
figure
subplot(2,2,1);hold on;plot(time,x5);grid
xlabel('t(sec)');ylabel('x5')
subplot(2,2,2);hold on;plot(time,x6);grid
xlabel('t(sec)');ylabel('x6')
subplot(2,2,3);hold on;plot(time,x7);grid
xlabel('t(sec)');ylabel('x7')
subplot(2,2,4);hold on;plot(time,x8);grid
xlabel('t(sec)');ylabel('x8')
figure
subplot(2,2,1);hold on;plot(time,x9);grid
xlabel('t(sec)');ylabel('x9')
subplot(2,2,2);hold on;plot(time,x10);grid
xlabel('t(sec)');ylabel('x10')
subplot(2,2,3);hold on;plot(time,x11);grid
xlabel('t(sec)');ylabel('x11')
Qe=eye(11);
Re=eye(8);
G=eye(11);
[L,Pe,E] = LQE(A,G,C,Qe,Re)
%display(L)
Ak=A-L*C;
sys=ss(Ak,B,C,D);
time=0:0.01:10;
x0=[1 0 0 0 0 -0.1 0 0 0 0 0]';
[y,t,x]=initial(sys,x0,time);
e1=[1 0 0 0 0 0 0 0 0 0 0]*x';
e2=[0 1 0 0 0 0 0 0 0 0 0]*x';
e3=[0 0 1 0 0 0 0 0 0 0 0]*x';
e4=[0 0 0 1 0 0 0 0 0 0 0]*x';
e5=[0 0 0 0 1 0 0 0 0 0 0]*x';
e6=[0 0 0 0 0 1 0 0 0 0 0]*x';
e7=[0 0 0 0 0 0 1 0 0 0 0]*x';
e8=[0 0 0 0 0 0 0 1 0 0 0]*x';
e9=[0 0 0 0 0 0 0 0 1 0 0]*x';
e10=[0 0 0 0 0 0 0 0 0 1 0]*x';
e11=[0 0 0 0 0 0 0 0 0 0 1]*x';
figure
subplot(2,2,1);hold on;plot(time,e1);grid
xlabel('t(sec)');ylabel('e1')
subplot(2,2,2);hold on;plot(time,e2);grid
xlabel('t(sec)');ylabel('e2')
subplot(2,2,3);hold on;plot(time,e3);grid
xlabel('t(sec)');ylabel('e3')
subplot(2,2,4);hold on;plot(time,e4);grid
xlabel('t(sec)');ylabel('e4')
figure
subplot(2,2,1);hold on;plot(time,e5);grid
xlabel('t(sec)');ylabel('e5')
subplot(2,2,2);hold on;plot(time,e6);grid
xlabel('t(sec)');ylabel('e6')
subplot(2,2,3);hold on;plot(time,e7);grid
xlabel('t(sec)');ylabel('e7')
subplot(2,2,4);hold on;plot(time,e8);grid
xlabel('t(sec)');ylabel('e8')
figure
subplot(2,2,1);hold on;plot(time,e9);grid
xlabel('t(sec)');ylabel('e9')
subplot(2,2,2);hold on;plot(time,e10);grid
xlabel('t(sec)');ylabel('e10')
subplot(2,2,3);hold on;plot(time,e11);grid
xlabel('t(sec)');ylabel('e11')
sys1=lqgreg(Kest,F)
AL=A-(B*F)-(L*C);
%display(A)
BL=zeros(size(B));
CL= zeros(size(C));
sys=ss(AL,BL,CL,D);
time=0:0.01:10;
x0=[1 0 0 0 0 -0.1 0 0 0 0 0]';
[y,t,x]=initial(sys,x0,time);
L1=[1 0 0 0 0 0 0 0 0 0 0]*x';
L2=[0 1 0 0 0 0 0 0 0 0 0]*x';
L3=[0 0 1 0 0 0 0 0 0 0 0]*x';
L4=[0 0 0 1 0 0 0 0 0 0 0]*x';
L5=[0 0 0 0 1 0 0 0 0 0 0]*x';
L6=[0 0 0 0 0 1 0 0 0 0 0]*x';
L7=[0 0 0 0 0 0 1 0 0 0 0]*x';
L8=[0 0 0 0 0 0 0 1 0 0 0]*x';
L9=[0 0 0 0 0 0 0 0 1 0 0]*x';
L10=[0 0 0 0 0 0 0 0 0 1 0]*x';
L11=[0 0 0 0 0 0 0 0 0 0 1]*x';
figure
subplot(2,2,1);hold on;plot(time,L1);grid
xlabel('t(sec)');ylabel('L1')
subplot(2,2,2);hold on;plot(time,L2);grid
xlabel('t(sec)');ylabel('L2')
subplot(2,2,3);hold on;plot(time,L3);grid
xlabel('t(sec)');ylabel('L3')
subplot(2,2,4);hold on;plot(time,L4);grid
xlabel('t(sec)');ylabel('L4')
figure
subplot(2,2,1);hold on;plot(time,L5);grid
xlabel('t(sec)');ylabel('L5')
subplot(2,2,2);hold on;plot(time,L6);grid
xlabel('t(sec)');ylabel('L6')
subplot(2,2,3);hold on;plot(time,L7);grid
xlabel('t(sec)');ylabel('L7')
subplot(2,2,4);hold on;plot(time,L8);grid
xlabel('t(sec)');ylabel('L8')
figure
subplot(2,2,1);hold on;plot(time,L9);grid
xlabel('t(sec)');ylabel('L9')
subplot(2,2,2);hold on;plot(time,L10);grid
xlabel('t(sec)');ylabel('L10')
subplot(2,2,3);hold on;plot(time,L11);grid
xlabel('t(sec)');ylabel('L11')