clc;
clear all;
a = [0 1; 0 -5.643]; % plant model is a spring-mass-damper
b = [0; 50.83]; % system has one input (v: force per unit mass)
c = [1 0]; % system has one output (position)
d = [0];
T= 0.001;
t0=0.0002;
[da,db,dc,dd] = ssdata(c2d(ss(a,b,c,d),T,’zoh’)); % the original system will be used for observer
q = diag([1 0]); % controller penalty for each state
r = diag([25]); % penalty for control action
k = lqr(a,b,q,r); % feedback gain matrix for control law: u = -k*x
k1=k(1);
k2=k(2);
qe = diag([1e1 1e1]); % covariance of the process noise
re = [1]; % covariance of the sensor noise
L = dlqe(da,diag([1e-3 1e-3]),dc,qe,re); % observer matrix DLQE(A,G,C,Q,R)
[ae,be,ce,de] = destim(da,db,dc,dd,L,[1],[1])