% losn_reak_kf
% Skisse til loesning av Kalman-filterdelen av oeving 7d.
% Emne: Systemidentifikasjon og optimal estimering.

clear all

% Prosess-parametre og stasjonaerverdier.
x1s=2.5; x2s=1; ths=10; us=25;
k1=50; k2=100; k3=10;

% Tidshorisont, samplingsintervall etc. for simuleringene
dt=0.001; t0=0; t1=0.1;
t=t0:dt:t1; N=length(t);

% Initialiseringer av arrays for simuleringene
X=zeros(N,2); Y=zeros(N,1); U=ones(N,1)*us; f=zeros(2,1);
Xhat=zeros(N,3); Xbar=Xhat; fe=zeros(3,1);

% Linearisert kontinuerlig modell
Dc=[0,1];
a11=-k1-2*k3*x1s-us; a12=0;
a21=k1;              a22=-k2-us;
Ac=[a11,a12;a21,a22];
Bc=[ths-x1s;-x2s];
Cc=[us;0];

% Linearisert diskret modell
A=eye(2)+dt*Ac;
B=dt*Bc;
C=dt*Cc;
D=Dc;

% Utvider med støymodell
At=[A C;zeros(1,2) 1]; Ct=[0;0;1]; Dt=[D 0];
im=1;
if im==1
   Ct=eye(3);
elseif im==2
    Ct=[0;0;1];
end

% Diskret prosess og maalestoey.
v=0.001*randn(N,2); 
w=0.0001*randn(N,1);

% Stasjonaer Kalman-filterforsterkning
if im==1
    W=0.0001; V=diag([0.01 0.01 0.005]); V=diag([0,0,0.005]);
    [K,Xb,Xh]=dlqe(At,Ct,Dt,V,W);
elseif im==2
    W=0.0001; V=0.005;
    [K,Xb,Xh]=dlqe(At,Ct,Dt,V,W);
end
K        % Stasjonær Kalmanfilterforsterkning for Kalmanfilter på apriori 
         % aposteriori form.
% Simulering
x=[x1s;x2s]; 
xb=[x1s;x2s;10]; xb=[2;0.9;9]; xb=[1;0.5;8];
for k=1:N
   % henter måling fra prosessen.
   y=D*x+w(k);
   
   Y(k,:)=y';                     % Lagrer maaling og tilstander.
   X(k,:)=x';
   Th(k,:)=ths;
   
   if k<50; u=U(k); else; u=25; ths=12; end
    
   %%% Start, Kalman filter
   yb=Dt*xb;                      % Prediktert maaling.
   
   K=Xb*Dt'*inv(Dt*Xb*Dt'+W);     % Kalmanfilterforsterkning.
   xh=xb+K*(y-yb);                % Aposteriori tilstandsestimatet,
   
   Xhat(k,:)=xh';                 % Lagrer tilstandsestimatene.
   Xbar(k,:)=xb';
       
   % Oppdaterer apriori tilstandsestimatet.
   fe(1)=-k1*xh(1)-k3*xh(1)^2+(xh(3)-xh(1))*u;
   fe(2)=k1*xh(1)-k2*xh(2)-xh(2)*u;
   fe(3)=0;
   xb=xh+dt*fe;
   
   % Oppdaterer kovariansmatrisene for xh og xb.
   Xh=(eye(3)-K*Dt)*Xb*(eye(3)-K*Dt)'+K*W*K';
   Xb=At*Xh*At'+Ct*V*Ct';                         % Merk: boer oppdatere A her!!!
   %%% Slutt, Kalmanfilter.
   
   % Prosess, oppdatering av prosessens tilstandsvektor.
   f(1)=-k1*x(1)-k3*x(1)^2+(ths-x(1))*u +v(k,1);
   f(2)=k1*x(1)-k2*x(2)-x(2)*u +v(k,2);
   x=x+dt*f;
end

subplot(311), plot(t,X(:,1),'r-',t,Xhat(:,1),'b--',t,Xbar(:,1),'k-.'), grid
title('x_1: virkelig tilstand og estimert tilstand')
subplot(312), plot(t,X(:,2),'r-',t,Xhat(:,2),'b--',t,Xbar(:,2),'k-.'), grid
title('x_2: virkelig tilstand og estimert tilstand')
subplot(313), plot(t,Th,'r-',t,Xhat(:,3),'b--',t,Xbar(:,3),'k-.'), grid
title('\theta: virkelig parameter og estimert parameter')