wolffd@0
|
1 % We navigate a robot around a square using a fixed control policy and no noise.
|
wolffd@0
|
2 % We assume the robot observes the relative distance to the nearest landmark.
|
wolffd@0
|
3 % Everything is linear-Gaussian.
|
wolffd@0
|
4
|
wolffd@0
|
5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
wolffd@0
|
6 % Create toy data set
|
wolffd@0
|
7
|
wolffd@0
|
8 seed = 0;
|
wolffd@0
|
9 rand('state', seed);
|
wolffd@0
|
10 randn('state', seed);
|
wolffd@0
|
11
|
wolffd@0
|
12 if 1
|
wolffd@0
|
13 T = 20;
|
wolffd@0
|
14 ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
|
wolffd@0
|
15 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
|
wolffd@0
|
16 else
|
wolffd@0
|
17 T = 5;
|
wolffd@0
|
18 ctrl_signal = repmat([1 0]', 1, T);
|
wolffd@0
|
19 end
|
wolffd@0
|
20
|
wolffd@0
|
21 nlandmarks = 4;
|
wolffd@0
|
22 true_landmark_pos = [1 1;
|
wolffd@0
|
23 4 1;
|
wolffd@0
|
24 4 4;
|
wolffd@0
|
25 1 4]';
|
wolffd@0
|
26 init_robot_pos = [0 0]';
|
wolffd@0
|
27
|
wolffd@0
|
28 true_robot_pos = zeros(2, T);
|
wolffd@0
|
29 true_data_assoc = zeros(1, T);
|
wolffd@0
|
30 true_rel_dist = zeros(2, T);
|
wolffd@0
|
31 for t=1:T
|
wolffd@0
|
32 if t>1
|
wolffd@0
|
33 true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
|
wolffd@0
|
34 else
|
wolffd@0
|
35 true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
|
wolffd@0
|
36 end
|
wolffd@0
|
37 nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
|
wolffd@0
|
38 %nn = t; % observe 1, 2, 3
|
wolffd@0
|
39 true_data_assoc(t) = nn;
|
wolffd@0
|
40 true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
|
wolffd@0
|
41 end
|
wolffd@0
|
42
|
wolffd@0
|
43 figure(1);
|
wolffd@0
|
44 %clf;
|
wolffd@0
|
45 hold on
|
wolffd@0
|
46 %plot(true_landmark_pos(1,:), true_landmark_pos(2,:), '*');
|
wolffd@0
|
47 for i=1:nlandmarks
|
wolffd@0
|
48 text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
|
wolffd@0
|
49 end
|
wolffd@0
|
50 for t=1:T
|
wolffd@0
|
51 text(true_robot_pos(1,t), true_robot_pos(2,t), sprintf('%d',t));
|
wolffd@0
|
52 end
|
wolffd@0
|
53 hold off
|
wolffd@0
|
54 axis([-1 6 -1 6])
|
wolffd@0
|
55
|
wolffd@0
|
56 R = 1e-3*eye(2); % noise added to observation
|
wolffd@0
|
57 Q = 1e-3*eye(2); % noise added to robot motion
|
wolffd@0
|
58
|
wolffd@0
|
59 % Create data set
|
wolffd@0
|
60 obs_noise_seq = sample_gaussian([0 0]', R, T)';
|
wolffd@0
|
61 obs_rel_pos = true_rel_dist + obs_noise_seq;
|
wolffd@0
|
62 %obs_rel_pos = true_rel_dist;
|
wolffd@0
|
63
|
wolffd@0
|
64
|
wolffd@0
|
65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
wolffd@0
|
66 % Create params for inference
|
wolffd@0
|
67
|
wolffd@0
|
68 % X(t) = A X(t-1) + B U(t) + noise(Q)
|
wolffd@0
|
69
|
wolffd@0
|
70 % [L1] = [1 ] * [L1] + [0] * Ut + [0 ]
|
wolffd@0
|
71 % [L2] [ 1 ] [L2] [0] [ 0 ]
|
wolffd@0
|
72 % [R ]t [ 1] [R ]t-1 [1] [ Q]
|
wolffd@0
|
73
|
wolffd@0
|
74 % Y(t)|S(t)=s = C(s) X(t) + noise(R)
|
wolffd@0
|
75 % Yt|St=1 = [1 0 -1] * [L1] + R
|
wolffd@0
|
76 % [L2]
|
wolffd@0
|
77 % [R ]
|
wolffd@0
|
78
|
wolffd@0
|
79 % Create indices into block structure
|
wolffd@0
|
80 bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
|
wolffd@0
|
81 robot_block = block(nlandmarks+1, bs);
|
wolffd@0
|
82 for i=1:nlandmarks
|
wolffd@0
|
83 landmark_block(:,i) = block(i, bs)';
|
wolffd@0
|
84 end
|
wolffd@0
|
85 Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
|
wolffd@0
|
86 Ysz = 2; % observe relative location
|
wolffd@0
|
87 Usz = 2; % input is (dx, dy)
|
wolffd@0
|
88
|
wolffd@0
|
89
|
wolffd@0
|
90 % create block-diagonal trans matrix for each switch
|
wolffd@0
|
91 A = zeros(Xsz, Xsz);
|
wolffd@0
|
92 for i=1:nlandmarks
|
wolffd@0
|
93 bi = landmark_block(:,i);
|
wolffd@0
|
94 A(bi, bi) = eye(2);
|
wolffd@0
|
95 end
|
wolffd@0
|
96 bi = robot_block;
|
wolffd@0
|
97 A(bi, bi) = eye(2);
|
wolffd@0
|
98 A = repmat(A, [1 1 nlandmarks]); % same for all switch values
|
wolffd@0
|
99
|
wolffd@0
|
100 % create block-diagonal system cov
|
wolffd@0
|
101
|
wolffd@0
|
102
|
wolffd@0
|
103 Qbig = zeros(Xsz, Xsz);
|
wolffd@0
|
104 bi = robot_block;
|
wolffd@0
|
105 Qbig(bi,bi) = Q; % only add noise to robot motion
|
wolffd@0
|
106 Qbig = repmat(Qbig, [1 1 nlandmarks]);
|
wolffd@0
|
107
|
wolffd@0
|
108 % create input matrix
|
wolffd@0
|
109 B = zeros(Xsz, Usz);
|
wolffd@0
|
110 B(robot_block,:) = eye(2); % only add input to robot position
|
wolffd@0
|
111 B = repmat(B, [1 1 nlandmarks]);
|
wolffd@0
|
112
|
wolffd@0
|
113 % create observation matrix for each value of the switch node
|
wolffd@0
|
114 % C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
|
wolffd@0
|
115 % This computes L(i) - R
|
wolffd@0
|
116 C = zeros(Ysz, Xsz, nlandmarks);
|
wolffd@0
|
117 for i=1:nlandmarks
|
wolffd@0
|
118 C(:, landmark_block(:,i), i) = eye(2);
|
wolffd@0
|
119 C(:, robot_block, i) = -eye(2);
|
wolffd@0
|
120 end
|
wolffd@0
|
121
|
wolffd@0
|
122 % create observation cov for each value of the switch node
|
wolffd@0
|
123 Rbig = repmat(R, [1 1 nlandmarks]);
|
wolffd@0
|
124
|
wolffd@0
|
125 % initial conditions
|
wolffd@0
|
126 init_x = zeros(Xsz, 1);
|
wolffd@0
|
127 init_v = zeros(Xsz, Xsz);
|
wolffd@0
|
128 bi = robot_block;
|
wolffd@0
|
129 init_x(bi) = init_robot_pos;
|
wolffd@0
|
130 init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
|
wolffd@0
|
131 for i=1:nlandmarks
|
wolffd@0
|
132 bi = landmark_block(:,i);
|
wolffd@0
|
133 init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
|
wolffd@0
|
134 %init_x(bi) = true_landmark_pos(:,i);
|
wolffd@0
|
135 %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
|
wolffd@0
|
136 end
|
wolffd@0
|
137
|
wolffd@0
|
138 %%%%%%%%%%%%%%%%%%%%%
|
wolffd@0
|
139 % Inference
|
wolffd@0
|
140 if 1
|
wolffd@0
|
141 [xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
|
wolffd@0
|
142 'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
|
wolffd@0
|
143
|
wolffd@0
|
144 est_robot_pos = xsmooth(robot_block, :);
|
wolffd@0
|
145 est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
|
wolffd@0
|
146
|
wolffd@0
|
147 for i=1:nlandmarks
|
wolffd@0
|
148 bi = landmark_block(:,i);
|
wolffd@0
|
149 est_landmark_pos(:,i) = xsmooth(bi, T);
|
wolffd@0
|
150 est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
|
wolffd@0
|
151 end
|
wolffd@0
|
152 end
|
wolffd@0
|
153
|
wolffd@0
|
154
|
wolffd@0
|
155 if 0
|
wolffd@0
|
156 figure(1); hold on
|
wolffd@0
|
157 for i=1:nlandmarks
|
wolffd@0
|
158 h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));
|
wolffd@0
|
159 set(h, 'color', 'r')
|
wolffd@0
|
160 end
|
wolffd@0
|
161 hold off
|
wolffd@0
|
162
|
wolffd@0
|
163 hold on
|
wolffd@0
|
164 for t=1:T
|
wolffd@0
|
165 h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t));
|
wolffd@0
|
166 set(h,'color','r')
|
wolffd@0
|
167 h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t));
|
wolffd@0
|
168 set(h,'color','r')
|
wolffd@0
|
169 end
|
wolffd@0
|
170 hold off
|
wolffd@0
|
171 end
|
wolffd@0
|
172
|
wolffd@0
|
173
|
wolffd@0
|
174 if 0
|
wolffd@0
|
175 figure(3)
|
wolffd@0
|
176 if 0
|
wolffd@0
|
177 for t=1:T
|
wolffd@0
|
178 imagesc(inv(Vsmooth(:,:,t)))
|
wolffd@0
|
179 colorbar
|
wolffd@0
|
180 fprintf('t=%d; press key to continue\n', t);
|
wolffd@0
|
181 pause
|
wolffd@0
|
182 end
|
wolffd@0
|
183 else
|
wolffd@0
|
184 for t=1:T
|
wolffd@0
|
185 subplot(5,4,t)
|
wolffd@0
|
186 imagesc(inv(Vsmooth(:,:,t)))
|
wolffd@0
|
187 end
|
wolffd@0
|
188 end
|
wolffd@0
|
189 end
|
wolffd@0
|
190
|
wolffd@0
|
191
|
wolffd@0
|
192
|
wolffd@0
|
193
|
wolffd@0
|
194
|
wolffd@0
|
195 %%%%%%%%%%%%%%%%%
|
wolffd@0
|
196 % DBN inference
|
wolffd@0
|
197
|
wolffd@0
|
198 if 1
|
wolffd@0
|
199 [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
|
wolffd@0
|
200 mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
|
wolffd@0
|
201 engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ...
|
wolffd@0
|
202 '/home/eecs/murphyk/matlab/loopyslam.txt');
|
wolffd@0
|
203 else
|
wolffd@0
|
204 [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ...
|
wolffd@0
|
205 mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
|
wolffd@0
|
206 engine = jtree_dbn_inf_engine(bnet);
|
wolffd@0
|
207 end
|
wolffd@0
|
208
|
wolffd@0
|
209 nnodes = bnet.nnodes_per_slice;
|
wolffd@0
|
210 evidence = cell(nnodes, T);
|
wolffd@0
|
211 evidence(Ynode, :) = num2cell(obs_rel_pos, 1);
|
wolffd@0
|
212 evidence(Unode, :) = num2cell(ctrl_signal, 1);
|
wolffd@0
|
213 evidence(Snode, :) = num2cell(true_data_assoc);
|
wolffd@0
|
214
|
wolffd@0
|
215
|
wolffd@0
|
216 [engine, ll, niter] = enter_evidence(engine, evidence);
|
wolffd@0
|
217 niter
|
wolffd@0
|
218
|
wolffd@0
|
219 loopy_est_robot_pos = zeros(2, T);
|
wolffd@0
|
220 for t=1:T
|
wolffd@0
|
221 m = marginal_nodes(engine, Rnode, t);
|
wolffd@0
|
222 loopy_est_robot_pos(:,t) = m.mu;
|
wolffd@0
|
223 end
|
wolffd@0
|
224
|
wolffd@0
|
225 for i=1:nlandmarks
|
wolffd@0
|
226 m = marginal_nodes(engine, Lnodes(i), T);
|
wolffd@0
|
227 loopy_est_landmark_pos(:,i) = m.mu;
|
wolffd@0
|
228 loopy_est_landmark_pos_cov(:,:,i) = m.Sigma;
|
wolffd@0
|
229 end
|
wolffd@0
|
230
|
wolffd@0
|
231
|