kalman1.m

kalman filter code by Wilson Lau

%%EE368A Project -- Road Extraction
%%1st Kalman Filtering Algorithm: Profile Matching + Kalman Filter
%%Written by Tun-Yu Chiang, May 2001

%%read file
clear all;
close all;
pic=imread('test4.tif','tif');
pic=im2double(pic)*255; 

pic3=pic(:,:,3); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%																  %%	
%%		SET-UPS												  %%
%%																  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%Initialize
%seedr=50;
%seedc=969;
%seedphi=-pi*40/180; 
seedr=901;
seedc=276;
seedphi=pi*(180-(90-52.125))/180;
width=14;
seedphidot=0;
phidot=seedphidot;
%width=18; 
dt=1;
x=zeros(6,ceil(size(pic3,1)/dt)*1.5);
xt=zeros(2,ceil(size(pic3,1)/dt)*1.5);
zt=zeros(2,ceil(size(pic3,1)/dt)*1.5);
road=zeros(2,width,1046);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%definition of state vector x																 %%
%%state x = [r c phi phidot cosphi sinphi] 												 %%	
%%		  		 r=row position																	 %%
%%		  		 c=column position																 %%
%%				 phi=direction(rad)																 %%
%%				 phidot=change of direction (curvature), assumed to be constant	 %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%give seed point value																			 
x(:,1)=[seedr;seedc;seedphi;seedphidot;cos(seedphi);sin(seedphi)];

%%xpre=x(t)
%%xad_t=x(t+dt)|t  : time-update of x
xpre=x(:,1);
xt(1:2,1)=xpre(1:2);
zt(1:2,1)=xpre(1:2);
xad_t=zeros(size(xpre));
%%give intial covariance matrix of the state vector
P=zeros(6,6);  %%assumption. Can change later

