Mercurial > hg > camir-aes2014
comparison toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/slam_offline_loopy.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 % Compare Kalman smoother with loopy | |
2 | |
3 seed = 0; | |
4 rand('state', seed); | |
5 randn('state', seed); | |
6 nlandmarks = 6; | |
7 T = 12; | |
8 | |
9 [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,... | |
10 true_landmark_pos, true_robot_pos, true_data_assoc, ... | |
11 obs_rel_pos, ctrl_signal] = mk_linear_slam(... | |
12 'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle'); | |
13 | |
14 [xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ... | |
15 'model', true_data_assoc, 'u', ctrl_signal, 'B', B); | |
16 | |
17 est_robot_pos = xsmooth(robot_block, :); | |
18 est_robot_pos_cov = Vsmooth(robot_block, robot_block, :); | |
19 | |
20 for i=1:nlandmarks | |
21 bi = landmark_block(:,i); | |
22 est_landmark_pos(:,i) = xsmooth(bi, T); | |
23 est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T); | |
24 end | |
25 | |
26 | |
27 if 1 | |
28 [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ... | |
29 mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block); | |
30 engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ... | |
31 '/home/eecs/murphyk/matlab/loopyslam.txt'); | |
32 else | |
33 [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ... | |
34 mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block); | |
35 engine = jtree_dbn_inf_engine(bnet); | |
36 end | |
37 | |
38 nnodes = bnet.nnodes_per_slice; | |
39 evidence = cell(nnodes, T); | |
40 evidence(Ynode, :) = num2cell(obs_rel_pos, 1); | |
41 evidence(Unode, :) = num2cell(ctrl_signal, 1); | |
42 evidence(Snode, :) = num2cell(true_data_assoc); | |
43 | |
44 [engine, ll, niter] = enter_evidence(engine, evidence); | |
45 niter | |
46 | |
47 loopy_est_robot_pos = zeros(2, T); | |
48 for t=1:T | |
49 m = marginal_nodes(engine, Rnode, t); | |
50 loopy_est_robot_pos(:,t) = m.mu; | |
51 end | |
52 | |
53 for i=1:nlandmarks | |
54 m = marginal_nodes(engine, Lnodes(i), T); | |
55 loopy_est_landmark_pos(:,i) = m.mu; | |
56 loopy_est_landmark_pos_cov(:,:,i) = m.Sigma; | |
57 end | |
58 | |
59 |