%
% Chapter 3, Example 3.15, pages 124-126
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function noise_example
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m1 = 1;
M = [m1];
K = [0];
C = 0.05*K;

A = [zeros(1) eye(1); -inv(M)*K -inv(M)*C];
B = [zeros(1,1);inv(M)*[1]];
G = [zeros(1,1);inv(M)*[1]];

Cout = [0 1];
D = [0];

Cout = eye(2); D = zeros(2,1);
Q = Cout'*Cout;
R = 0.01;
parvec = struct('A',A,'B',B,'C',Cout,'D',D','G',G,'Q',Q,'R',R);

[Klqr] = lqr(A,B,Q,R);

Ac = A-B*Klqr;

sys1 = ss(Ac,G,Cout,D);
[y1,t1,x2] = impulse(sys1,30);

xopt = fminunc(@cost,Klqr,[],parvec)
flqr = cost(Klqr,parvec)
fnoise = cost(xopt,parvec)

% Generate plots of the variation of states due to input noise
tvec = linspace(0,30,1001);
unoise = randn(length(tvec),1);
W = 1;
P = lyap((A-B*Klqr),G*W*G');
x0 = [1 0];
[yn,tn,xn] = lsim(sys1,unoise,tvec,x0);

% Integrate P_dot equation
PA = [0 2 0;-Klqr(1) -Klqr(2) 1; 0 -2*Klqr(1) -2*Klqr(2)];
PB = [0;0;1];
Psys = ss(PA,PB,eye(3),[0;0;0]);
P0 = x0(:)*x0(:)';
Pinput = ones(size(tvec));
[yP,tP] = lsim(Psys,Pinput,tvec,[P0(1,1);P0(1,2);P0(2,2)]);

figure(1);
clf
subplot(211), plot(tn,xn(:,1),tP,1*sqrt(yP(:,1)),'--r',tP,-1*sqrt(yP(:,1)),'--r'); xlabel('Time'); ylabel('Displacement');
subplot(212), plot(tn,xn(:,2),tP,1*sqrt(yP(:,3)),'--r',tP,-1*sqrt(yP(:,3)),'--r'); xlabel('Time'); ylabel('Velocity');

q11 = Q(1,1); q22 = Q(2,2); % Analytical solution
Knoise = [ (Q(1,1)/R)^(1/2) 1/(q11*R)^(1/2)/R*((q11*R)^(1/2)*R*(2*q11*R+(q11*R)^(1/2)*q22))^(1/2)]
KLqq = [sqrt(q11/R) (2*(1/R*q11)^(1/2)+1/R*q22)^(1/2)]

function f = cost(x,parvec);
x = x(:);

A = parvec.A;
B = parvec.B;
C = parvec.C;
D = parvec.D;
G = parvec.G;
Q = parvec.Q;
R = parvec.R;

W = 5;

P = lyap((A-B*x'),G*W*G');
f = trace(P*(Q+x*R*x'));
