close all
clc
clear all
% Massonnet Francois Nov 2010
% Simple implementation of the Kalman Filter

% This example is taken from Daget, N., Revue des méthodes d’assimilation, Technical Report (2007)
%available on http://www.cerfacs.fr/~daget/cv_fr.html (in french)

% Here's the story: A boat is cruising along a rectilinear coast. Due to bad weather, the boat runs aground and the captain jumps into a lifeboat at time t=0. Let (x,y) be the positions (x along the coast, y perpendicular) of the lifeboat. Let the origin be the big boat. We assume that the winds are such that the lifeboat (x,y) positions at time i+1 are those at time i deviated according a normal law (diffusion-like process).

% The captain also measures the distance between him and the coast.

% How can he deduce his position from the coast at each time step in an optimal way?

%=========================================================

% 1. Time
% Suppose the captain makes measurements every T seconds during N*T seconds
To=3;		% laps between two measurements
t0=0;		%initial time, seconds
tf=100;		%final time, seconds		
to=[0:To:tf];
lto=length(to);
% Here we note that there might be a miscorrespondence between the times at which obs's are made and those at which model is run.

% 2. True State
% -------------
% The state to be estimated here is of course the (x,y) position of the lifeboat at each time step. Thus
ndim=2;		%two elements in the state vector
Tm=1;		%model time step
tm=[0:Tm:tf];
ltm=length(tm);
x=NaN(ndim,ltm);

% The initial state is supposed to be known as (0,0);
x0=[0;0];
x(:,1)=x0;

% 3. Model 
% --------
% Here the matrix that serves as "background" is a simple identity, because of diffusion-process is assumed. This is our idealized view: we make the gross assumption that position is stationnary from one time step to another, but we allow to have made a mistake.
M=eye(ndim);
% The error follows a normal law centered around zero of covariance matrix Q
sigx=0.4;	%80 cm
sigy=0.4;	%20cm
Q=[sigx^2 0; 0 sigy^2;]; % We chose a triangular matrix but the only requirement is that it is symmetric. Here we add the assumption that x and y errors are not correlated. Furthermore we suppose Q is constant in time.

% 4. Analysis state	
% This will contain the analyzed state, that is the optimal state
xa=NaN(ndim,ltm);
Pa=NaN(ndim,ndim,ltm);	% This is the covariance matrix on analyzed state errors
%Initialize xa with known initial state
xa(:,1)=x0;
%Initialize Pa with zeros, that is certain that we are at (0,0)
Pa(:,:,1)=zeros(ndim,ndim);

% 5. Forecast state
% This will contain the state as forecast by the model M from last time step.
xf=NaN(ndim,ltm);
Pf=NaN(ndim,ndim,ltm);		%This is the covariance matrix of forecast errors
Pf(:,:,1)=zeros(ndim,ndim);		%We initialize it to zero

% 6. Observations
% As the captain observes the coast, he has a direct measure of y. Thus
H=[0 1];
% The error being made is again supposed centered normal
sigh=0.5;		% Note that this error might evolve in time (not here) but is not allowed, under the KF hypothesis, to evolve following the state vector.
R=sigh^2;
%Finally we prepare a vector y that will contain our obs
y=NaN(1,lto);


% Start the loop
for i=2:ltm
	% Make reality evolve
	x(:,i)=M*(x(:,i-1))+Q*randn(ndim,1);
	% Make our forecast
	xf(:,i)=M*(xa(:,i-1));	

	% If we are at a time of observation 
	if to(to==tm(i))
	
	% Find index to which t(i) corresponds
	iobs=find(to==tm(i));
	y(iobs)=H*x(:,i)+R*randn(1);
	
	% Compute the covariance matrix of forecast
	Pf(:,:,i)=M*Pa(:,:,i-1)*M'+Q;

	%Compute the Kalman gain
	K(:,:,i)=Pf(:,:,i)*H'*(H*Pf(:,:,i)*H'+R)^(-1);

	%Evolve the analysis error
	Pa(:,:,i)=(eye(ndim)-K(:,:,i)*H)*Pf(:,:,i);
	
	% Make correction to initial guess
	xa(:,i)=xf(:,i)+K(:,:,i)*(y(iobs)-H*xf(:,i));
	
	%Else if no obs
	
	else
	
	xa(:,i)=xf(:,i);	%that is, no correction is applied
	Pf(:,:,i)=M*Pa(:,:,i-1)*M'+Q;
	Pa(:,:,i)=Pf(:,:,i);
	end
	%input('r')
end

% Diagnostic
fig=figure;
hold on;
box on;
grid on;
plot(tm,x(2,:),'k','Linewidth',2);
plot(to,y,'g','Linewidth',2);
plot(tm,xf(2,:),'b','Linewidth',2);
plot(tm,xa(2,:),'r','Linewidth',2);
legend('\bfTrue (unknown) distance','\bfObs estimate','\bfForecast estimate','\bfKalman estimate','fontsize',18);
xlabel('\bfTime (seconds)','fontsize',18);
ylabel('\bfDistance to coast','fontsize',18);
set(gca,'fontsize',18);
%Statistic
for iobs=[2 5 10 15];
imod=find(tm==to(iobs));
fig=figure;
xtest=linspace(-10*sigx,10*sigx,1e4);
hold on
grid on
box on
set(gca,'fontsize',18);
xlabel('\bfDistance to coast','fontsize',18);
ylabel('\bfProbability density','fontsize',18);
title(['\bfPDFs at time = ',num2str(tm(imod))],'fontsize',18);
plot(xtest,1/(sqrt(2*pi*Pf(2,2,imod)^2))*exp(-0.5*(xtest-x(2,imod)).^2/Pf(2,2,imod)^2),'Linewidth',2);		%This is the pdf of the forecast
plot(xtest,1/(sqrt(2*pi*R^2))*exp(-0.5*(xtest-x(2,imod)).^2/R^2),'g','Linewidth',2)					%Observation
plot(xtest,1/(sqrt(2*pi*Pa(2,2,imod)^2))*exp(-0.5*(xtest-x(2,imod)).^2/Pa(2,2,imod)^2),'r','Linewidth',2);	%Pdf of analysis
plot(x(2,imod),0,'k.','Markersize',45);
plot(xf(2,imod),0,'b.','Markersize',45);
plot(y(iobs),0,'g.','Markersize',45);
plot(xa(2,imod),0,'r.','Markersize',45);
legend('Forecast PDF','Obs PDF','Analysis PDF','True state','Forecast','Obs','Analysis');
axis([x(2,imod)-3*sigx x(2,imod)+3*sigx 0 2])
end
