if CDiff < ODiff
dt = CDiff;
for i=1:NPARTICLES
particles(i)= predict (particles(i), V,G,Qe, WHEELBASE,dt, SWITCH_PREDICT_NOISE);
end
cIndex = cIndex + 1;
else
dt = ODiff;
z1 = Ob(:,OIndex);
z = cell2mat (z1);
z(1) = z(1)/100;
for i=1:NPARTICLES
particles(i)= predict (particles(i), V,G,Qe, WHEELBASE,dt, SWITCH_PREDICT_NOISE);
end
for i=1:NPARTICLES
[zf,idf,zn]= data_associate(particles(i),z,R);
if ~isempty(zf)
w= compute_weight(particles(i), zf,idf, R);
if w <= 1e-5
w = 1e-5;
end
particles(i).w= particles(i).w * w;
particles(i)= feature_update(particles(i), zf, idf, R);
end
if ~isempty(zn)
particles(i)= add_feature(particles(i), zn,R);
end
end
particles= resample_particles(particles, NEFFECTIVE, SWITCH_RESAMPLE);
dt2 = timestamp(cIndex+1)-Obser_time(OIndex);
for i=1:NPARTICLES
particles(i)= predict (particles(i), V,G,Qe, WHEELBASE,dt2, SWITCH_PREDICT_NOISE);
end