% Oving 6, oppgave 3.
% Simulering med analytiske uttrykk for R(t) og h(t).


 A=0; B=1; D=1; x0=0;
 Q=1; P=1; S=2;
 t0=0; t1=10; ts=5; dt=0.01;              % dt er skrittlengde for Euler int.

S=fchan('Vekt S',S);
Q=fchan('Vekt Q',Q);

Ns=length(t1:-dt:t0);                    % Lager referansevektoren r(t).
[r,tt]=ts4u(0,1,Ns,ts/dt,dt,0);

h_t1=-D'*S*r(Ns);
R_t1=D'*S*D;

sq=sqrt(Q);

tid=0; x=x0;                             % Simulerer prosessen med
for it = 1:Ns                            % optimalt paadrag.
  et(it)=tid;
  y=x;

  phi_11=cosh(sq*(t1-tid));
  phi_21=-sq*sinh(sq*(t1-tid));
  phi_12=-sinh(sq*(t1-tid))/sq;
  phi_22=cosh(sq*(t1-tid));

  Rt(it)=(R_t1*phi_11-phi_21)/(phi_22-R_t1*phi_12);

  if tid < ts
   h1=r(Ns)*(1-cosh(sq*(t1-ts)));
   h2=r(Ns)*sq*sinh(sq*(t1-ts));
  else
   h1=r(Ns)*(1-cosh(sq*(t1-tid)));
   h2=r(Ns)*sq*sinh(sq*(t1-tid));
  end

  ht(it)=(R_t1*h1-h2+h_t1)/(phi_22-R_t1*phi_12);


  u = -inv(P)*B'*Rt(it)*x - inv(P)*B'*ht(it);
  es(it,1)=ht(it);
  es(it,2)=y;
  es(it,3)=u;
  fx=A*x+B*u;
  x = x + dt*fx;
  tid=tid+dt;
end

figure(1)                                % Plotter resultatene.
subplot(211), plot(tt,es(:,1))
title('Feed forward signal h(t)')
grid
subplot(212), plot(tt,Rt)
title('Solution R(t) of the Riccati equation')
xlabel('Continuous time [sec]')
grid
% print -deps fig1_ex2_3_track

figure(2)
subplot(211), plot(tt,[es(:,2) r])
title('Process output y(t) and reference r(t)')
grid
subplot(212), plot(tt,[es(:,3)])
title('Optimal control u(t)')
xlabel('Continuous time [sec]')
grid
% print -deps fig2_ex2_3_track
