wolffd@0: % This is like skf_data_assoc_gmux, except the objects don't move.
wolffd@0: % We are uncertain of their initial positions, and get more and more observations
wolffd@0: % over time. The goal is to test deterministic links (0 covariance).
wolffd@0: % This is like robot1, except the robot doesn't move and is always at [0 0],
wolffd@0: % so the relative location is simply L(s).
wolffd@0: 
wolffd@0: nobj = 2;
wolffd@0: N = nobj+2;
wolffd@0: Xs = 1:nobj;
wolffd@0: S = nobj+1;
wolffd@0: Y = nobj+2;
wolffd@0: 
wolffd@0: intra = zeros(N,N);
wolffd@0: inter = zeros(N,N);
wolffd@0: intra([Xs S], Y) =1;
wolffd@0: for i=1:nobj
wolffd@0:   inter(Xs(i), Xs(i))=1;
wolffd@0: end
wolffd@0: 
wolffd@0: Xsz = 2; % state space = (x y)
wolffd@0: Ysz = 2;
wolffd@0: ns = zeros(1,N);
wolffd@0: ns(Xs) = Xsz;
wolffd@0: ns(Y) = Ysz;
wolffd@0: ns(S) = nobj;
wolffd@0: 
wolffd@0: bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);
wolffd@0: 
wolffd@0: % For each object, we have
wolffd@0: % X(t+1) = F X(t) + noise(Q)
wolffd@0: % Y(t) = H X(t) + noise(R)
wolffd@0: F = eye(2);
wolffd@0: H = eye(2);
wolffd@0: Q = 0*eye(Xsz); % no noise in dynamics
wolffd@0: R = eye(Ysz);
wolffd@0: 
wolffd@0: init_state{1} = [10 10]';
wolffd@0: init_state{2} = [10 -10]';
wolffd@0: init_cov = eye(2);
wolffd@0: 
wolffd@0: % Uncertain of initial state (position)
wolffd@0: for i=1:nobj
wolffd@0:   bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', init_cov);
wolffd@0: end
wolffd@0: bnet.CPD{S} = root_CPD(bnet, S); % always observed
wolffd@0: bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
wolffd@0: % slice 2
wolffd@0: eclass = bnet.equiv_class;
wolffd@0: for i=1:nobj
wolffd@0:   bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
wolffd@0: end
wolffd@0: 
wolffd@0: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wolffd@0: % Create LDS params 
wolffd@0: 
wolffd@0: % X(t) = A X(t-1) + B U(t) + noise(Q)
wolffd@0: 
wolffd@0: % [L11]  = [1  ]  * [L1]       +   [Q ]
wolffd@0: % [L2]     [  1]    [L2]           [ Q]
wolffd@0: 
wolffd@0: % Y(t)|S(t)=s  = C(s) X(t) + noise(R)
wolffd@0: % Yt|St=1 = [1 0]  * [L1]  + R
wolffd@0: %                    [L2]    
wolffd@0: 
wolffd@0: nlandmarks = nobj;
wolffd@0: 
wolffd@0: % Create indices into block structure
wolffd@0: bs = 2*ones(1, nobj); % sizes of blocks in state space
wolffd@0: for i=1:nlandmarks
wolffd@0:   landmark_block(:,i) = block(i, bs)';
wolffd@0: end
wolffd@0: Xsz = 2*(nlandmarks); % 2 values for each landmark plus robot
wolffd@0: Ysz = 2; % observe relative location
wolffd@0: 
wolffd@0: % create block-diagonal trans matrix for each switch
wolffd@0: A = zeros(Xsz, Xsz);
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   A(bi, bi) = eye(2);
wolffd@0: end
wolffd@0: A = repmat(A, [1 1 nlandmarks]); % same for all switch values
wolffd@0: 
wolffd@0: % create block-diagonal system cov
wolffd@0: Qbig = zeros(Xsz, Xsz);
wolffd@0: Qbig = repmat(Qbig, [1 1 nlandmarks]);
wolffd@0: 
wolffd@0: 
wolffd@0: % create observation matrix for each value of the switch node
wolffd@0: % C(:,:,i) = (0 ... I ...) where the I is in the i'th posn.
wolffd@0: C = zeros(Ysz, Xsz, nlandmarks);
wolffd@0: for i=1:nlandmarks
wolffd@0:   C(:, landmark_block(:,i), i) = eye(2);
wolffd@0: end
wolffd@0: 
wolffd@0: % create observation cov for each value of the switch node
wolffd@0: Rbig = repmat(R, [1 1 nlandmarks]);
wolffd@0: 
wolffd@0: % initial conditions
wolffd@0: init_x = [init_state{1}; init_state{2}];
wolffd@0: init_V = zeros(Xsz, Xsz);
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   init_V(bi,bi) = init_cov;
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: 
wolffd@0: %%%%%%%%%%%%%%%%
wolffd@0: % Observe objects at random
wolffd@0: T = 10;
wolffd@0: evidence = cell(N, T);
wolffd@0: data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
wolffd@0: evidence(S,:) = num2cell(data_assoc);
wolffd@0: evidence = sample_dbn(bnet, 'evidence', evidence);
wolffd@0: 
wolffd@0: 
wolffd@0: % Inference
wolffd@0: ev = cell(N,T);
wolffd@0: ev(bnet.observed,:) = evidence(bnet.observed, :);
wolffd@0: y = cell2num(evidence(Y,:));
wolffd@0: 
wolffd@0: engine = pearl_unrolled_dbn_inf_engine(bnet);
wolffd@0: engine = enter_evidence(engine, ev);
wolffd@0: 
wolffd@0: loopy_est_pos = zeros(2, nlandmarks);
wolffd@0: loopy_est_pos_cov = zeros(2, 2, nlandmarks);
wolffd@0: for i=1:nobj
wolffd@0:   m = marginal_nodes(engine, Xs(i), T);
wolffd@0:   loopy_est_pos(:,i) = m.mu;
wolffd@0:   loopy_est_pos_cov(:,:,i) = m.Sigma;
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: [xsmooth, Vsmooth] = kalman_smoother(y, A, C, Qbig, Rbig, init_x, init_V, 'model', data_assoc);
wolffd@0: 
wolffd@0: kf_est_pos = zeros(2, nlandmarks);
wolffd@0: kf_est_pos_cov = zeros(2, 2, nlandmarks);
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   kf_est_pos(:,i) = xsmooth(bi, T);
wolffd@0:   kf_est_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: kf_est_pos
wolffd@0: loopy_est_pos
wolffd@0: 
wolffd@0: kf_est_pos_time = zeros(2, nlandmarks, T);
wolffd@0: for t=1:T
wolffd@0:   for i=1:nlandmarks
wolffd@0:     bi = landmark_block(:,i);
wolffd@0:     kf_est_pos_time(:,i,t) = xsmooth(bi, t);
wolffd@0:   end
wolffd@0: end
wolffd@0: kf_est_pos_time % same for all t since smoothed