% main_ov6_1.m
% Skisse til loesning av oving 6, oppgave 1.


% Prosessparametre
a=-0.1054; b=0.5268; c=0.6322; d=1;
T=-1/a;            % Systemets tidskonstant.

% Simuleringshorisont og samplingstid etc.
dt=1;
t=0:dt:5*T; N=length(t);

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

%%% Kontinuerlig Kalmanfilterforsterkning
[K,X]=lqe(a,c,d,V,W);

% Simulerer systemet med kalmanfilter
u=1;
x=0;
xhat=2;
for k=1:N
   % Prosessmaaling
   y=d*x + w(k);
   
   Y(k,:)=y';
   X(k,:)=x';
   
   % Kalmanfilter
   yhat=d*xhat;
   
   Yhat(:,k)=yhat';
   Xhat(:,k)=xhat';
   
   xhat=xhat+dt*(a*xhat+b*u+K*(y-yhat));
   
   % Oppdaterer prosessen
   x=x+dt*(a*x+b*u+c*v(k));
end

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