|
wolffd@0
|
1 % Compare Kalman smoother with loopy
|
|
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 [xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
|
|
wolffd@0
|
15 'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
|
|
wolffd@0
|
16
|
|
wolffd@0
|
17 est_robot_pos = xsmooth(robot_block, :);
|
|
wolffd@0
|
18 est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
|
|
wolffd@0
|
19
|
|
wolffd@0
|
20 for i=1:nlandmarks
|
|
wolffd@0
|
21 bi = landmark_block(:,i);
|
|
wolffd@0
|
22 est_landmark_pos(:,i) = xsmooth(bi, T);
|
|
wolffd@0
|
23 est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
|
|
wolffd@0
|
24 end
|
|
wolffd@0
|
25
|
|
wolffd@0
|
26
|
|
wolffd@0
|
27 if 1
|
|
wolffd@0
|
28 [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
|
|
wolffd@0
|
29 mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
|
|
wolffd@0
|
30 engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ...
|
|
wolffd@0
|
31 '/home/eecs/murphyk/matlab/loopyslam.txt');
|
|
wolffd@0
|
32 else
|
|
wolffd@0
|
33 [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ...
|
|
wolffd@0
|
34 mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
|
|
wolffd@0
|
35 engine = jtree_dbn_inf_engine(bnet);
|
|
wolffd@0
|
36 end
|
|
wolffd@0
|
37
|
|
wolffd@0
|
38 nnodes = bnet.nnodes_per_slice;
|
|
wolffd@0
|
39 evidence = cell(nnodes, T);
|
|
wolffd@0
|
40 evidence(Ynode, :) = num2cell(obs_rel_pos, 1);
|
|
wolffd@0
|
41 evidence(Unode, :) = num2cell(ctrl_signal, 1);
|
|
wolffd@0
|
42 evidence(Snode, :) = num2cell(true_data_assoc);
|
|
wolffd@0
|
43
|
|
wolffd@0
|
44 [engine, ll, niter] = enter_evidence(engine, evidence);
|
|
wolffd@0
|
45 niter
|
|
wolffd@0
|
46
|
|
wolffd@0
|
47 loopy_est_robot_pos = zeros(2, T);
|
|
wolffd@0
|
48 for t=1:T
|
|
wolffd@0
|
49 m = marginal_nodes(engine, Rnode, t);
|
|
wolffd@0
|
50 loopy_est_robot_pos(:,t) = m.mu;
|
|
wolffd@0
|
51 end
|
|
wolffd@0
|
52
|
|
wolffd@0
|
53 for i=1:nlandmarks
|
|
wolffd@0
|
54 m = marginal_nodes(engine, Lnodes(i), T);
|
|
wolffd@0
|
55 loopy_est_landmark_pos(:,i) = m.mu;
|
|
wolffd@0
|
56 loopy_est_landmark_pos_cov(:,:,i) = m.Sigma;
|
|
wolffd@0
|
57 end
|
|
wolffd@0
|
58
|
|
wolffd@0
|
59
|
