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