Daniel@0: % Plot how precision matrix changes over time for KF solution Daniel@0: Daniel@0: seed = 0; Daniel@0: rand('state', seed); Daniel@0: randn('state', seed); Daniel@0: Daniel@0: [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,... Daniel@0: true_landmark_pos, true_robot_pos, true_data_assoc, ... Daniel@0: obs_rel_pos, ctrl_signal] = mk_linear_slam(... Daniel@0: 'nlandmarks', 6, 'T', 12, 'ctrl', 'leftright', 'data-assoc', 'cycle'); Daniel@0: Daniel@0: figure(1); clf Daniel@0: hold on Daniel@0: for i=1:nlandmarks Daniel@0: %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i)); Daniel@0: plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*') Daniel@0: end Daniel@0: hold off Daniel@0: Daniel@0: Daniel@0: [x, V] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ... Daniel@0: 'model', true_data_assoc, 'u', ctrl_signal, 'B', B); Daniel@0: Daniel@0: est_robot_pos = x(robot_block, :); Daniel@0: est_robot_pos_cov = V(robot_block, robot_block, :); Daniel@0: Daniel@0: for i=1:nlandmarks Daniel@0: bi = landmark_block(:,i); Daniel@0: est_landmark_pos(:,i) = x(bi, T); Daniel@0: est_landmark_pos_cov(:,:,i) = V(bi, bi, T); Daniel@0: end Daniel@0: Daniel@0: Daniel@0: if 0 Daniel@0: figure(1); hold on Daniel@0: for i=1:nlandmarks Daniel@0: h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i)); Daniel@0: set(h, 'color', 'r') Daniel@0: end Daniel@0: hold off Daniel@0: Daniel@0: hold on Daniel@0: for t=1:T Daniel@0: h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t)); Daniel@0: set(h,'color','r') Daniel@0: h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t)); Daniel@0: set(h,'color','r') Daniel@0: end Daniel@0: hold off Daniel@0: end Daniel@0: Daniel@0: Daniel@0: P = zeros(size(V)); Daniel@0: for t=1:T Daniel@0: P(:,:,t) = inv(V(:,:,t)); Daniel@0: end Daniel@0: Daniel@0: if 0 Daniel@0: figure(2) Daniel@0: for t=1:T Daniel@0: subplot(T/2,2,t) Daniel@0: imagesc(P(1:2:end,1:2:end, t)) Daniel@0: colorbar Daniel@0: end Daniel@0: else Daniel@0: figure(2) Daniel@0: for t=1:T Daniel@0: subplot(T/2,2,t) Daniel@0: imagesc(V(1:2:end,1:2:end, t)) Daniel@0: colorbar Daniel@0: end Daniel@0: end Daniel@0: Daniel@0: % marginalize out robot position and then check structure Daniel@0: bi = landmark_block(:); Daniel@0: V = V(bi,bi,T); Daniel@0: P = inv(V); Daniel@0: P(1:2:end,1:2:end)