wolffd@0: function [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
wolffd@0:     mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block)
wolffd@0: 
wolffd@0: % Make DBN
wolffd@0: 
wolffd@0: % S
wolffd@0: % | L1 -------> L1'
wolffd@0: % |  | L2 ----------> L2'
wolffd@0: % \  | /
wolffd@0: %  v v v
wolffd@0: %    Ls
wolffd@0: %    |
wolffd@0: %    v
wolffd@0: %    Y
wolffd@0: %    ^
wolffd@0: %    |
wolffd@0: %    R ------->  R'
wolffd@0: %    ^      
wolffd@0: %    |      
wolffd@0: %    U      
wolffd@0: %
wolffd@0: %
wolffd@0: % S is a switch, Ls is a deterministic gmux, Y = Ls-R,
wolffd@0: % R(t+1) = R(t) + U(t+1), L(t+1) = L(t)
wolffd@0: 
wolffd@0: 
wolffd@0: % number nodes topologically
wolffd@0: Snode = 1;
wolffd@0: Lnodes = 2:nlandmarks+1;
wolffd@0: Lsnode = nlandmarks+2;
wolffd@0: Unode = nlandmarks+3;
wolffd@0: Rnode = nlandmarks+4;
wolffd@0: Ynode = nlandmarks+5;
wolffd@0: 
wolffd@0: nnodes = nlandmarks+5; 
wolffd@0: intra = zeros(nnodes, nnodes);
wolffd@0: intra([Snode Lnodes], Lsnode) =1;
wolffd@0: intra(Unode,Rnode)=1;
wolffd@0: intra([Rnode Lsnode], Ynode)=1;
wolffd@0: 
wolffd@0: inter = zeros(nnodes, nnodes);
wolffd@0: inter(Rnode, Rnode)=1;
wolffd@0: for i=1:nlandmarks
wolffd@0:   inter(Lnodes(i), Lnodes(i))=1;
wolffd@0: end
wolffd@0: 
wolffd@0: Lsz = 2; % (x y) posn of landmark
wolffd@0: Rsz = 2; % (x y) posn of robot
wolffd@0: Ysz = 2; % relative distance
wolffd@0: Usz = 2; % (dx dy) ctrl
wolffd@0: Ssz = nlandmarks; % can switch between any landmark
wolffd@0: 
wolffd@0: ns = zeros(1,nnodes);
wolffd@0: ns(Snode) = Ssz;
wolffd@0: ns(Lnodes) = Lsz;
wolffd@0: ns(Lsnode) = Lsz;
wolffd@0: ns(Ynode) = Ysz;
wolffd@0: ns(Rnode) = Rsz;
wolffd@0: ns(Ynode) = Usz;
wolffd@0: ns(Unode) = Usz;
wolffd@0: 
wolffd@0: bnet = mk_dbn(intra, inter, ns, 'discrete', Snode, 'observed', [Snode Ynode Unode]);
wolffd@0: 
wolffd@0: 
wolffd@0: bnet.CPD{Snode} = root_CPD(bnet, Snode); % always observed
wolffd@0: bnet.CPD{Unode} = root_CPD(bnet, Unode); % always observed
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   bnet.CPD{Lnodes(i)} = gaussian_CPD(bnet, Lnodes(i), 'mean', init_x(bi), 'cov', init_V(bi,bi));
wolffd@0: end
wolffd@0: bi = robot_block;
wolffd@0: bnet.CPD{Rnode} = gaussian_CPD(bnet, Rnode, 'mean', init_x(bi), 'cov', init_V(bi,bi), 'weights', eye(2));
wolffd@0: bnet.CPD{Lsnode} = gmux_CPD(bnet, Lsnode, 'cov', repmat(zeros(Lsz,Lsz), [1 1 nlandmarks]), ...
wolffd@0: 			    'weights', repmat(eye(Lsz,Lsz), [1 1 nlandmarks]));
wolffd@0: W = [eye(2) -eye(2)]; % Y = Ls - R, where Ls is the lower-numbered parent
wolffd@0: bnet.CPD{Ynode} = gaussian_CPD(bnet, Ynode, 'mean', zeros(Ysz,1), 'cov', R, 'weights', W);
wolffd@0: 
wolffd@0: % slice 2
wolffd@0: eclass = bnet.equiv_class;
wolffd@0: W = [eye(2) eye(2)]; % R(t) = R(t-1) + U(t), where R(t-1) is the lower-numbered parent
wolffd@0: bnet.CPD{eclass(Rnode,2)} = gaussian_CPD(bnet, Rnode+nnodes, 'mean', zeros(Rsz,1), 'cov', Q, 'weights', W);
wolffd@0: for i=1:nlandmarks
wolffd@0:   bnet.CPD{eclass(Lnodes(i), 2)} = gaussian_CPD(bnet, Lnodes(i)+nnodes, 'mean', zeros(2,1), ...
wolffd@0: 						   'cov', zeros(2,2), 'weights', eye(2));
wolffd@0: end