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