% main_ov6_2.m
% Skisse til loesning av oving 6, oppgave 2.



a=-0.1054; b=0.5268; c=0.6322; d=1;
T=-1/a;

dt=1;
t=0:dt:5*T; N=length(t);

q0=0.1; V=q0^2; 
r0=0.1; W=r0^2;
v=q0*randn(N,1);
w=r0*randn(N,1);

%%% Diskretisert modell
[ad,bdcd,dd]=c2dm(a,[b c],d,[0 0],dt,'zoh');
bd=bdcd(:,1);
cd=bdcd(:,2);

% Diskret Kalmanfilter
[K,Xbar,Xhat]=dlqe(ad,cd,dd,V,W);

u=1;
x=0;
xbar=2;
xhat=2;
for k=1:N
   % Prosessmaaling
   y=dd*x + w(k);
      
   Y(k,:)=y';
   X(k,:)=x';
   
   % Diskret Kalmanfilter paa apriori aposteriori form
   ybar=dd*xbar;
   
   Ybar(k,:)=ybar';
   Xhat(k,:)=xhat';
   Xbar(k,:)=xbar;
   
   xhat=xbar+K*(y-ybar);
   xbar=ad*xhat+bd*u;
   
   % Oppdaterer prosessens tilstand
   x=ad*x+bd*u+cd*v(k);
end

figure(2)
plot(t,X,'b-',t,Xhat,'r--')
grid
title('Tilstand og tilstandsestimat')
legend('x','xhat')