%%give (estimated) system noise covariance matrix Q, assuming white guassian noise
%%this is not quite correct due to cos(phi)&sin(phi)		
Q=[0.05 0 0 0 0 0;0 0.05 0 0 0 0;0 0 1/4000 0 0 0;0 0 0 1/4000 0 0;0 0 0 0 0 0;0 0 0 0 0 0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
%		give first road profile         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%cut the first piece
phi=seedphi;
for i=1:18
   rtestr=round(seedr-i*sin(phi+dt*phidot));
   rtestc=round(seedc+i*cos(phi+dt*phidot));
   rightwindow(i)=pic3(rtestr,rtestc);
   rightzposition(1,i)=rtestr;
   rightzposition(2,i)=rtestc;
   ltestr=round(seedr+i*sin(phi+dt*phidot));
   ltestc=round(seedc-i*cos(phi+dt*phidot));
   leftwindow(i)=pic3(ltestr,ltestc);			
   leftzposition(1,i)=ltestr;
   leftzposition(2,i)=ltestc;
end;  
leftwindow=fliplr(leftwindow);
leftzposition=fliplr(leftzposition);
testx=pic3(seedr,seedc);
window=[leftwindow,testx',rightwindow];
roadprofile=window(18+1-12:18+12);
zposition=[leftzposition,[seedr seedc]',rightzposition];
road(:,:,1)=zposition(:,18+1-width/2:18+width/2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%state transition matrix A
%%time update relation:
%%x(t+dt)|t=r(t)+dt*cos(phi(t)+phidot/2*dt) = 1	0	0	0	cos(phidot/2*dt)dt	-sin(phidot/2*dt)dt  * x(t)
%%			   c(t)+dt*cos(phi(t)+phidot/2*dt) = 0	1	0	0	sin(phidot/2*dt)dt	 cos(phidot/2*dt)dt
%%			   phi(t)+phidot*dt					  = 0	0	1	dt				0							0
%%			   phidot								  = 0	0	0	0				0							1
%%			   cos(phi(t)+phidot*dt)			  = 0	0	0	0  cos(phidot*dt)			-sin(phidot*dt)
%%			   sin(phi(t)+phidot*dt)			  = 0	0	0	0	sin(phidot*dt)			cos(phidot*dt)
%%
%%phi is defined as the angle deviate from perpendicular line, and '+' means counterclockwise
%%note that in the above expression I used
%%cos(a+b)=cos(a)cos(b)-sin(a)sin(b)
%%sin(a+b)=sin(a)cos(b)+sin(a)cos(b)
%%%and the fact that phidot and dt are constants
%%
%%Notice that: A is a constant matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=[1 0 0 0 cos(phidot/2*dt)*dt -sin(phidot/2*dt)*dt; ...
      0 1 0 0 sin(phidot/2*dt)*dt cos(phidot/2*dt)*dt; ...
      0 0 1 dt 0 0; ...
      0 0 0 1 0 0; ...
      0 0 0 0 cos(phidot*dt) -sin(phidot*dt); ...
      0 0 0 0 sin(phidot*dt) cos(phidot*dt)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%z(t+dt) = r(t+dt) = r(t+dt)|t - s*sin(phi(t+dt|t))						%%	 	
%%				c(t+dt) = c(t+dt)|t + s*cos(phi(t+dt|t))						%%
%%																							%%
%% relation btw z and x: z=Hx														%%
%%																							%%
%% z=[1 0 0 0 0 0]x where H is the constant observation matrix			%%
%%	  [0 1 0 0 0 0]																	%%
%%																							%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
H=[1 0 0 0 0 0;0 1 0 0 0 0];  



for j=2:1200
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%																  %%	
   %%		UPDATES												  %%
   %%																  %%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%update time preduction
   xad_t=A*xpre;
   xt(1:2,j)=round(xad_t(1:2));
   
   phi=xad_t(3);
   
   %%update covariance matrix P of the predicted state vector
   %%
   %% P(t+dt) = A(t)P(t)A(t)' + Q    <-- question here, how to give Q?
   %% P is not a constant matrix, should be update each time
   %%
   Ppre=P;
   P=A*Ppre*A'+Q;
   
   %%observation: To find z(t+dt)
   %%
   %%estimated shift s is obtained from minimising the square sum of the grey value diff btw the profiles
   %%s is perpendicular to the direction at time t+dt, and its sign is
   %%defined to be positive = to the right of x(t+dt)|t
   %%					 negative = to the left  of x(t+dt)|t
   %%
   rightwindow=zeros(1,18);
   leftwindow=zeros(1,18);
   rightzposition=zeros(2,18);
   leftzposition=zeros(2,18);
   %%prepare the data to compare, use a window in perpendicular direction to phi
   for i=1:18
      rtestr=round(xad_t(1)-i*sin(phi+dt*phidot));
      rtestc=round(xad_t(2)+i*cos(phi+dt*phidot));
      rightwindow(i)=pic3(rtestr,rtestc);
      rightzposition(1,i)=rtestr;
      rightzposition(2,i)=rtestc;
      ltestr=round(xad_t(1)+i*sin(phi+dt*phidot));
      ltestc=round(xad_t(2)-i*cos(phi+dt*phidot));
      leftwindow(i)=pic3(ltestr,ltestc);			
      leftzposition(1,i)=ltestr;
      leftzposition(2,i)=ltestc;
   end;
   leftwindow=fliplr(leftwindow);
   leftzposition=fliplr(leftzposition);  
   testx=pic3(xad_t(1),xad_t(2));
   window=[leftwindow,testx',rightwindow];
   zposition=[leftzposition,xad_t(1:2),rightzposition];
   mse=zeros(size(window));
   testwindow=zeros(1,24);
   %%computing the MSE
   for i=12+1:2*18-12+1
      testwindow=window(i-12:i+11);
      mse(i)=sum((testwindow-roadprofile).^2);
   end;
   [Y,I]=min(mse(13:25));
   testwindow=window(I+12-12:I+12+11);
   m=corrcoef(testwindow,roadprofile);
   if abs(m(1,2))>0.4
      z=zposition(:,I+12);
      roadprofile=testwindow;
      road(:,:,j)=zposition(:,I+12-width/2:I+11+width/2);
   else
      z=xad_t(1:2);
      roadprofile=window(18+1-12:18+12);
      road(:,:,j)=zposition(:,18+1-width/2:18+width/2);
   end;
   
   %%covariance of z
   %%
   %% R(t+dt)= var(s) * [ sin^2(phi(t+dt))					sin(phi(t+dt))cos(phi(t+dt))]
   %%							[ cos(phi(t+dt))sin(phi(t+dt))	cos^2(phi(t+dt))				 ]	
   %%
   sigmaS=0.25;  %%heuristic, assume max error=1, 1/4=0.25
   R=sigmaS*[(sin(xad_t(3)))^2,sin(xad_t(3))*cos(xad_t(3)); ...
         cos(xad_t(3))*sin(xad_t(3)),(cos(xad_t(3)))^2];
   
   %%Kalman filter K
   %%
   %%	K(t+dt) = P(t+dt)|t H' (R(t+dt) + H P(t+dt)|t H')^-1
   %%
   K = P*H'*(R+H*P*H')^-1;
   
   %%Filtering
   %%
   %%	x(t+dt) = x(t+dt)|t + K(t+dt)*(z(t+dt)-H*x(t+dt)|t)
   xad=xad_t+K*(z-H*xad_t);
   x(1,j)=round(xad(1));
   x(2,j)=round(xad(2));
   x(3:6,j)=xad(3:6);
   xpre=xad; 
   zt(1:2,j)=round(z);
   if (x(1,j)<15 | x(2,j)<15 | x(1,j)>(size(pic3,1)-15)|x(2,j)>(size(pic3,2)-15))
      break;
   end;
end;



for k=1:j-1
   for q=1:width
      dr=road(1,q,k);
      dc=road(2,q,k);
      pic(dr,dc,1)=255;
      pic(dr,dc,2)=0;
      pic(dr,dc,3)=0;
   end;   
   pic(x(1,k),x(2,k),1)=255;
   pic(x(1,k),x(2,k),2)=0;
   pic(x(1,k),x(2,k),3)=0;
end;

figure;
pic(:,:,1)=pic(:,:,1)./max(max(pic(:,:,1)));
pic(:,:,2)=pic(:,:,2)./max(max(pic(:,:,2)));
pic(:,:,3)=pic(:,:,3)./max(max(pic(:,:,3)));
imagesc(pic);