function v = kalmanSkalar(Mu0,Sigma0,Phi,A,Q,R,n)
%
% simulates a scalar AR(1) chain and computes corresponding Kalman
% filtered and Kalman smoothed values. Plots these results and
% corresponding 95%-intervals.
%

hold off
pauseLength = 1.0;

%
% initialisation
%
x(1) = normrnd(Mu0,sqrt(Sigma0));
y(1) = A * x(1) + normrnd(0,sqrt(R));

xttm(1) = Mu0;
Pttm(1) = Sigma0;

%
% simulate and run Kalman filter
%

for (t = 1:n)
  if (t > 1)
    xttm(t) = Phi * xtt(t-1);
    Pttm(t) = Phi * Ptt(t-1) * Phi + Q;
    
    x(t) = Phi * x(t-1) + normrnd(0,sqrt(Q));
    y(t) = A * x(t) + normrnd(0,sqrt(R));
  end;
  
  K(t) = Pttm(t) * A / (A * Pttm(t) * A + R);
  xtt(t) = xttm(t) + K(t) * (y(t) - A * xttm(t));
  Ptt(t) = (1.0 - K(t) * A) * Pttm(t);
end;

%
% run Kalman smoother
%

xtn(n) = xtt(n);
Ptn(n) = Ptt(n);

for (t = n:-1:2)
  J(t-1) = Ptt(t-1) * Phi / Pttm(t);
  xtn(t-1) = xtt(t-1) + J(t-1) * (xtn(t) - xttm(t));
  Ptn(t-1) = Ptt(t-1) + J(t-1) * (Ptn(t) - Pttm(t)) * J(t-1);
end;


%
% plot results step by step
%

ymin = min([x y,xtt-1.96*sqrt(Ptt) xtt+1.96*sqrt(Ptt)]);
ymax = max([x y,xtt-1.96*sqrt(Ptt) xtt+1.96*sqrt(Ptt)]);
figure(1);
plot(0,x(1),'b.')
plot(0,y(1),'go');
axis([0 n-1 ymin ymax]);
hold on

for (t = 1:n)
  pause(pauseLength);
  if (t == 1)
    plot(t-1,xtt(t),'r.');
    plot(t-1,xtt(t)-1.96*sqrt(Ptt(t)),'r.',t-1,xtt(t)+1.96*sqrt(Ptt(t)),'r.');
  else
    plot([t-2 t-1],xtt(t-1:t),'r-');
    plot([t-2 t-1],xtt(t-1:t)-1.96*sqrt(Ptt(t-1:t)),'r-');
    plot([t-2 t-1],xtt(t-1:t)+1.96*sqrt(Ptt(t-1:t)),'r-');
  end
  pause(pauseLength);
  if (t < n)
    plot([t-1 t],x(t:t+1),'b-');
    plot([t],y(t+1),'go');
    pause(pauseLength);
  end;
end;

pause(pauseLength);

for (t = n-1:-1:1)
  plot([t-1:t],xtn(t:t+1),'k-');
  plot([t-1:t],xtn(t:t+1)-1.96*sqrt(Ptn(t:t+1)),'k-');
  plot([t-1:t],xtn(t:t+1)+1.96*sqrt(Ptn(t:t+1)),'k-');
  pause(pauseLength);
end;


hold off

