comparison toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/skf_data_assoc_gmux.m @ 0:e9a9cd732c1e tip

first hg version after svn

+% We consider a switching Kalman filter of the kind studied
+% by Zoubin Ghahramani, i.e., where the switch node determines
+% which of the hidden chains we get to observe (data association).
+% e.g., for n=2 chains
+% 
+% X1 -> X1
+% | X2 -> X2
+% \ |
+%  v
+%  Y
+%  ^
+%  |
+%  S
+%
+% Y is a gmux (multiplexer) node, where S switches in one of the parents.
+% We differ from Zoubin by not connecting the S nodes over time (which
+% doesn't make sense for data association).
+% Indeed, we assume the S nodes are always observed.
+% 
+%
+% We will track 2 objects (points) moving in the plane, as in BNT/Kalman/tracking_demo.
+% We will alternate between observing them.
+
+nobj = 2;
+N = nobj+2;
+Xs = 1:nobj;
+S = nobj+1;
+Y = nobj+2;
+
+intra = zeros(N,N);
+inter = zeros(N,N);
+intra([Xs S], Y) =1;
+for i=1:nobj
+  inter(Xs(i), Xs(i))=1;
+end
+
+Xsz = 4; % state space = (x y xdot ydot)
+Ysz = 2;
+ns = zeros(1,N);
+ns(Xs) = Xsz;
+ns(Y) = Ysz;
+ns(S) = n;
+
+bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);
+
+% For each object, we have
+% X(t+1) = F X(t) + noise(Q)
+% Y(t) = H X(t) + noise(R)
+F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
+H = [1 0 0 0; 0 1 0 0];
+Q = 1e-3*eye(Xsz);
+%R = 1e-3*eye(Ysz);
+R = eye(Ysz);
+
+% We initialise object 1 moving to the right, and object 2 moving to the left
+% (Here, we assume nobj=2)
+init_state{1} = [10 10 1 0]';
+init_state{2} = [10 -10 -1 0]';
+
+for i=1:nobj
+  bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', 1e-4*eye(Xsz));
+end
+bnet.CPD{S} = root_CPD(bnet, S); % always observed
+bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
+% slice 2
+eclass = bnet.equiv_class;
+for i=1:nobj
+  bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
+end
+
+% Observe objects at random
+T = 10;
+evidence = cell(N, T);
+data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
+evidence(S,:) = num2cell(data_assoc);
+evidence = sample_dbn(bnet, 'evidence', evidence);
+
+% plot the data
+true_state = cell(1,nobj);
+for i=1:nobj
+  true_state{i} = cell2num(evidence(Xs(i), :)); % true_state{i}(:,t) = [x y xdot ydot]'
+end
+obs_pos = cell2num(evidence(Y,:));
+figure(1)
+clf
+hold on
+styles = {'rx', 'go', 'b+', 'k*'};
+for i=1:nobj
+  plot(true_state{i}(1,:), true_state{i}(2,:), styles{i});
+end
+for t=1:T
+  text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
+end
+hold off
+relax_axes(0.1)
+
+
+% Inference
+ev = cell(N,T);
+ev(bnet.observed,:) = evidence(bnet.observed, :);
+
+engines = {};
+engines{end+1} = jtree_dbn_inf_engine(bnet);
+%engines{end+1} = scg_unrolled_dbn_inf_engine(bnet, T);
+engines{end+1} = pearl_unrolled_dbn_inf_engine(bnet);
+E = length(engines);
+
+inferred_state = cell(nobj,E); % inferred_state{i,e}(:,t)
+for e=1:E
+  engines{e} = enter_evidence(engines{e}, ev);
+  for i=1:nobj
+    inferred_state{i,e} = zeros(4, T);
+    for t=1:T
+      m = marginal_nodes(engines{e}, Xs(i), t);
+      inferred_state{i,e}(:,t) = m.mu;
+    end
+  end
+end
+inferred_state{1,1}
+inferred_state{1,2}
+
+% Plot results
+figure(2)
+clf
+hold on
+styles = {'rx', 'go', 'b+', 'k*'};
+nstyles = length(styles);
+c = 1;
+for e=1:E
+  for i=1:nobj
+    plot(inferred_state{i,e}(1,:), inferred_state{i,e}(2,:), styles{mod(c-1,nstyles)+1});
+    c = c + 1;
+  end
+end
+for t=1:T
+  text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
+end
+hold off
+relax_axes(0.1)