Kalman vs pole plac

clc
clear all

format long
w=0.00007291;
r=422000;





As=[0 1 0 0;3*w*w 0 0 2*r*w;0 0 0 1;0 -(2*w)/r 0 0]
Bs=[0 0 ;1 0; 0 0 ; 0 1/r]
Cs=[1/r 0 0 0; 0 0 1 0]
%Cs=eye(4)








[p, n]=size(Cs);
Ag=[As zeros(n,p); -Cs zeros(p,p)]
Bg=[Bs; zeros(p,p)]
Cg=[Cs zeros(p,p)]






p1=-0.236619558550974
p2= -0.236619558550973
p3= -0.010598006934913;
p4= -0.069459950787419
p5= -0.034729982561367
p6= -0.034729982561367



K=place(Ag,Bg,[p1 p2 p3 p4 p5 p6]);





Kf=K(:,1:4)
Ki=K(:,5:end)  %% simple feedback control with integrator






%%Kalman


Qk=diag([1 1 10 10 2000000000 900000000]);
Rk=diag([16000000 1800000000000000]);
KKottimo=lqr(Ag,Bg,Qk,Rk)

Kkfottimo=KKottimo(:,1:4);   // matrix for the lqg control
Kkiottimo=KKottimo(:,5:end);


Q=0.05
v_y=0.00000000003;

Qtilde=Q*diag([1 1 1 1]);
Rtilde=v_y*diag([1 1]);
Lk=lqr(As',Cs',Qtilde,Rtilde)';
Afk=As-Lk*Cs;   %%matrix of the kalman filter
Bfk=[Bs-Lk*D Lk];
Cfk=eye(4);
Dfk=zeros(4,4);