Mercurial > hg > camir-aes2014
comparison toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.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 % This is like skf_data_assoc_gmux, except the objects don't move. | |
2 % We are uncertain of their initial positions, and get more and more observations | |
3 % over time. The goal is to test deterministic links (0 covariance). | |
4 % This is like robot1, except the robot doesn't move and is always at [0 0], | |
5 % so the relative location is simply L(s). | |
6 | |
7 nobj = 2; | |
8 N = nobj+2; | |
9 Xs = 1:nobj; | |
10 S = nobj+1; | |
11 Y = nobj+2; | |
12 | |
13 intra = zeros(N,N); | |
14 inter = zeros(N,N); | |
15 intra([Xs S], Y) =1; | |
16 for i=1:nobj | |
17 inter(Xs(i), Xs(i))=1; | |
18 end | |
19 | |
20 Xsz = 2; % state space = (x y) | |
21 Ysz = 2; | |
22 ns = zeros(1,N); | |
23 ns(Xs) = Xsz; | |
24 ns(Y) = Ysz; | |
25 ns(S) = nobj; | |
26 | |
27 bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]); | |
28 | |
29 % For each object, we have | |
30 % X(t+1) = F X(t) + noise(Q) | |
31 % Y(t) = H X(t) + noise(R) | |
32 F = eye(2); | |
33 H = eye(2); | |
34 Q = 0*eye(Xsz); % no noise in dynamics | |
35 R = eye(Ysz); | |
36 | |
37 init_state{1} = [10 10]'; | |
38 init_state{2} = [10 -10]'; | |
39 init_cov = eye(2); | |
40 | |
41 % Uncertain of initial state (position) | |
42 for i=1:nobj | |
43 bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', init_cov); | |
44 end | |
45 bnet.CPD{S} = root_CPD(bnet, S); % always observed | |
46 bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj])); | |
47 % slice 2 | |
48 eclass = bnet.equiv_class; | |
49 for i=1:nobj | |
50 bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F); | |
51 end | |
52 | |
53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
54 % Create LDS params | |
55 | |
56 % X(t) = A X(t-1) + B U(t) + noise(Q) | |
57 | |
58 % [L11] = [1 ] * [L1] + [Q ] | |
59 % [L2] [ 1] [L2] [ Q] | |
60 | |
61 % Y(t)|S(t)=s = C(s) X(t) + noise(R) | |
62 % Yt|St=1 = [1 0] * [L1] + R | |
63 % [L2] | |
64 | |
65 nlandmarks = nobj; | |
66 | |
67 % Create indices into block structure | |
68 bs = 2*ones(1, nobj); % sizes of blocks in state space | |
69 for i=1:nlandmarks | |
70 landmark_block(:,i) = block(i, bs)'; | |
71 end | |
72 Xsz = 2*(nlandmarks); % 2 values for each landmark plus robot | |
73 Ysz = 2; % observe relative location | |
74 | |
75 % create block-diagonal trans matrix for each switch | |
76 A = zeros(Xsz, Xsz); | |
77 for i=1:nlandmarks | |
78 bi = landmark_block(:,i); | |
79 A(bi, bi) = eye(2); | |
80 end | |
81 A = repmat(A, [1 1 nlandmarks]); % same for all switch values | |
82 | |
83 % create block-diagonal system cov | |
84 Qbig = zeros(Xsz, Xsz); | |
85 Qbig = repmat(Qbig, [1 1 nlandmarks]); | |
86 | |
87 | |
88 % create observation matrix for each value of the switch node | |
89 % C(:,:,i) = (0 ... I ...) where the I is in the i'th posn. | |
90 C = zeros(Ysz, Xsz, nlandmarks); | |
91 for i=1:nlandmarks | |
92 C(:, landmark_block(:,i), i) = eye(2); | |
93 end | |
94 | |
95 % create observation cov for each value of the switch node | |
96 Rbig = repmat(R, [1 1 nlandmarks]); | |
97 | |
98 % initial conditions | |
99 init_x = [init_state{1}; init_state{2}]; | |
100 init_V = zeros(Xsz, Xsz); | |
101 for i=1:nlandmarks | |
102 bi = landmark_block(:,i); | |
103 init_V(bi,bi) = init_cov; | |
104 end | |
105 | |
106 | |
107 | |
108 %%%%%%%%%%%%%%%% | |
109 % Observe objects at random | |
110 T = 10; | |
111 evidence = cell(N, T); | |
112 data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T); | |
113 evidence(S,:) = num2cell(data_assoc); | |
114 evidence = sample_dbn(bnet, 'evidence', evidence); | |
115 | |
116 | |
117 % Inference | |
118 ev = cell(N,T); | |
119 ev(bnet.observed,:) = evidence(bnet.observed, :); | |
120 y = cell2num(evidence(Y,:)); | |
121 | |
122 engine = pearl_unrolled_dbn_inf_engine(bnet); | |
123 engine = enter_evidence(engine, ev); | |
124 | |
125 loopy_est_pos = zeros(2, nlandmarks); | |
126 loopy_est_pos_cov = zeros(2, 2, nlandmarks); | |
127 for i=1:nobj | |
128 m = marginal_nodes(engine, Xs(i), T); | |
129 loopy_est_pos(:,i) = m.mu; | |
130 loopy_est_pos_cov(:,:,i) = m.Sigma; | |
131 end | |
132 | |
133 | |
134 [xsmooth, Vsmooth] = kalman_smoother(y, A, C, Qbig, Rbig, init_x, init_V, 'model', data_assoc); | |
135 | |
136 kf_est_pos = zeros(2, nlandmarks); | |
137 kf_est_pos_cov = zeros(2, 2, nlandmarks); | |
138 for i=1:nlandmarks | |
139 bi = landmark_block(:,i); | |
140 kf_est_pos(:,i) = xsmooth(bi, T); | |
141 kf_est_pos_cov(:,:,i) = Vsmooth(bi, bi, T); | |
142 end | |
143 | |
144 | |
145 kf_est_pos | |
146 loopy_est_pos | |
147 | |
148 kf_est_pos_time = zeros(2, nlandmarks, T); | |
149 for t=1:T | |
150 for i=1:nlandmarks | |
151 bi = landmark_block(:,i); | |
152 kf_est_pos_time(:,i,t) = xsmooth(bi, t); | |
153 end | |
154 end | |
155 kf_est_pos_time % same for all t since smoothed |