|
wolffd@0
|
1 % Plot how precision matrix changes over time for KF solution
|
|
wolffd@0
|
2
|
|
wolffd@0
|
3 seed = 0;
|
|
wolffd@0
|
4 rand('state', seed);
|
|
wolffd@0
|
5 randn('state', seed);
|
|
wolffd@0
|
6
|
|
wolffd@0
|
7 [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
|
|
wolffd@0
|
8 true_landmark_pos, true_robot_pos, true_data_assoc, ...
|
|
wolffd@0
|
9 obs_rel_pos, ctrl_signal] = mk_linear_slam(...
|
|
wolffd@0
|
10 'nlandmarks', 6, 'T', 12, 'ctrl', 'leftright', 'data-assoc', 'cycle');
|
|
wolffd@0
|
11
|
|
wolffd@0
|
12 figure(1); clf
|
|
wolffd@0
|
13 hold on
|
|
wolffd@0
|
14 for i=1:nlandmarks
|
|
wolffd@0
|
15 %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
|
|
wolffd@0
|
16 plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
|
|
wolffd@0
|
17 end
|
|
wolffd@0
|
18 hold off
|
|
wolffd@0
|
19
|
|
wolffd@0
|
20
|
|
wolffd@0
|
21 [x, V] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
|
|
wolffd@0
|
22 'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
|
|
wolffd@0
|
23
|
|
wolffd@0
|
24 est_robot_pos = x(robot_block, :);
|
|
wolffd@0
|
25 est_robot_pos_cov = V(robot_block, robot_block, :);
|
|
wolffd@0
|
26
|
|
wolffd@0
|
27 for i=1:nlandmarks
|
|
wolffd@0
|
28 bi = landmark_block(:,i);
|
|
wolffd@0
|
29 est_landmark_pos(:,i) = x(bi, T);
|
|
wolffd@0
|
30 est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
|
|
wolffd@0
|
31 end
|
|
wolffd@0
|
32
|
|
wolffd@0
|
33
|
|
wolffd@0
|
34 if 0
|
|
wolffd@0
|
35 figure(1); hold on
|
|
wolffd@0
|
36 for i=1:nlandmarks
|
|
wolffd@0
|
37 h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));
|
|
wolffd@0
|
38 set(h, 'color', 'r')
|
|
wolffd@0
|
39 end
|
|
wolffd@0
|
40 hold off
|
|
wolffd@0
|
41
|
|
wolffd@0
|
42 hold on
|
|
wolffd@0
|
43 for t=1:T
|
|
wolffd@0
|
44 h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t));
|
|
wolffd@0
|
45 set(h,'color','r')
|
|
wolffd@0
|
46 h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t));
|
|
wolffd@0
|
47 set(h,'color','r')
|
|
wolffd@0
|
48 end
|
|
wolffd@0
|
49 hold off
|
|
wolffd@0
|
50 end
|
|
wolffd@0
|
51
|
|
wolffd@0
|
52
|
|
wolffd@0
|
53 P = zeros(size(V));
|
|
wolffd@0
|
54 for t=1:T
|
|
wolffd@0
|
55 P(:,:,t) = inv(V(:,:,t));
|
|
wolffd@0
|
56 end
|
|
wolffd@0
|
57
|
|
wolffd@0
|
58 if 0
|
|
wolffd@0
|
59 figure(2)
|
|
wolffd@0
|
60 for t=1:T
|
|
wolffd@0
|
61 subplot(T/2,2,t)
|
|
wolffd@0
|
62 imagesc(P(1:2:end,1:2:end, t))
|
|
wolffd@0
|
63 colorbar
|
|
wolffd@0
|
64 end
|
|
wolffd@0
|
65 else
|
|
wolffd@0
|
66 figure(2)
|
|
wolffd@0
|
67 for t=1:T
|
|
wolffd@0
|
68 subplot(T/2,2,t)
|
|
wolffd@0
|
69 imagesc(V(1:2:end,1:2:end, t))
|
|
wolffd@0
|
70 colorbar
|
|
wolffd@0
|
71 end
|
|
wolffd@0
|
72 end
|
|
wolffd@0
|
73
|
|
wolffd@0
|
74 % marginalize out robot position and then check structure
|
|
wolffd@0
|
75 bi = landmark_block(:);
|
|
wolffd@0
|
76 V = V(bi,bi,T);
|
|
wolffd@0
|
77 P = inv(V);
|
|
wolffd@0
|
78 P(1:2:end,1:2:end)
|
