|
wolffd@0
|
1 function [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
|
|
wolffd@0
|
2 mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block)
|
|
wolffd@0
|
3
|
|
wolffd@0
|
4 % Make DBN
|
|
wolffd@0
|
5
|
|
wolffd@0
|
6 % S
|
|
wolffd@0
|
7 % | L1 -------> L1'
|
|
wolffd@0
|
8 % | | L2 ----------> L2'
|
|
wolffd@0
|
9 % \ | /
|
|
wolffd@0
|
10 % v v v
|
|
wolffd@0
|
11 % Ls
|
|
wolffd@0
|
12 % |
|
|
wolffd@0
|
13 % v
|
|
wolffd@0
|
14 % Y
|
|
wolffd@0
|
15 % ^
|
|
wolffd@0
|
16 % |
|
|
wolffd@0
|
17 % R -------> R'
|
|
wolffd@0
|
18 % ^
|
|
wolffd@0
|
19 % |
|
|
wolffd@0
|
20 % U
|
|
wolffd@0
|
21 %
|
|
wolffd@0
|
22 %
|
|
wolffd@0
|
23 % S is a switch, Ls is a deterministic gmux, Y = Ls-R,
|
|
wolffd@0
|
24 % R(t+1) = R(t) + U(t+1), L(t+1) = L(t)
|
|
wolffd@0
|
25
|
|
wolffd@0
|
26
|
|
wolffd@0
|
27 % number nodes topologically
|
|
wolffd@0
|
28 Snode = 1;
|
|
wolffd@0
|
29 Lnodes = 2:nlandmarks+1;
|
|
wolffd@0
|
30 Lsnode = nlandmarks+2;
|
|
wolffd@0
|
31 Unode = nlandmarks+3;
|
|
wolffd@0
|
32 Rnode = nlandmarks+4;
|
|
wolffd@0
|
33 Ynode = nlandmarks+5;
|
|
wolffd@0
|
34
|
|
wolffd@0
|
35 nnodes = nlandmarks+5;
|
|
wolffd@0
|
36 intra = zeros(nnodes, nnodes);
|
|
wolffd@0
|
37 intra([Snode Lnodes], Lsnode) =1;
|
|
wolffd@0
|
38 intra(Unode,Rnode)=1;
|
|
wolffd@0
|
39 intra([Rnode Lsnode], Ynode)=1;
|
|
wolffd@0
|
40
|
|
wolffd@0
|
41 inter = zeros(nnodes, nnodes);
|
|
wolffd@0
|
42 inter(Rnode, Rnode)=1;
|
|
wolffd@0
|
43 for i=1:nlandmarks
|
|
wolffd@0
|
44 inter(Lnodes(i), Lnodes(i))=1;
|
|
wolffd@0
|
45 end
|
|
wolffd@0
|
46
|
|
wolffd@0
|
47 Lsz = 2; % (x y) posn of landmark
|
|
wolffd@0
|
48 Rsz = 2; % (x y) posn of robot
|
|
wolffd@0
|
49 Ysz = 2; % relative distance
|
|
wolffd@0
|
50 Usz = 2; % (dx dy) ctrl
|
|
wolffd@0
|
51 Ssz = nlandmarks; % can switch between any landmark
|
|
wolffd@0
|
52
|
|
wolffd@0
|
53 ns = zeros(1,nnodes);
|
|
wolffd@0
|
54 ns(Snode) = Ssz;
|
|
wolffd@0
|
55 ns(Lnodes) = Lsz;
|
|
wolffd@0
|
56 ns(Lsnode) = Lsz;
|
|
wolffd@0
|
57 ns(Ynode) = Ysz;
|
|
wolffd@0
|
58 ns(Rnode) = Rsz;
|
|
wolffd@0
|
59 ns(Ynode) = Usz;
|
|
wolffd@0
|
60 ns(Unode) = Usz;
|
|
wolffd@0
|
61
|
|
wolffd@0
|
62 bnet = mk_dbn(intra, inter, ns, 'discrete', Snode, 'observed', [Snode Ynode Unode]);
|
|
wolffd@0
|
63
|
|
wolffd@0
|
64
|
|
wolffd@0
|
65 bnet.CPD{Snode} = root_CPD(bnet, Snode); % always observed
|
|
wolffd@0
|
66 bnet.CPD{Unode} = root_CPD(bnet, Unode); % always observed
|
|
wolffd@0
|
67 for i=1:nlandmarks
|
|
wolffd@0
|
68 bi = landmark_block(:,i);
|
|
wolffd@0
|
69 bnet.CPD{Lnodes(i)} = gaussian_CPD(bnet, Lnodes(i), 'mean', init_x(bi), 'cov', init_V(bi,bi));
|
|
wolffd@0
|
70 end
|
|
wolffd@0
|
71 bi = robot_block;
|
|
wolffd@0
|
72 bnet.CPD{Rnode} = gaussian_CPD(bnet, Rnode, 'mean', init_x(bi), 'cov', init_V(bi,bi), 'weights', eye(2));
|
|
wolffd@0
|
73 bnet.CPD{Lsnode} = gmux_CPD(bnet, Lsnode, 'cov', repmat(zeros(Lsz,Lsz), [1 1 nlandmarks]), ...
|
|
wolffd@0
|
74 'weights', repmat(eye(Lsz,Lsz), [1 1 nlandmarks]));
|
|
wolffd@0
|
75 W = [eye(2) -eye(2)]; % Y = Ls - R, where Ls is the lower-numbered parent
|
|
wolffd@0
|
76 bnet.CPD{Ynode} = gaussian_CPD(bnet, Ynode, 'mean', zeros(Ysz,1), 'cov', R, 'weights', W);
|
|
wolffd@0
|
77
|
|
wolffd@0
|
78 % slice 2
|
|
wolffd@0
|
79 eclass = bnet.equiv_class;
|
|
wolffd@0
|
80 W = [eye(2) eye(2)]; % R(t) = R(t-1) + U(t), where R(t-1) is the lower-numbered parent
|
|
wolffd@0
|
81 bnet.CPD{eclass(Rnode,2)} = gaussian_CPD(bnet, Rnode+nnodes, 'mean', zeros(Rsz,1), 'cov', Q, 'weights', W);
|
|
wolffd@0
|
82 for i=1:nlandmarks
|
|
wolffd@0
|
83 bnet.CPD{eclass(Lnodes(i), 2)} = gaussian_CPD(bnet, Lnodes(i)+nnodes, 'mean', zeros(2,1), ...
|
|
wolffd@0
|
84 'cov', zeros(2,2), 'weights', eye(2));
|
|
wolffd@0
|
85 end
|
