newlqr.m

1)LQG design of an unmanned helicopter 2) LQR control of an unmanned helicopter 3) speed torque ch

A=[-0.1778 0 0 0 0 -9.7807 -9.7807 0 0 0 0;0 -0.3104 0 0 9.7807 0 0 9.7807 0 0 0;-0.3326 -0.5353 0 0 0 0 75.764 343.86 0 0 0; 
    -0.903 -.249 0 0 0 0 172.62 -59.958 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 -1 0 0 -8.1222 4.6535 0 0 0;0 0 -1 0 0 0 -0.0921 -8.1222 0 0 0;0 0 0 0 0 0 0 0 -0.6821 -0.0535 0;
    0 0 0 0 0 0 0 0 -0.2892 -5.5561 -36.674;0 0 0 0 0 0 0 0 0 2.7492 -11.112];
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;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=[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 2 0 0 0 0;0 0 0 0 0 0 0 2 0 0 0;0 0 0 0 0 0 0 0 2 0 0;0 0 0 0 0 0 0 0 0 2 0;0 0 0 0 0 0 0 0 0 0 2];
%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];
R=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];

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')
        
        
Qe=[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];
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);

 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')