controllerwithdisrejectionpluskalman.m

controller with disrejection plus kalman

function controllerwithdisrejectionpluskalman()
clc; 
tspan=[0 0.008];
einit=[-1;0;0;0;-1;0];   %initial values
beta=64013000;
A=[0 1;0 0]; 
B=[0;beta];
Bw=[0;beta];
C=[1 0];
Win=-0.003;
xd=[1;0];               %what is it???????
Q=[1e7 0; 0 1];
R=1;
%lqr feedback gain
Ax=[A B;0 0 0];
Bx=[0;0;1]; 
Qx=[Q [0;0];0 0 0];
P=are(Ax, Bx*Bx'/R,Qx); 
P11=P(1:2,1:2); 
P12=P(1:2,3); P22=P(3,3);
K1=P22*inv(B'*B)*B'/R;
K2=(P12'/R)-P22*inv(B'*B)*B'*A/R; 
K=[K1 K2]; 
K1;
K2;
%Kalman filter gain
Qe=[600 0;0 600]; 
Re=9e-6;
Pe=are(A',C'*C/Re, Qe);
Ke=Pe*C'/Re;

[t,e]=ode15s(@lqrcontrol,tspan,einit,[],K1,K2,Ke,A,B,C,Bw,Win,xd);
Wout=0.1*sin(110*pi*t)+0.05*sin(220*pi*t)+0.02*sin(440*pi*t)+0.01*sin(880*pi*t);
plot(t,e(:,1)+xd(1)+Wout);
grid on;


figure; 
plot(t,-e(:,1:4)*K');
-e(:,1:4)*K';    %the useage??????????
e(:,1)+xd(1)+Wout;      %the useage??????????
grid on;

syms s;
K=K1+(K2/s);
Abar=(A-B*K-Ke*C);
G1=K*inv(s*eye(2)-Abar)*Ke;
G=beta/(s^2);
Gol=simplify(G*G1);
[N,D]=numden(Gol);
d=sym2poly(D);
n=sym2poly(N);
plantopenloop=tf(n,d);
RM=tf([0.912 457.4 1.433*1e8],[1 359.2 1.433*1e8])*tf([0.7586 962.2 2.491*1e8],[1 789.1 2.491*1e8])*tf([9.917*1e8],[1 1575 9.917*1e8])*tf([2.731*1e8],[1 2613 2.731*1e8]);
tfsys=plantopenloop*RM;
figure;
margin(tfsys);
grid on;
figure;
nyquist(tfsys);
grid on;
figure;
bodemag(1/(1+tfsys));
grid on;
figure;
bodemag(tfsys/(1+tfsys));
grid on;