Mercurial > hg > camir-aes2014

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