wolffd@0
|
1 % We consider a switching Kalman filter of the kind studied
|
wolffd@0
|
2 % by Zoubin Ghahramani, i.e., where the switch node determines
|
wolffd@0
|
3 % which of the hidden chains we get to observe (data association).
|
wolffd@0
|
4 % e.g., for n=2 chains
|
wolffd@0
|
5 %
|
wolffd@0
|
6 % X1 -> X1
|
wolffd@0
|
7 % | X2 -> X2
|
wolffd@0
|
8 % \ |
|
wolffd@0
|
9 % v
|
wolffd@0
|
10 % Y
|
wolffd@0
|
11 % ^
|
wolffd@0
|
12 % |
|
wolffd@0
|
13 % S
|
wolffd@0
|
14 %
|
wolffd@0
|
15 % Y is a gmux (multiplexer) node, where S switches in one of the parents.
|
wolffd@0
|
16 % We differ from Zoubin by not connecting the S nodes over time (which
|
wolffd@0
|
17 % doesn't make sense for data association).
|
wolffd@0
|
18 % Indeed, we assume the S nodes are always observed.
|
wolffd@0
|
19 %
|
wolffd@0
|
20 %
|
wolffd@0
|
21 % We will track 2 objects (points) moving in the plane, as in BNT/Kalman/tracking_demo.
|
wolffd@0
|
22 % We will alternate between observing them.
|
wolffd@0
|
23
|
wolffd@0
|
24 nobj = 2;
|
wolffd@0
|
25 N = nobj+2;
|
wolffd@0
|
26 Xs = 1:nobj;
|
wolffd@0
|
27 S = nobj+1;
|
wolffd@0
|
28 Y = nobj+2;
|
wolffd@0
|
29
|
wolffd@0
|
30 intra = zeros(N,N);
|
wolffd@0
|
31 inter = zeros(N,N);
|
wolffd@0
|
32 intra([Xs S], Y) =1;
|
wolffd@0
|
33 for i=1:nobj
|
wolffd@0
|
34 inter(Xs(i), Xs(i))=1;
|
wolffd@0
|
35 end
|
wolffd@0
|
36
|
wolffd@0
|
37 Xsz = 4; % state space = (x y xdot ydot)
|
wolffd@0
|
38 Ysz = 2;
|
wolffd@0
|
39 ns = zeros(1,N);
|
wolffd@0
|
40 ns(Xs) = Xsz;
|
wolffd@0
|
41 ns(Y) = Ysz;
|
wolffd@0
|
42 ns(S) = n;
|
wolffd@0
|
43
|
wolffd@0
|
44 bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);
|
wolffd@0
|
45
|
wolffd@0
|
46 % For each object, we have
|
wolffd@0
|
47 % X(t+1) = F X(t) + noise(Q)
|
wolffd@0
|
48 % Y(t) = H X(t) + noise(R)
|
wolffd@0
|
49 F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
|
wolffd@0
|
50 H = [1 0 0 0; 0 1 0 0];
|
wolffd@0
|
51 Q = 1e-3*eye(Xsz);
|
wolffd@0
|
52 %R = 1e-3*eye(Ysz);
|
wolffd@0
|
53 R = eye(Ysz);
|
wolffd@0
|
54
|
wolffd@0
|
55 % We initialise object 1 moving to the right, and object 2 moving to the left
|
wolffd@0
|
56 % (Here, we assume nobj=2)
|
wolffd@0
|
57 init_state{1} = [10 10 1 0]';
|
wolffd@0
|
58 init_state{2} = [10 -10 -1 0]';
|
wolffd@0
|
59
|
wolffd@0
|
60 for i=1:nobj
|
wolffd@0
|
61 bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', 1e-4*eye(Xsz));
|
wolffd@0
|
62 end
|
wolffd@0
|
63 bnet.CPD{S} = root_CPD(bnet, S); % always observed
|
wolffd@0
|
64 bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
|
wolffd@0
|
65 % slice 2
|
wolffd@0
|
66 eclass = bnet.equiv_class;
|
wolffd@0
|
67 for i=1:nobj
|
wolffd@0
|
68 bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
|
wolffd@0
|
69 end
|
wolffd@0
|
70
|
wolffd@0
|
71 % Observe objects at random
|
wolffd@0
|
72 T = 10;
|
wolffd@0
|
73 evidence = cell(N, T);
|
wolffd@0
|
74 data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
|
wolffd@0
|
75 evidence(S,:) = num2cell(data_assoc);
|
wolffd@0
|
76 evidence = sample_dbn(bnet, 'evidence', evidence);
|
wolffd@0
|
77
|
wolffd@0
|
78 % plot the data
|
wolffd@0
|
79 true_state = cell(1,nobj);
|
wolffd@0
|
80 for i=1:nobj
|
wolffd@0
|
81 true_state{i} = cell2num(evidence(Xs(i), :)); % true_state{i}(:,t) = [x y xdot ydot]'
|
wolffd@0
|
82 end
|
wolffd@0
|
83 obs_pos = cell2num(evidence(Y,:));
|
wolffd@0
|
84 figure(1)
|
wolffd@0
|
85 clf
|
wolffd@0
|
86 hold on
|
wolffd@0
|
87 styles = {'rx', 'go', 'b+', 'k*'};
|
wolffd@0
|
88 for i=1:nobj
|
wolffd@0
|
89 plot(true_state{i}(1,:), true_state{i}(2,:), styles{i});
|
wolffd@0
|
90 end
|
wolffd@0
|
91 for t=1:T
|
wolffd@0
|
92 text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
|
wolffd@0
|
93 end
|
wolffd@0
|
94 hold off
|
wolffd@0
|
95 relax_axes(0.1)
|
wolffd@0
|
96
|
wolffd@0
|
97
|
wolffd@0
|
98 % Inference
|
wolffd@0
|
99 ev = cell(N,T);
|
wolffd@0
|
100 ev(bnet.observed,:) = evidence(bnet.observed, :);
|
wolffd@0
|
101
|
wolffd@0
|
102 engines = {};
|
wolffd@0
|
103 engines{end+1} = jtree_dbn_inf_engine(bnet);
|
wolffd@0
|
104 %engines{end+1} = scg_unrolled_dbn_inf_engine(bnet, T);
|
wolffd@0
|
105 engines{end+1} = pearl_unrolled_dbn_inf_engine(bnet);
|
wolffd@0
|
106 E = length(engines);
|
wolffd@0
|
107
|
wolffd@0
|
108 inferred_state = cell(nobj,E); % inferred_state{i,e}(:,t)
|
wolffd@0
|
109 for e=1:E
|
wolffd@0
|
110 engines{e} = enter_evidence(engines{e}, ev);
|
wolffd@0
|
111 for i=1:nobj
|
wolffd@0
|
112 inferred_state{i,e} = zeros(4, T);
|
wolffd@0
|
113 for t=1:T
|
wolffd@0
|
114 m = marginal_nodes(engines{e}, Xs(i), t);
|
wolffd@0
|
115 inferred_state{i,e}(:,t) = m.mu;
|
wolffd@0
|
116 end
|
wolffd@0
|
117 end
|
wolffd@0
|
118 end
|
wolffd@0
|
119 inferred_state{1,1}
|
wolffd@0
|
120 inferred_state{1,2}
|
wolffd@0
|
121
|
wolffd@0
|
122 % Plot results
|
wolffd@0
|
123 figure(2)
|
wolffd@0
|
124 clf
|
wolffd@0
|
125 hold on
|
wolffd@0
|
126 styles = {'rx', 'go', 'b+', 'k*'};
|
wolffd@0
|
127 nstyles = length(styles);
|
wolffd@0
|
128 c = 1;
|
wolffd@0
|
129 for e=1:E
|
wolffd@0
|
130 for i=1:nobj
|
wolffd@0
|
131 plot(inferred_state{i,e}(1,:), inferred_state{i,e}(2,:), styles{mod(c-1,nstyles)+1});
|
wolffd@0
|
132 c = c + 1;
|
wolffd@0
|
133 end
|
wolffd@0
|
134 end
|
wolffd@0
|
135 for t=1:T
|
wolffd@0
|
136 text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
|
wolffd@0
|
137 end
|
wolffd@0
|
138 hold off
|
wolffd@0
|
139 relax_axes(0.1)
|