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