clear all
 Ir=1.560259409e-1;
 Iz=1.622368437e-1;
 Lu=37.2823e-3;
 Ll=155.3623e-3;
 %L1=12.7123e-3;
 %L2=175.9123e-3;
 k1=1.34e-6;
 k2=1.41e-6;
 m=12.868;
 %omega=2000;
 gs=80000;
 x0=0.25e-3;
 r=9e-6;
 lc=14.8e-3
 %for omega=(0:1:400)
 f1=0
 omega1=2*pi*f1
 A1=[0 0 1 0 0 0 0 0
     0 0 0 1 0 0 0 0
     0 0 0 0 0 0 0 0
     0 0 0 0 0 0 0 omega1*Iz/Ir
     0 0 0 0 0 0 1 0
     0 0 0 0 0 0 0 1
     0 0 0 0 0 0 0 0
     0 0 0 -omega1*Iz/Ir 0 0 0 0];
 B=[0 0 0 0
   0 0 0 0
   1/m 1/m 0 0 
   0 0 Lu/Ir Ll/Ir 
   0 0 0 0
   0 0 0 0
   0 0 1/m 1/m 
   Lu/Ir Ll/Ir 0 0];
 C=[1 0 0 0 0 Lu 0 0
    1 0 0 0 0 Ll 0 0
   0 Lu 0 0 1 0 0 0
   0 Ll 0 0 1 0 0 0];
 D=[0 0 0 0 
    0 0 0 0 
    0 0 0 0 
    0 0 0 0];
A11=zeros(8,4);
A22=zeros(4);
A2=[A1 A11
    -C A22];
B11=zeros(4);
B1=[B
    B11];
 Q=[10000000000 0 0 0 0 0 0 0 0 0 0 0
    0 10000000000 0 0 0 0 0 0 0 0 0 0
    0 0 100000 0 0 0 0 0 0 0 0 0
    0 0 0 100000 0 0 0 0 0 0 0 0
    0 0 0 0 10000000000 0 0 0 0 0 0 0
    0 0 0 0 0 10000000000 0 0 0 0 0 0
    0 0 0 0 0 0 100000 0 0 0 0 0
    0 0 0 0 0 0 0 100000 0 0 0 0
    0 0 0 0 0 0 0 0 1000000000000 0 0 0
    0 0 0 0 0 0 0 0 0 1000000000000 0 0
    0 0 0 0 0 0 0 0 0 0 1000000000000 0
    0 0 0 0 0 0 0 0 0 0 0 1000000000000];
  R=[1 0 0 0 
     0 1 0 0 
     0 0 1 0 
     0 0 0 1];
F=lqr(A2,B1,Q,R);
F1=F(:,1:8)
F2=F(:,9:12)

 f3=40
 omega3=2*pi*f3
 A3=[0 0 1 0 0 0 0 0
   0 0 0 1 0 0 0 0
   0 0 0 0 0 0 0 0
   0 0 0 0 0 0 0 omega3*Iz/Ir
   0 0 0 0 0 0 1 0
   0 0 0 0 0 0 0 1
   0 0 0 0 0 0 0 0
   0 0 0 -omega3*Iz/Ir 0 0 0 0];
 Q2=[1000 0 0 0
    0 1000 0 0
    0 0 1000 0
    0 0 0 1000];
R1=[0.01 0 0 0
    0 0.01 0 0
    0 0 0.01 0
    0 0 0 0.01];
omega=omega3
G=[0 0
   0 0
   0 r*(omega^2)
   lc*m*r*(omega^2) 0
   0 0
   0 0
  r*(omega^2) 0
   0 lc*r*(omega^2)];
syskalman=ss(A3,B,C,D);
[kest,L,S]=kalman(syskalman,Q2,R1);