comparison toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/slam_partial_kf.m @ 0:e9a9cd732c1e tip

first hg version after svn

+% See how well partial Kalman filter updates work
+
+seed = 0;
+rand('state', seed);
+randn('state', seed);
+nlandmarks = 6;
+T = 12;
+
+[A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
+	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
+	  obs_rel_pos, ctrl_signal] = mk_linear_slam(...
+	      'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle');
+
+% exact
+[xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+
+% approx
+%k = nlandmarks-1; % exact
+k = 3;
+ndx = {};
+for t=1:T
+  landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
+  tmp = [landmark_block(:, landmarks) robot_block'];
+  ndx{t} = tmp(:);
+end
+
+[xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+			 'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
+		       'ndx', ndx);
+
+
+
+nrows = 10;
+stepsize = T/(2*nrows);
+ts = 1:stepsize:T;
+
+if 1 % plot
+  
+clim = [0 max(max(Va(:,:,end)))];
+
+figure(2)
+if 0
+  imagesc(Ve(1:2:end,1:2:end, T))
+  clim = get(gca,'clim');
+else
+  i = 1;
+  for t=ts(:)'
+    subplot(nrows,2,i)
+    i = i + 1;
+    imagesc(Ve(1:2:end,1:2:end, t))
+    set(gca, 'clim', clim)
+    colorbar
+  end
+end
+suptitle('exact')
+
+
+figure(3)
+if 0
+  imagesc(Va(1:2:end,1:2:end, T))
+  set(gca,'clim', clim)
+else
+  i = 1;
+  for t=ts(:)'
+    subplot(nrows,2,i)
+    i = i+1;
+    imagesc(Va(1:2:end,1:2:end, t))
+    set(gca, 'clim', clim)
+    colorbar
+  end
+end
+suptitle('approx')
+
+
+figure(4)
+i = 1;
+for t=ts(:)'
+  subplot(nrows,2,i)
+  i = i+1;
+  Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
+  imagesc(Vd)
+  set(gca, 'clim', clim)
+  colorbar
+end
+suptitle('diff')
+
+end % all plot
+
+
+for t=1:T
+  %err(t)=rms(xa(:,t), xe(:,t));
+  err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
+end
+figure(5);plot(err)
+title('rms mean pos')
+
+
+for t=1:T
+  i = 1:2*nlandmarks;
+  denom = Ve(i,i,t) + (Ve(i,i,t)==0);
+  Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
+  Verr(t) = max(Vd(:));
+end
+figure(6); plot(Verr)
+title('max relative Verr')