clear all; clc;
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 (velocity)
d = [0];
% step 1: now the inputs are v and ref, and outputs are x, xdot, anderror (xdot-r)
b2 = [b zeros(2,1)];
c2 = [1 0
0 1
0 1];
d2 =[ 0 0
0 0
0 -1];
% step 2: create additional state as integral of error
[ai,bi,ci,di] = zp2ss([],[0],[1]); % Integrator system
[aa,ab,ac,ad] = append(a,b2,c2,d2,ai,bi,ci,di); % [SYS1 0;0 SYS2]: 3 inputs, 3 states, 3 outputs
sys1 = ss(aa,ab,ac,ad);
sys2 = ss(0,0,0,1);
clsys = feedback(sys1,sys2,[3],[3],+1); % clsys has 3 states, the 3rd state is integral of error
[aa,ab,ac,ad] = ssdata(clsys)
[xa,xb,xc,xd]=ssdelete(aa,ab,ac,ad,[2 3],[3],[4])
plant = ss(xa,xb,xc,xd);
% xa,xb,xc,xd has 1 input [v] and three outputs [x; xdot; integral(xdot-r)]
T= 0.001;
t0=T/5;
[dax,dbx,dcx,ddx] = ssdata(c2d(plant,T,’zoh’)); % the augmented system will be used for controller design
[da,db,dc,dd] = ssdata(c2d(ss(a,b,c,d),T,’zoh’)); % the original system will be used for observer
q = diag([0.1 50 4000]); % controller penalty for each state
r = diag([0.1]); % penalty for control action
k = dlqry(dax,dbx,dcx,ddx,q,r) % feedback gain matrix for control law: u = -k*x
k1=k(1);
k2=k(2);
k3=k(3);
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])