kalman2.m

kalman filter code by Wilson Lau

%%EE368A Project -- Road Extraction
%%Kalman Algorithm #2
%%Generalized Multi-hypothesis Kalman Filtering
%%writton by Tun-Yu Chiang, May 2001

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

pic3=pic(:,:,3); %chose this as test because it seems has a higher contrast

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%																  %%	
%%		SET-UPS												  %%
%%																  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%give the first seed value
seedr=45;
seedc=969;
seedr2=49;
seedd=18;
seedu=80;

%%give first window
%window size
N1=7;
N2=31;
window=pic3(seedr:seedr+(N1-1),seedc-(N2-1)/2:seedc+(N2-1)/2);

%%give output vector/matrices
x=zeros(4,1);
center=zeros(2,2000);
bound1=zeros(2,2000);
bound2=zeros(2,2000);
%width=zeros(1,2000);
%intensity=zeros(1,2000);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%definition of state vector x						  %%
%%state x(k) = [x1(k) x2(k) d(k) u(k) ]			  %%
%%																  %%	
%%																  %%	
%%		  		 x1=current road center					  %%
%%		  		 x2=previous road center				  %%
%%				 d =road width								  %%
%%				 u =mean road intensity					  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%give initial covariance matrix of the state vector
P=zeros(4,4);  %%assumption. Can change later

%%give (estimated) system noise covariance matrix Q, assuming white guassian noise
Q=[0.05 0 0 0 ;0 0.05 0 0 ;0 0 1/4000 0 ;0 0 0 1/4000];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%state transition matrix A														%%
%%																							%%
%%Notice that: A is a constant matrix											%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=[2 -1 0 0;1 0 0 0;0 0 1 0;0 0 0 1];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%observation matrices H															%%
%%																							%%
%%cases: two-boundary pionts measurement H2									%%
%%			one-boundary point(left) measurement H1_1							%%
%%			one-boundary point(right) measurement H1_2						%%
%%			no-boundary point(right) measurement H0							%%
%%																							%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
H2=[1 0 -0.5 0;1 0 0.5 0; 0 0 0 1];
H1_1=[1 0 -0.5 0;0 0 0 1];
H1_2=[1 0 0.5 0;0 0 0 1];
H0=[0 0 0 1];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%																							%%
%%ROAD TRACKING																		%%
%%																							%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%give first state value
%%
xpre=[(N2-1)/2+1 (N2-1)/2 seedd seedu]';
xad_t=zeros(size(xpre));

stop=0;
rowindex=seedr;
colindex=seedc-(N2-1)/2;
zbound1=(colindex-1)+xpre(1)-seedd/2; %%index for these two are absolute matrix index
zbound2=(colindex-1)+xpre(1)+seedd/2;

k=2;%general index for the output values

while (stop==0)
   
   for j=1:N1  %% loop within each window
      
      swflag=0;  
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%%																  				  %%%	
      %%%		UPDATES												  				  %%%
      %%%																  			     %%%
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
      %%update state transition
      xad_t=A*xpre;
      
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%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)
      %%
      %% each time open 2 gates around estimated boundary points
      %% do the observation by taking difference btw every 2 pixel
      %% highlight the detected boundary points
      %% decide which case is the observation
      
      %estimated boundary points
      ebound1=round([1 0 -0.5 0]*xad_t);
      ebound2=round([1 0 0.5 0]*xad_t);   
      %open observatio gates around estimated points
      gate1=window(j,1:ebound1+4)';
      gate2=window(j,ebound2-4:N2)';
      %take difference on the points within each gate, always (closer to road)-(far from road)
      diffmat1=-1*toeplitz([1 zeros(1,size(gate1,1)-2)],[1 -1 zeros(1,size(gate1,1)-2)]);
      diffmat2=toeplitz([1 zeros(1,size(gate2,1)-2)],[1 -1 zeros(1,size(gate2,1)-2)]);
      diff1=diffmat1*gate1;
      diff2=diffmat2*gate2;
      %find the max difference in each gate
      [Y1,I1]=max(diff1);   
      if (Y1>30)			%%left edge found
         zbound1=(colindex-1)+I1;
         swflag=swflag+1;   
      end;
      [Y2,I2]=max(diff2);
      if (Y2>30)			%%right edge found
         zbound2=(colindex-1)+(ebound2-4)+I2;
         swflag=swflag+1;
      end;
      
      switch swflag
      case 0				%%no edges detected
         H=H0;
         z_u=xad_t(4);
         z=z_u;
      case 1				%%left edge detected
         if Y1>30
            H=H1_1;
            z_u=sum(window(j,I1+1:xad_t(1)))/(xad_t(1)-(I1+1)+1);
            z=[I1+1 z_u]';
         else				%%right edge detected
            H=H1_2;
            z_u=sum(window(j,xad_t(1):(ebound2-4+I2-1)))/((ebound2-4+I2-1)-xad_t(1)+1);
            z=[ebound2-4+I2-1 z_u]';
         end;
         case 2				%%both edge detected
            H=H2;
            z_u=sum(window(j,I1+1:ebound2-4+I2-1))/((ebound2-4+I2-1)-(I1+1)+1);
            z=[I1+1 ebound2-4+I2-1 z_u]';
         end;   
         
         %%covariance of z
         %%
         %%arbitrarily injected
         R=0.001*eye(size(H,1));
         
         %%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);
         xpre=xad;
         x(:,k)=xad;
         center(:,k)=[j+rowindex-1,xad(1)+colindex-1]';
         bound1(:,k)=[j+rowindex-1,zbound1]';
         bound2(:,k)=[j+rowindex-1,zbound2]';
         %width(:,k)=;
         
         k=k+1;   
      end;
      %%give next window
      if ((rowindex+N1)>size(pic3,1) | (colindex+N1)>size(pic3,2))
         break;
      end;
      switch swflag
      case 0		%%no edge detected
         rowindex=rowindex+N1-3;
         colindex=(colindex-1)+x(1,k-3)-(N2-1)/2;
         window=pic(rowindex:rowindex+N1-1,colindex-(N2-1)/2:colindex+(N2-1)/2);      
      case 1
         if Y1>30  %%has only left edge, move window to the right 
            rowindex=rowindex+N1-3;
            colindex=bound1(2,k-3)-(N2-1)/2;
            window=pic(rowindex:rowindex+N1-1,colindex:colindex+N2-1);
         else		 %%has only right edge, move window to the left
            rowindex=rowindex+N1-3;
            colindex=bound2(2,k-3)-(N2-1)/2;
            window=pic(rowindex:rowindex+N1-1,colindex:colindex+N2-1); 
         end;
         
      case 2		 %%has both edges, new window center= (N-3)th road center
         rowindex=rowindex+N1-3;
         colindex=(colindex-1)+x(1,k-3)-(N2-1)/2;
         window=pic(rowindex:rowindex+N1-1,colindex:colindex+N2-1);    
      end;
   end;
   
   %%DISPLAY
   
   for a=2:k-1
      r1=bound1(1,a);
      c1=bound1(2,a);
      r2=bound2(1,a);
      c2=bound2(2,a);
         pic(r1,c1,1)=255;
         pic(r1,c1,2)=0;
         pic(r1,c1,3)=0;
         pic(r2,c2,1)=255;
         pic(r2,c2,2)=0;
         pic(r2,c2,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);