% Pole placement design of regulator using prediction estimator for flexible structure

% 2 masses coupled by spring and damper
% States are [d ddot y ydot]'

M = 1; m = 0.1; k = 0.091; b = 0.0036;

A = [
   0  1  0  0
   -k/m  -b/m  k/m  b/m
   0  0  0  1
   k/M  b/M  -k/M  -b/M
];

B = [0; 0; 0; 1/M];

C = [1 0 0 0
   0 0 1 0
   0 0 0 0];  % get d, y and u as outputs

D = [0; 0; 1];

Gp = ss(A, B, C, D, 'InputName', 'force u', 'OutputName', ...
   {'disp d', 'disp y', 'force u'});
set(Gp, 'StateName', {'d', 'ddot', 'y', 'ydot'})

% Discrete model
T = 0.4;
Gpd = c2d(Gp, T);
Phi = Gpd.a; Gam = Gpd.b;

% FPW suggest closed-loop poles:
dclz = [0.9+j*0.05; 0.9-j*0.05; 0.8+j*0.4; 0.8-j*0.4];
K = acker(Phi, Gam, dclz);

% Luenberger estimator looking at d and y
% Place estimator poles 5 times as fast as controller poles
dcls = 1/T*log(dclz); deps = 5*dcls; depz = exp(T*deps);
Cy = Gpd(1:2,1).c;  % extract C matrix which gives d and y as outputs
Lp = place(Phi', Cy', depz)';

% Regulator
controls = [1];  % force u
sensors = [1, 2];  % d and y are sensed
known = [];  % no disturbance input
H = reg(Gpd, K, Lp, sensors, known, controls);

% Closed-loop system
feedin = [1];  % force u
feedout = [1, 2];  % d and y
Gcl = feedback(Gpd, H, feedin, feedout, +1);

% Response to initial condition
x0 = zeros(8,1); x0(1) = 1;
figure(1), clf, initial(Gcl, x0)

% Get all states and compare actual with estimates 
[y, t, x] = initial(Gcl, x0, 10);

figure(2), clf 
subplot(2,2,1), stairs(t,x(:, [1 5])), grid, legend('x_1=d', 'x_1hat')
title('Response of states and predictive estimates to x_1(0) = 1'), xlabel('Time (s)')
subplot(2,2,2), stairs(t,x(:, [2 6])), grid, legend('x_2=ddot', 'x_2hat')
xlabel('Time (s)')
subplot(2,2,3), stairs(t,x(:, [3 7])), grid, legend('x_3=y', 'x_3hat')
xlabel('Time (s)')
subplot(2,2,4), stairs(t,x(:, [4 8])), grid, legend('x_4=ydot', 'x_4hat')
xlabel('Time (s)')
