wolffd@0: % Plot how precision matrix changes over time for KF solution
wolffd@0: 
wolffd@0: seed = 0;
wolffd@0: rand('state', seed);
wolffd@0: randn('state', seed);
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', 6, 'T', 12, 'ctrl', 'leftright', 'data-assoc', 'cycle');
wolffd@0: 
wolffd@0: figure(1); clf
wolffd@0: hold on
wolffd@0: for i=1:nlandmarks
wolffd@0:   %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
wolffd@0:   plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
wolffd@0: end
wolffd@0: hold off
wolffd@0: 
wolffd@0: 
wolffd@0: [x, V] = 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: est_robot_pos = x(robot_block, :);
wolffd@0: est_robot_pos_cov = V(robot_block, robot_block, :);
wolffd@0: 
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   est_landmark_pos(:,i) = x(bi, T);
wolffd@0:   est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: if 0
wolffd@0: figure(1); hold on
wolffd@0: for i=1:nlandmarks
wolffd@0:   h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));
wolffd@0:   set(h, 'color', 'r')
wolffd@0: end
wolffd@0: hold off
wolffd@0: 
wolffd@0: hold on
wolffd@0: for t=1:T
wolffd@0:   h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t));
wolffd@0:   set(h,'color','r')
wolffd@0:   h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t));
wolffd@0:   set(h,'color','r')
wolffd@0: end
wolffd@0: hold off
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: P = zeros(size(V));
wolffd@0: for t=1:T
wolffd@0:   P(:,:,t) = inv(V(:,:,t));
wolffd@0: end
wolffd@0: 
wolffd@0: if 0
wolffd@0:   figure(2)
wolffd@0:   for t=1:T
wolffd@0:     subplot(T/2,2,t)
wolffd@0:     imagesc(P(1:2:end,1:2:end, t))
wolffd@0:     colorbar
wolffd@0:   end
wolffd@0: else
wolffd@0:   figure(2)
wolffd@0:   for t=1:T
wolffd@0:     subplot(T/2,2,t)
wolffd@0:     imagesc(V(1:2:end,1:2:end, t))
wolffd@0:     colorbar
wolffd@0:   end
wolffd@0: end
wolffd@0: 
wolffd@0: % marginalize out robot position and then check structure
wolffd@0: bi = landmark_block(:);
wolffd@0: V = V(bi,bi,T); 
wolffd@0: P = inv(V);
wolffd@0: P(1:2:end,1:2:end)