% LQG regulator for flexible structure with measurements of d and y

% 2 masses coupled by spring and damper
% States are [d ddot y ydot]'
% Linear Quadratic Regulator Example
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;

% Optimal control
% Weight displacements but not velocities
% Get trial diagonal elements for Q
% Scalar input
Rc = 1
Q11 = input('Enter controller Q11: ');
Q22 = input('Enter controller Q22: ');
Qc = diag([Q11 0 Q22 0]);

K = dlqr(Phi, Gam, Qc, Rc);

% Optimal estimation
% Assume rms noise of 1% on each sensor channel
Re = 0.01^2 * eye(2)
% Get trial  Q
Qe = input('Enter estimator Q: ');
sensors = [1, 2];  % d and y are sensed
known = [1];  % force u
P = ss(Phi, [Gam Gam], C, [D D], T);
[He, L] = kalman(P, Qe, Re, [], sensors, known);

% Form regulator
Hreg = lqgreg(He, K, 'current');
% Hreg = lqgreg(He, K);

% Closed-loop system
feedin = [1];  % force u
feedout = [1, 2];  % d and y
Gcl = feedback(Gpd, Hreg, 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', 0)
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', 0)
xlabel('Time (s)')
subplot(2,2,3), stairs(t,x(:, [3 7])), grid, legend('x_3=y', 'x_3hat', 0)
xlabel('Time (s)')
subplot(2,2,4), stairs(t,x(:, [4 8])), grid, legend('x_4=ydot', 'x_4hat', 0)
xlabel('Time (s)')
