lqrcontrol.m

controller with disrejection plus kalman

function dydt=lqrcontrol(t,e,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);
U=-K1*[e(5);e(6)]-K2*e(3:4);
ed=A*e(1:2)+B*U+Bw*Win+A*xd;
edot=[ed(1);ed(2)];
zdot=[e(5);e(6)];
y=awgn(e(1)+Wout,200,'measured');
ybar=C*e(5:6);
ebardot=A*e(5:6)+B*U+Bw*Win+A*xd+Ke*(y-ybar);
dydt=[edot;zdot;ebardot];