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