Add files via upload

shared/Distance.m

@@ -0,0 +1,8 @@
+function d=Distance(x1,x2);   %calculate the distance between two points, macshen
+dimen=length(x1);
+d2=0;
+for k=1:dimen
+    d2=d2+(x1(k)-x2(k))^2;
+end
+d=sqrt(d2);
+end

shared/Distance_to_road.m

@@ -0,0 +1,15 @@
+function d=Distance_to_road(x,grid_size) %calculate the distance to road centers by interpolation from the pre-calculated values
+ global dis;
+ global grad;
+nx=floor(x/grid_size);
+size_d=size(dis);
+if nx(1)<0|nx(2)<0|nx(1)>size_d(1)-1|nx(2)>size_d(2)-1   %the considered point locates outside the grid
+    d=100;   %assign a large distance
+else
+X=nx(1)*grid_size;
+Y=nx(2)*grid_size;
+delta_x=x(1)-X;
+delta_y=x(2)-Y;
+d=dis(nx(1)+1,nx(2)+1)+sum(grad{nx(1)+1,nx(2)+1}.*[delta_x,delta_y]);
+end
+end

shared/cal_weight.m

@@ -0,0 +1,10 @@
+function weight=cal_weight(pf,estenu,P,sigma_map2,lane_width,vehicle_width,grid_size)  %called by main program to calculate weight for particle filter
+point=pf(1,1:2)+estenu(1,1:2);  %position of the hypothetical particle position
+sigma_w2=trace(P)+sigma_map2;
+d=Distance_to_road(point,grid_size);
+if d<=lane_width-vehicle_width/2
+    weight=pf(1,3);  %weight does not change
+else %outside the road
+    weight=pf(1,3)*exp(-(d-lane_width+vehicle_width/2)^2/2/sigma_w2);  %Here it should be considered how to calculate the weight in a better way
+end
+end

shared/generate_geometry.m

@@ -0,0 +1,59 @@
+clear all
+nodes=[50,0;
+    50,100;
+    0,50;
+    100,50];   %x and y coordinates of nodes representing the roads, the length of nodes can be a very large number
+Refine_nodes(1:500,1)=50;
+Refine_nodes(1:500,2)=0.2:0.2:100;
+Refine_nodes(501:1000,1)=0.2:0.2:100;
+Refine_nodes(501:1000,2)=50;    %Refine the nodes on the roads for computing the distance of each points on the grid 
+%map to the roads centers
+grid_size=0.5;
+Nx=100/grid_size+1;
+Ny=100/grid_size+1;
+x=(0:Nx-1)*grid_size;
+y=(0:Ny-1)*grid_size;   %get the x,y coordinates of the grid mesh
+N_nodes=size(Refine_nodes,1);  %get the total number of road nodes
+for i=1:Nx
+    for j=1:Ny
+d_smallest=Distance([x(i),y(j)],Refine_nodes(1,:));
+for k=2:N_nodes
+    d_temp=Distance([x(i),y(j)],Refine_nodes(k,:));
+    if d_temp<d_smallest
+        d_smallest=d_temp;
+    end  %end (if d_temp<d_smallest)
+end  %end (for k=2:N_nodes)
+distance(i,j)=d_smallest;   %calculated the distance of i,j grid points to road centers
+    end %end (for j=1:Ny)
+end %end (for i=1:Nx)
+
+grad_dis=cell(Nx,Ny);
+for i=2:Nx-1
+    for j=2:Ny-1
+        grad_dis{i,j}=[distance(i+1,j)-distance(i-1,j),distance(i,j+1)-distance(i,j-1)]/(2*grid_size);
+    end
+end
+for i=1:Nx  %there is a small bug which does not matter
+    grad_dis{i,1}=grad_dis{i,2};
+    grad_dis{i,Ny}=grad_dis{i,Ny-1};
+end
+for j=1:Ny
+    grad_dis{1,j}=grad_dis{2,j};
+    grad_dis{Nx,j}=grad_dis{Nx-1,j};
+end
+% figure(1)
+% hold
+% for i=1:Nx
+%     for j=1:Ny
+%         if distance(i,j)>10
+%             plot(x(i),y(j),'o')
+%         else
+%             plot(x(i),y(j),'*')
+%         end
+%     end
+% end
+
+
+
+
+

shared/pf_to_Gaussian.m

@@ -0,0 +1,11 @@
+function [mu,cov]=pf_to_Gaussian(pf);
+N=size(pf,1);
+mu=[mean(pf(:,1)),mean(pf(:,2))];
+X_tilda=[pf(:,1)-mu(1)];
+Y_tilda=[pf(:,2)-mu(2)];
+sigma_x=1/(N-1)*sum(X_tilda.^2);
+sigma_y=1/(N-1)*sum(X_tilda.^2);
+sigma_xy=1/(N-1)*sum(X_tilda.*Y_tilda);
+cov=[sigma_x,sigma_xy;
+    sigma_xy,sigma_y];
+end

shared/resample.m

@@ -0,0 +1,23 @@
+function [nS] = resample(St)  
+St(:,3)=St(:,3)/sum(St(:,3));  % first normalize the weight
+N=length(St(:,1));
+nS=[];
+m=0;
+c(1)=St(1,3);
+for i=2:N
+    c(i)=c(i-1)+St(i,3);
+end
+
+u(1)=1/N*rand(1);
+i=1;
+for j=1:N
+    while(u(j)>c(i))
+
+        i=i+1;
+         end
+                nS(m+1,1:2)=St(i,1:2);
+        nS(m+1,3)=1/N;
+        m=m+1;
+        u(j+1)=u(j)+1/N;
+end
+end