%read MIRS data

T=0.01;

C=Cc;
D=Dc;

%expanded system
Ae=[Ac zeros(4,2); -Cc zeros(2,2)];
Be=[Bc; zeros(2,2)];
Qr=diag([0 0 0 0 1000 1000]);
Rr=eye(2)*1;
Ks=lqr(Ae,Be,Qr,Rr);
K=Ks(:,1:4);
Ki=Ks(:,5:6);

Qob=diag([1 1 1 1]);
Rob=diag([1 1]);
Hc=lqr(Ac',Cc',Qob,Rob)';

% Servo-Observer system
Acc=[Ac-Bc*K-Hc*Cc  -Bc*Ki ; zeros(2,4) zeros(2,2)];
Bcc=[zeros(4,2) zeros(4,2) ; eye(2) -eye(2)];
Ccc=-Ks;
Dcc = zeros(2,4);

%digital system
sysd=c2d(ss(Acc,Bcc,Ccc,Dcc),T);

[A B] = c2d(Ac,Bc,T);
C=Cc;

Ad=sysd.a;
Bd=sysd.b;
Cd=sysd.c;
Dd=sysd.d;

% initial value
x0=[0;0;0;0];
xob0=[0;0;0;0];

% reference value
xr=0.1;
xl=-0.1;
thr=2.*3.14;
thl=-2.*3.14;
t1=[0;2;50];
t2=[0;thr;thr];
fr=[t1 t2];
r1=[0;2;50];
r2=[0;thl;thl];
fl=[r1 r2];
