Mercurial > hg > camir-aes2014

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

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
first hg version after svn
author wolffd
date Tue, 10 Feb 2015 15:05:51 +0000
parents
children
rev   line source
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)