The function named sample_motion_model_velocity could be virtually anything, but I used one from chapter 5 (Table 5.4):
Watch it though, be sure that the values you choose for a(1) to a(6) do not underestimate the error on the mesurement SigmaZ!
Otherwise, this could decimate almost all the samples you have when you will weighting (because v and w could become too big).
Also, the function called landmark_model_known_correspondence can also be anything and I used the algorithm from Table 6.4:
function[ p ] =landmark_model_known_correspondence(x, z, c, m, sigmaZ)q=zeros(1,numel(c));fori=1:1:numel(c)j=c(i);r_=sqrt((m(1,j)-x(1)).^2+(m(2,j)-x(2)).^2);b_=wrapToPi(atan2(m(2,j)-x(2),m(1,j)-x(1))-x(3));q(i)=prob(z(1,j)-r_,sigmaZ(1))*prob(wrapToPi(z(2,j)-b_),sigmaZ(2));%If sigmaZ is underestimated (see parameter a(1) to a(6) from the sampling model)endp=prod(q);endfunction[p] =prob(x,sigma)p=normpdf(x,0,sigma)/normpdf(0,0,sigma);%Put p between 0 and 1, because small sigmas could put p way over 1end
Of course, many parameters can be tuned up in order to optimize the algorithm.