fixed bug when sampling trajectory in the particle filter

Former-commit-id: d8b2b16 [formerly d8b2b16 [formerly ccb4307]]
Former-commit-id: ca79fc6e8b1fcc2af36a9fa2faa72a5b0512fa87
Former-commit-id: b1e548c

Author: compops

matlab/sm.m

@@ -62,7 +62,6 @@
     % Resample ( multinomial )
     %=========================================================
     idx = randsample( nPart, nPart, true, w(:,tt-1) ); 
-    idx = idx( randperm( nPart ) );
 
     %=========================================================
     % Propagate

matlab/sm_sv.m

@@ -60,7 +60,6 @@
     % Resample ( multinomial )
     %=========================================================
     idx = randsample( N, N, true, w(:,tt-1) ); 
-    idx = idx( randperm( N ) );
 
     % Resample the ancestory linage
     a(:,1:tt-1) = a(idx,1:tt-1);
@@ -91,9 +90,11 @@
   end
 
   % Sample the state estimate using the weights at tt=T
+  xhatf = zeros( 1, T+1);
   nIdx  = randsample( N, 1, true, w(:,T) );
-  indices = sub2ind(size(p), a(nIdx,:), 1:(T+1));
-  xhatf = p( indices );
+  for tt = 1:(T+1)
+    xhatf(tt) = p( a(nIdx,tt), tt );
+  end
 end
 
 % Helper for computing the logarithm of the Gaussian density
@@ -105,4 +106,4 @@
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % End of file
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%