|
wolffd@0
|
1 % See how well partial Kalman filter updates work
|
|
wolffd@0
|
2
|
|
wolffd@0
|
3 seed = 0;
|
|
wolffd@0
|
4 rand('state', seed);
|
|
wolffd@0
|
5 randn('state', seed);
|
|
wolffd@0
|
6 nlandmarks = 6;
|
|
wolffd@0
|
7 T = 12;
|
|
wolffd@0
|
8
|
|
wolffd@0
|
9 [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
|
|
wolffd@0
|
10 true_landmark_pos, true_robot_pos, true_data_assoc, ...
|
|
wolffd@0
|
11 obs_rel_pos, ctrl_signal] = mk_linear_slam(...
|
|
wolffd@0
|
12 'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle');
|
|
wolffd@0
|
13
|
|
wolffd@0
|
14 % exact
|
|
wolffd@0
|
15 [xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
|
|
wolffd@0
|
16 'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
|
|
wolffd@0
|
17
|
|
wolffd@0
|
18
|
|
wolffd@0
|
19 % approx
|
|
wolffd@0
|
20 %k = nlandmarks-1; % exact
|
|
wolffd@0
|
21 k = 3;
|
|
wolffd@0
|
22 ndx = {};
|
|
wolffd@0
|
23 for t=1:T
|
|
wolffd@0
|
24 landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
|
|
wolffd@0
|
25 tmp = [landmark_block(:, landmarks) robot_block'];
|
|
wolffd@0
|
26 ndx{t} = tmp(:);
|
|
wolffd@0
|
27 end
|
|
wolffd@0
|
28
|
|
wolffd@0
|
29 [xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
|
|
wolffd@0
|
30 'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
|
|
wolffd@0
|
31 'ndx', ndx);
|
|
wolffd@0
|
32
|
|
wolffd@0
|
33
|
|
wolffd@0
|
34
|
|
wolffd@0
|
35 nrows = 10;
|
|
wolffd@0
|
36 stepsize = T/(2*nrows);
|
|
wolffd@0
|
37 ts = 1:stepsize:T;
|
|
wolffd@0
|
38
|
|
wolffd@0
|
39 if 1 % plot
|
|
wolffd@0
|
40
|
|
wolffd@0
|
41 clim = [0 max(max(Va(:,:,end)))];
|
|
wolffd@0
|
42
|
|
wolffd@0
|
43 figure(2)
|
|
wolffd@0
|
44 if 0
|
|
wolffd@0
|
45 imagesc(Ve(1:2:end,1:2:end, T))
|
|
wolffd@0
|
46 clim = get(gca,'clim');
|
|
wolffd@0
|
47 else
|
|
wolffd@0
|
48 i = 1;
|
|
wolffd@0
|
49 for t=ts(:)'
|
|
wolffd@0
|
50 subplot(nrows,2,i)
|
|
wolffd@0
|
51 i = i + 1;
|
|
wolffd@0
|
52 imagesc(Ve(1:2:end,1:2:end, t))
|
|
wolffd@0
|
53 set(gca, 'clim', clim)
|
|
wolffd@0
|
54 colorbar
|
|
wolffd@0
|
55 end
|
|
wolffd@0
|
56 end
|
|
wolffd@0
|
57 suptitle('exact')
|
|
wolffd@0
|
58
|
|
wolffd@0
|
59
|
|
wolffd@0
|
60 figure(3)
|
|
wolffd@0
|
61 if 0
|
|
wolffd@0
|
62 imagesc(Va(1:2:end,1:2:end, T))
|
|
wolffd@0
|
63 set(gca,'clim', clim)
|
|
wolffd@0
|
64 else
|
|
wolffd@0
|
65 i = 1;
|
|
wolffd@0
|
66 for t=ts(:)'
|
|
wolffd@0
|
67 subplot(nrows,2,i)
|
|
wolffd@0
|
68 i = i+1;
|
|
wolffd@0
|
69 imagesc(Va(1:2:end,1:2:end, t))
|
|
wolffd@0
|
70 set(gca, 'clim', clim)
|
|
wolffd@0
|
71 colorbar
|
|
wolffd@0
|
72 end
|
|
wolffd@0
|
73 end
|
|
wolffd@0
|
74 suptitle('approx')
|
|
wolffd@0
|
75
|
|
wolffd@0
|
76
|
|
wolffd@0
|
77 figure(4)
|
|
wolffd@0
|
78 i = 1;
|
|
wolffd@0
|
79 for t=ts(:)'
|
|
wolffd@0
|
80 subplot(nrows,2,i)
|
|
wolffd@0
|
81 i = i+1;
|
|
wolffd@0
|
82 Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
|
|
wolffd@0
|
83 imagesc(Vd)
|
|
wolffd@0
|
84 set(gca, 'clim', clim)
|
|
wolffd@0
|
85 colorbar
|
|
wolffd@0
|
86 end
|
|
wolffd@0
|
87 suptitle('diff')
|
|
wolffd@0
|
88
|
|
wolffd@0
|
89 end % all plot
|
|
wolffd@0
|
90
|
|
wolffd@0
|
91
|
|
wolffd@0
|
92 for t=1:T
|
|
wolffd@0
|
93 %err(t)=rms(xa(:,t), xe(:,t));
|
|
wolffd@0
|
94 err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
|
|
wolffd@0
|
95 end
|
|
wolffd@0
|
96 figure(5);plot(err)
|
|
wolffd@0
|
97 title('rms mean pos')
|
|
wolffd@0
|
98
|
|
wolffd@0
|
99
|
|
wolffd@0
|
100 for t=1:T
|
|
wolffd@0
|
101 i = 1:2*nlandmarks;
|
|
wolffd@0
|
102 denom = Ve(i,i,t) + (Ve(i,i,t)==0);
|
|
wolffd@0
|
103 Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
|
|
wolffd@0
|
104 Verr(t) = max(Vd(:));
|
|
wolffd@0
|
105 end
|
|
wolffd@0
|
106 figure(6); plot(Verr)
|
|
wolffd@0
|
107 title('max relative Verr')
|
