BNW using Octave instead of Matlab.

This version of BNW should perform the same as the original version. The only difference is that it uses Octave instead of Matlab when running BayesNet Toolbox during parameter learning.

I am calling this BNW_1.02. It can be accessed at:
compbio.uthsc.edu/BNW_1.02

16 files changed, 1460 insertions, 0 deletions

diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Entries b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Entries
new file mode 100644
index 00000000..6810ce1d
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Entries
@@ -0,0 +1,7 @@
+/mk_gmux_robot_dbn.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/mk_linear_slam.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/slam_kf.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/slam_offline_loopy.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/slam_partial_kf.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/slam_stationary_loopy.m/1.1.1.1/Wed May 29 15:59:54 2002//
+D/Old////
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Repository b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Repository
new file mode 100644
index 00000000..e32a23fa
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Repository
@@ -0,0 +1 @@
+FullBNT/BNT/examples/dynamic/SLAM
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Root b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Root
new file mode 100644
index 00000000..f3bd14a6
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/CVS/Root
@@ -0,0 +1 @@
+:ext:nsaunier@bnt.cvs.sourceforge.net:/cvsroot/bnt
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Entries b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Entries
new file mode 100644
index 00000000..37fe6bb1
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Entries
@@ -0,0 +1,5 @@
+/offline_loopy_slam.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/paskin1.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/skf_data_assoc_gmux2.m/1.1.1.1/Wed May 29 15:59:54 2002//
+/slam_kf.m/1.1.1.1/Wed May 29 15:59:54 2002//
+D
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Repository b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Repository
new file mode 100644
index 00000000..1bae1a70
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Repository
@@ -0,0 +1 @@
+FullBNT/BNT/examples/dynamic/SLAM/Old
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Root b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Root
new file mode 100644
index 00000000..f3bd14a6
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Root
@@ -0,0 +1 @@
+:ext:nsaunier@bnt.cvs.sourceforge.net:/cvsroot/bnt
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/offline_loopy_slam.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/offline_loopy_slam.m
new file mode 100644
index 00000000..377a4659
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/offline_loopy_slam.m
@@ -0,0 +1,231 @@
+% We navigate a robot around a square using a fixed control policy and no noise.
+% We assume the robot observes the relative distance to the nearest landmark.
+% Everything is linear-Gaussian.
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Create toy data set
+
+seed = 0;
+rand('state', seed);
+randn('state', seed);
+
+if 1
+  T = 20;
+  ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
+		 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
+else
+  T = 5;
+  ctrl_signal = repmat([1 0]', 1, T);
+end
+
+nlandmarks = 4;
+true_landmark_pos = [1 1;
+		     4 1;
+		     4 4;
+		     1 4]';
+init_robot_pos = [0 0]';
+
+true_robot_pos = zeros(2, T);
+true_data_assoc = zeros(1, T);
+true_rel_dist = zeros(2, T);
+for t=1:T
+  if t>1
+    true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
+  else
+    true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
+  end
+  nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
+  %nn = t; % observe 1, 2, 3
+  true_data_assoc(t) = nn;
+  true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
+end
+
+figure(1);
+%clf; 
+hold on
+%plot(true_landmark_pos(1,:), true_landmark_pos(2,:), '*');
+for i=1:nlandmarks
+  text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
+end
+for t=1:T
+  text(true_robot_pos(1,t), true_robot_pos(2,t), sprintf('%d',t));
+end
+hold off
+axis([-1 6 -1 6])
+
+R = 1e-3*eye(2); % noise added to observation
+Q = 1e-3*eye(2); % noise added to robot motion
+
+% Create data set
+obs_noise_seq = sample_gaussian([0 0]', R, T)';
+obs_rel_pos = true_rel_dist + obs_noise_seq;
+%obs_rel_pos = true_rel_dist;
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Create params for inference
+
+% X(t) = A X(t-1) + B U(t) + noise(Q)
+
+% [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
+% [L2]    [  1   ]    [L2]         [0]          [ 0  ]
+% [R ]t   [     1]    [R ]t-1      [1]          [   Q]
+
+% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
+% Yt|St=1 = [1 0 -1]  * [L1]  + R
+%                       [L2]    
+%                       [R ]    
+
+% Create indices into block structure
+bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
+robot_block =  block(nlandmarks+1, bs);
+for i=1:nlandmarks
+  landmark_block(:,i) = block(i, bs)';
+end
+Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
+Ysz = 2; % observe relative location
+Usz = 2; % input is (dx, dy)
+
+
+% create block-diagonal trans matrix for each switch
+A = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  A(bi, bi) = eye(2);
+end
+bi = robot_block;
+A(bi, bi) = eye(2);
+A = repmat(A, [1 1 nlandmarks]); % same for all switch values
+
+% create block-diagonal system cov
+
+
+Qbig = zeros(Xsz, Xsz);
+bi = robot_block;
+Qbig(bi,bi) = Q; % only add noise to robot motion
+Qbig = repmat(Qbig, [1 1 nlandmarks]);
+
+% create input matrix
+B = zeros(Xsz, Usz);
+B(robot_block,:) = eye(2); % only add input to robot position
+B = repmat(B, [1 1 nlandmarks]);
+
+% create observation matrix for each value of the switch node
+% C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
+% This computes L(i) - R
+C = zeros(Ysz, Xsz, nlandmarks);
+for i=1:nlandmarks
+  C(:, landmark_block(:,i), i) = eye(2); 
+  C(:, robot_block, i) = -eye(2);
+end
+
+% create observation cov for each value of the switch node
+Rbig = repmat(R, [1 1 nlandmarks]);
+
+% initial conditions
+init_x = zeros(Xsz, 1);
+init_v = zeros(Xsz, Xsz);
+bi = robot_block;
+init_x(bi) = init_robot_pos;
+init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
+  %init_x(bi) = true_landmark_pos(:,i);
+  %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
+end
+
+%%%%%%%%%%%%%%%%%%%%%
+% Inference
+if 1
+[xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+est_robot_pos = xsmooth(robot_block, :);
+est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
+
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  est_landmark_pos(:,i) = xsmooth(bi, T);
+  est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
+end
+end
+
+
+if 0
+figure(1); hold on
+for i=1:nlandmarks
+  h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));
+  set(h, 'color', 'r')
+end
+hold off
+
+hold on
+for t=1:T
+  h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t));
+  set(h,'color','r')
+  h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t));
+  set(h,'color','r')
+end
+hold off
+end
+
+
+if 0
+figure(3)
+if 0
+  for t=1:T
+    imagesc(inv(Vsmooth(:,:,t)))
+    colorbar
+    fprintf('t=%d; press key to continue\n', t);
+    pause
+  end
+else
+  for t=1:T
+    subplot(5,4,t)
+    imagesc(inv(Vsmooth(:,:,t)))
+  end
+end
+end
+
+
+
+
+
+%%%%%%%%%%%%%%%%%
+% DBN inference
+
+if 1
+  [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
+      mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
+  engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ...
+					 '/home/eecs/murphyk/matlab/loopyslam.txt');
+else
+  [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ...
+      mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
+  engine = jtree_dbn_inf_engine(bnet);
+end
+
+nnodes = bnet.nnodes_per_slice;
+evidence = cell(nnodes, T);
+evidence(Ynode, :) = num2cell(obs_rel_pos, 1);
+evidence(Unode, :) = num2cell(ctrl_signal, 1);
+evidence(Snode, :) = num2cell(true_data_assoc);
+
+
+[engine, ll, niter] = enter_evidence(engine, evidence);
+niter
+
+loopy_est_robot_pos = zeros(2, T);
+for t=1:T
+  m = marginal_nodes(engine, Rnode, t);
+  loopy_est_robot_pos(:,t) = m.mu;
+end
+
+for i=1:nlandmarks
+  m = marginal_nodes(engine, Lnodes(i), T);
+  loopy_est_landmark_pos(:,i) = m.mu;
+  loopy_est_landmark_pos_cov(:,:,i) = m.Sigma;
+end
+
+
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/paskin1.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/paskin1.m
new file mode 100644
index 00000000..286793d3
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/paskin1.m
@@ -0,0 +1,238 @@
+% This is like robot1, except we only use a Kalman filter.
+% The goal is to study how the precision matrix changes.
+
+seed = 1;
+rand('state', seed);
+randn('state', seed);
+
+if 0
+  T = 20;
+  ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
+		 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
+else
+  T = 60;
+  ctrl_signal = repmat([1 0]', 1, T);
+end
+
+nlandmarks = 6;
+if 0
+  true_landmark_pos = [1 1;
+		    4 1;
+		    4 4;
+		    1 4]';
+else
+  true_landmark_pos = 10*rand(2,nlandmarks);
+end
+if 0
+figure(1); clf
+hold on
+for i=1:nlandmarks
+  %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
+  plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
+end
+hold off
+end
+
+init_robot_pos = [0 0]';
+
+true_robot_pos = zeros(2, T);
+true_data_assoc = zeros(1, T);
+true_rel_dist = zeros(2, T);
+for t=1:T
+  if t>1
+    true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
+  else
+    true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
+  end
+  nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
+  %true_data_assoc(t) = nn;
+  %true_data_assoc = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ...
+  true_data_assoc  = sample_discrete(normalise(ones(1,nlandmarks)),1,T);
+  true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
+end
+
+R = 1e-3*eye(2); % noise added to observation
+Q = 1e-3*eye(2); % noise added to robot motion
+
+% Create data set
+obs_noise_seq = sample_gaussian([0 0]', R, T)';
+obs_rel_pos = true_rel_dist + obs_noise_seq;
+%obs_rel_pos = true_rel_dist;
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Create params for inference
+
+% X(t) = A X(t-1) + B U(t) + noise(Q) 
+
+% [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
+% [L2]    [  1   ]    [L2]         [0]          [ 0  ]
+% [R ]t   [     1]    [R ]t-1      [1]          [   Q]
+
+% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
+% Yt|St=1 = [1 0 -1]  * [L1]  + R
+%                       [L2]    
+%                       [R ]    
+
+% Create indices into block structure
+bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
+robot_block =  block(nlandmarks+1, bs);
+for i=1:nlandmarks
+  landmark_block(:,i) = block(i, bs)';
+end
+Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
+Ysz = 2; % observe relative location
+Usz = 2; % input is (dx, dy)
+
+
+% create block-diagonal trans matrix for each switch
+A = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  A(bi, bi) = eye(2);
+end
+bi = robot_block;
+A(bi, bi) = eye(2);
+A = repmat(A, [1 1 nlandmarks]); % same for all switch values
+
+% create block-diagonal system cov
+
+
+Qbig = zeros(Xsz, Xsz);
+bi = robot_block;
+Qbig(bi,bi) = Q; % only add noise to robot motion
+Qbig = repmat(Qbig, [1 1 nlandmarks]);
+
+% create input matrix
+B = zeros(Xsz, Usz);
+B(robot_block,:) = eye(2); % only add input to robot position
+B = repmat(B, [1 1 nlandmarks]);
+
+% create observation matrix for each value of the switch node
+% C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
+% This computes L(i) - R
+C = zeros(Ysz, Xsz, nlandmarks);
+for i=1:nlandmarks
+  C(:, landmark_block(:,i), i) = eye(2); 
+  C(:, robot_block, i) = -eye(2);
+end
+
+% create observation cov for each value of the switch node
+Rbig = repmat(R, [1 1 nlandmarks]);
+
+% initial conditions
+init_x = zeros(Xsz, 1);
+init_v = zeros(Xsz, Xsz);
+bi = robot_block;
+init_x(bi) = init_robot_pos;
+%init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
+init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
+  %init_x(bi) = true_landmark_pos(:,i);
+  %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
+end
+
+%k = nlandmarks-1; % exact
+k = 3;
+ndx = {};
+for t=1:T
+  landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
+  tmp = [landmark_block(:, landmarks) robot_block'];
+  ndx{t} = tmp(:);
+end
+
+[xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
+		       'ndx', ndx);
+
+[xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+
+if 0
+est_robot_pos = x(robot_block, :);
+est_robot_pos_cov = V(robot_block, robot_block, :);
+
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  est_landmark_pos(:,i) = x(bi, T);
+  est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
+end
+end
+
+
+
+nrows = 10;
+stepsize = T/(2*nrows);
+ts = 1:stepsize:T;
+
+if 1 % plot
+  
+clim = [0 max(max(Va(:,:,end)))];
+
+figure(2)
+if 0
+  imagesc(Ve(1:2:end,1:2:end, T))
+  clim = get(gca,'clim');
+else
+  i = 1;
+  for t=ts(:)'
+    subplot(nrows,2,i)
+    i = i + 1;
+    imagesc(Ve(1:2:end,1:2:end, t))
+    set(gca, 'clim', clim)
+    colorbar
+  end
+end
+suptitle('exact')
+
+
+figure(3)
+if 0
+  imagesc(Va(1:2:end,1:2:end, T))
+  set(gca,'clim', clim)
+else
+  i = 1;
+  for t=ts(:)'
+    subplot(nrows,2,i)
+    i = i+1;
+    imagesc(Va(1:2:end,1:2:end, t))
+    set(gca, 'clim', clim)
+    colorbar
+  end
+end
+suptitle('approx')
+
+
+figure(4)
+i = 1;
+for t=ts(:)'
+  subplot(nrows,2,i)
+  i = i+1;
+  Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
+  imagesc(Vd)
+  set(gca, 'clim', clim)
+  colorbar
+end
+suptitle('diff')
+
+end % all plot
+
+
+for t=1:T
+  i = 1:2*nlandmarks;
+  denom = Ve(i,i,t) + (Ve(i,i,t)==0);
+  Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
+  Verr(t) = max(Vd(:));
+end
+figure(6); plot(Verr)
+title('max relative Verr')
+
+for t=1:T
+  %err(t)=rms(xa(:,t), xe(:,t));
+  err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
+end
+figure(5);plot(err)
+title('rms mean pos')
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.m
new file mode 100644
index 00000000..0272d3f6
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.m
@@ -0,0 +1,155 @@
+% This is like skf_data_assoc_gmux, except the objects don't move.
+% We are uncertain of their initial positions, and get more and more observations
+% over time. The goal is to test deterministic links (0 covariance).
+% This is like robot1, except the robot doesn't move and is always at [0 0],
+% so the relative location is simply L(s).
+
+nobj = 2;
+N = nobj+2;
+Xs = 1:nobj;
+S = nobj+1;
+Y = nobj+2;
+
+intra = zeros(N,N);
+inter = zeros(N,N);
+intra([Xs S], Y) =1;
+for i=1:nobj
+  inter(Xs(i), Xs(i))=1;
+end
+
+Xsz = 2; % state space = (x y)
+Ysz = 2;
+ns = zeros(1,N);
+ns(Xs) = Xsz;
+ns(Y) = Ysz;
+ns(S) = nobj;
+
+bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);
+
+% For each object, we have
+% X(t+1) = F X(t) + noise(Q)
+% Y(t) = H X(t) + noise(R)
+F = eye(2);
+H = eye(2);
+Q = 0*eye(Xsz); % no noise in dynamics
+R = eye(Ysz);
+
+init_state{1} = [10 10]';
+init_state{2} = [10 -10]';
+init_cov = eye(2);
+
+% Uncertain of initial state (position)
+for i=1:nobj
+  bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', init_cov);
+end
+bnet.CPD{S} = root_CPD(bnet, S); % always observed
+bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
+% slice 2
+eclass = bnet.equiv_class;
+for i=1:nobj
+  bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Create LDS params 
+
+% X(t) = A X(t-1) + B U(t) + noise(Q)
+
+% [L11]  = [1  ]  * [L1]       +   [Q ]
+% [L2]     [  1]    [L2]           [ Q]
+
+% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
+% Yt|St=1 = [1 0]  * [L1]  + R
+%                    [L2]    
+
+nlandmarks = nobj;
+
+% Create indices into block structure
+bs = 2*ones(1, nobj); % sizes of blocks in state space
+for i=1:nlandmarks
+  landmark_block(:,i) = block(i, bs)';
+end
+Xsz = 2*(nlandmarks); % 2 values for each landmark plus robot
+Ysz = 2; % observe relative location
+
+% create block-diagonal trans matrix for each switch
+A = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  A(bi, bi) = eye(2);
+end
+A = repmat(A, [1 1 nlandmarks]); % same for all switch values
+
+% create block-diagonal system cov
+Qbig = zeros(Xsz, Xsz);
+Qbig = repmat(Qbig, [1 1 nlandmarks]);
+
+
+% create observation matrix for each value of the switch node
+% C(:,:,i) = (0 ... I ...) where the I is in the i'th posn.
+C = zeros(Ysz, Xsz, nlandmarks);
+for i=1:nlandmarks
+  C(:, landmark_block(:,i), i) = eye(2);
+end
+
+% create observation cov for each value of the switch node
+Rbig = repmat(R, [1 1 nlandmarks]);
+
+% initial conditions
+init_x = [init_state{1}; init_state{2}];
+init_V = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  init_V(bi,bi) = init_cov;
+end
+
+
+
+%%%%%%%%%%%%%%%%
+% Observe objects at random
+T = 10;
+evidence = cell(N, T);
+data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
+evidence(S,:) = num2cell(data_assoc);
+evidence = sample_dbn(bnet, 'evidence', evidence);
+
+
+% Inference
+ev = cell(N,T);
+ev(bnet.observed,:) = evidence(bnet.observed, :);
+y = cell2num(evidence(Y,:));
+
+engine = pearl_unrolled_dbn_inf_engine(bnet);
+engine = enter_evidence(engine, ev);
+
+loopy_est_pos = zeros(2, nlandmarks);
+loopy_est_pos_cov = zeros(2, 2, nlandmarks);
+for i=1:nobj
+  m = marginal_nodes(engine, Xs(i), T);
+  loopy_est_pos(:,i) = m.mu;
+  loopy_est_pos_cov(:,:,i) = m.Sigma;
+end
+
+
+[xsmooth, Vsmooth] = kalman_smoother(y, A, C, Qbig, Rbig, init_x, init_V, 'model', data_assoc);
+
+kf_est_pos = zeros(2, nlandmarks);
+kf_est_pos_cov = zeros(2, 2, nlandmarks);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  kf_est_pos(:,i) = xsmooth(bi, T);
+  kf_est_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
+end
+
+
+kf_est_pos
+loopy_est_pos
+
+kf_est_pos_time = zeros(2, nlandmarks, T);
+for t=1:T
+  for i=1:nlandmarks
+    bi = landmark_block(:,i);
+    kf_est_pos_time(:,i,t) = xsmooth(bi, t);
+  end
+end
+kf_est_pos_time % same for all t since smoothed
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/slam_kf.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/slam_kf.m
new file mode 100644
index 00000000..ba98140f
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/slam_kf.m
@@ -0,0 +1,172 @@
+% This is like robot1, except we only use a Kalman filter.
+% The goal is to study how the precision matrix changes.
+
+seed = 0;
+rand('state', seed);
+randn('state', seed);
+
+if 0
+  T = 20;
+  ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
+		 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
+else
+  T = 12;
+  ctrl_signal = repmat([1 0]', 1, T);
+end
+
+nlandmarks = 6;
+if 0
+  true_landmark_pos = [1 1;
+		    4 1;
+		    4 4;
+		    1 4]';
+else
+  true_landmark_pos = 10*rand(2,nlandmarks);
+end
+figure(1); clf
+hold on
+for i=1:nlandmarks
+  %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
+  plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
+end
+hold off
+
+init_robot_pos = [0 0]';
+
+true_robot_pos = zeros(2, T);
+true_data_assoc = zeros(1, T);
+true_rel_dist = zeros(2, T);
+for t=1:T
+  if t>1
+    true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
+  else
+    true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
+  end
+  %nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
+  nn = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ...
+  true_data_assoc(t) = nn;
+  true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
+end
+
+R = 1e-3*eye(2); % noise added to observation
+Q = 1e-3*eye(2); % noise added to robot motion
+
+% Create data set
+obs_noise_seq = sample_gaussian([0 0]', R, T)';
+obs_rel_pos = true_rel_dist + obs_noise_seq;
+%obs_rel_pos = true_rel_dist;
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Create params for inference
+
+% X(t) = A X(t-1) + B U(t) + noise(Q)
+
+% [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
+% [L2]    [  1   ]    [L2]         [0]          [ 0  ]
+% [R ]t   [     1]    [R ]t-1      [1]          [   Q]
+
+% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
+% Yt|St=1 = [1 0 -1]  * [L1]  + R
+%                       [L2]    
+%                       [R ]    
+
+% Create indices into block structure
+bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
+robot_block =  block(nlandmarks+1, bs);
+for i=1:nlandmarks
+  landmark_block(:,i) = block(i, bs)';
+end
+Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
+Ysz = 2; % observe relative location
+Usz = 2; % input is (dx, dy)
+
+
+% create block-diagonal trans matrix for each switch
+A = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  A(bi, bi) = eye(2);
+end
+bi = robot_block;
+A(bi, bi) = eye(2);
+A = repmat(A, [1 1 nlandmarks]); % same for all switch values
+
+% create block-diagonal system cov
+
+
+Qbig = zeros(Xsz, Xsz);
+bi = robot_block;
+Qbig(bi,bi) = Q; % only add noise to robot motion
+Qbig = repmat(Qbig, [1 1 nlandmarks]);
+
+% create input matrix
+B = zeros(Xsz, Usz);
+B(robot_block,:) = eye(2); % only add input to robot position
+B = repmat(B, [1 1 nlandmarks]);
+
+% create observation matrix for each value of the switch node
+% C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
+% This computes L(i) - R
+C = zeros(Ysz, Xsz, nlandmarks);
+for i=1:nlandmarks
+  C(:, landmark_block(:,i), i) = eye(2); 
+  C(:, robot_block, i) = -eye(2);
+end
+
+% create observation cov for each value of the switch node
+Rbig = repmat(R, [1 1 nlandmarks]);
+
+% initial conditions
+init_x = zeros(Xsz, 1);
+init_v = zeros(Xsz, Xsz);
+bi = robot_block;
+init_x(bi) = init_robot_pos;
+init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
+  %init_x(bi) = true_landmark_pos(:,i);
+  %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
+end
+
+[xsmooth, Vsmooth] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+est_robot_pos = xsmooth(robot_block, :);
+est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
+
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  est_landmark_pos(:,i) = xsmooth(bi, T);
+  est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
+end
+
+
+
+P = zeros(size(Vsmooth));
+for t=1:T
+  P(:,:,t) = inv(Vsmooth(:,:,t));
+end
+
+figure(1)
+for t=1:T
+  subplot(T/2,2,t)
+  imagesc(P(1:2:end,1:2:end, t))
+  colorbar
+end
+
+figure(2)
+for t=1:T
+  subplot(T/2,2,t)
+  imagesc(Vsmooth(1:2:end,1:2:end, t))
+  colorbar
+end
+
+
+
+% marginalize out robot position and then check structure
+bi = landmark_block(:);
+V = Vsmooth(bi,bi,T); 
+P = inv(V);
+P(1:2:end,1:2:end)
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/mk_gmux_robot_dbn.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/mk_gmux_robot_dbn.m
new file mode 100644
index 00000000..8ee3a7ca
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/mk_gmux_robot_dbn.m
@@ -0,0 +1,85 @@
+function [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
+    mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block)
+
+% Make DBN
+
+% S
+% | L1 -------> L1'
+% |  | L2 ----------> L2'
+% \  | /
+%  v v v
+%    Ls
+%    |
+%    v
+%    Y
+%    ^
+%    |
+%    R ------->  R'
+%    ^      
+%    |      
+%    U      
+%
+%
+% S is a switch, Ls is a deterministic gmux, Y = Ls-R,
+% R(t+1) = R(t) + U(t+1), L(t+1) = L(t)
+
+
+% number nodes topologically
+Snode = 1;
+Lnodes = 2:nlandmarks+1;
+Lsnode = nlandmarks+2;
+Unode = nlandmarks+3;
+Rnode = nlandmarks+4;
+Ynode = nlandmarks+5;
+
+nnodes = nlandmarks+5; 
+intra = zeros(nnodes, nnodes);
+intra([Snode Lnodes], Lsnode) =1;
+intra(Unode,Rnode)=1;
+intra([Rnode Lsnode], Ynode)=1;
+
+inter = zeros(nnodes, nnodes);
+inter(Rnode, Rnode)=1;
+for i=1:nlandmarks
+  inter(Lnodes(i), Lnodes(i))=1;
+end
+
+Lsz = 2; % (x y) posn of landmark
+Rsz = 2; % (x y) posn of robot
+Ysz = 2; % relative distance
+Usz = 2; % (dx dy) ctrl
+Ssz = nlandmarks; % can switch between any landmark
+
+ns = zeros(1,nnodes);
+ns(Snode) = Ssz;
+ns(Lnodes) = Lsz;
+ns(Lsnode) = Lsz;
+ns(Ynode) = Ysz;
+ns(Rnode) = Rsz;
+ns(Ynode) = Usz;
+ns(Unode) = Usz;
+
+bnet = mk_dbn(intra, inter, ns, 'discrete', Snode, 'observed', [Snode Ynode Unode]);
+
+
+bnet.CPD{Snode} = root_CPD(bnet, Snode); % always observed
+bnet.CPD{Unode} = root_CPD(bnet, Unode); % always observed
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  bnet.CPD{Lnodes(i)} = gaussian_CPD(bnet, Lnodes(i), 'mean', init_x(bi), 'cov', init_V(bi,bi));
+end
+bi = robot_block;
+bnet.CPD{Rnode} = gaussian_CPD(bnet, Rnode, 'mean', init_x(bi), 'cov', init_V(bi,bi), 'weights', eye(2));
+bnet.CPD{Lsnode} = gmux_CPD(bnet, Lsnode, 'cov', repmat(zeros(Lsz,Lsz), [1 1 nlandmarks]), ...
+			    'weights', repmat(eye(Lsz,Lsz), [1 1 nlandmarks]));
+W = [eye(2) -eye(2)]; % Y = Ls - R, where Ls is the lower-numbered parent
+bnet.CPD{Ynode} = gaussian_CPD(bnet, Ynode, 'mean', zeros(Ysz,1), 'cov', R, 'weights', W);
+
+% slice 2
+eclass = bnet.equiv_class;
+W = [eye(2) eye(2)]; % R(t) = R(t-1) + U(t), where R(t-1) is the lower-numbered parent
+bnet.CPD{eclass(Rnode,2)} = gaussian_CPD(bnet, Rnode+nnodes, 'mean', zeros(Rsz,1), 'cov', Q, 'weights', W);
+for i=1:nlandmarks
+  bnet.CPD{eclass(Lnodes(i), 2)} = gaussian_CPD(bnet, Lnodes(i)+nnodes, 'mean', zeros(2,1), ...
+						   'cov', zeros(2,2), 'weights', eye(2));
+end
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/mk_linear_slam.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/mk_linear_slam.m
new file mode 100644
index 00000000..b8a819a2
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/mk_linear_slam.m
@@ -0,0 +1,164 @@
+function [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
+	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
+	  obs_rel_pos, ctrl_signal] = mk_linear_slam(varargin)
+
+% We create data from a linear system for testing SLAM algorithms.
+% i.e. , new robot pos = old robot pos + ctrl_signal, which is just a displacement vector.
+% and  observation = landmark_pos - robot_pos, which is just a displacement vector.
+%
+% The behavior is determined by the following optional arguments:
+%
+% 'nlandmarks' - num. landmarks
+% 'landmarks' - 'rnd' means random locations in the unit sqyare
+%               'square' means at [1 1], [4 1], [4 4] and [1 4]
+% 'T' - num steps to run
+% 'ctrl' - 'stationary' means the robot remains at [0 0],
+%          'leftright' means the robot receives a constant contol of [1 0],
+%          'square' means we navigate the robot around the square
+% 'data-assoc' - 'rnd' means we observe landmarks at random
+%                'nn' means we observe the nearest neighbor landmark
+%                'cycle' means we observe landmarks in order 1,2,.., 1, 2, ...
+
+args = varargin;
+% get mandatory params
+for i=1:2:length(args)
+  switch args{i},
+   case 'nlandmarks', nlandmarks = args{i+1};
+   case 'T', T = args{i+1};
+  end
+end
+
+% set defaults
+true_landmark_pos = rand(2,nlandmarks);
+true_data_assoc = [];
+
+% get args
+for i=1:2:length(args)
+  switch args{i},
+   case 'landmarks',
+    switch args{i+1},
+     case 'rnd',   true_landmark_pos = rand(2,nlandmarks);
+     case 'square',   true_landmark_pos = [1 1; 4 1; 4 4; 1 4]';
+    end
+   case 'ctrl',
+    switch args{i+1},
+     case 'stationary', ctrl_signal = repmat([0 0]', 1, T);
+     case 'leftright', ctrl_signal = repmat([1 0]', 1, T);
+     case 'square',   ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
+		    repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
+    end
+   case 'data-assoc', 
+    switch args{i+1},
+     case 'rnd', true_data_assoc  = sample_discrete(normalise(ones(1,nlandmarks)),1,T);
+     case 'cycle', true_data_assoc = wrap(1:T, nlandmarks);
+    end
+  end
+end
+if isempty(true_data_assoc)
+  use_nn = 1;
+else
+  use_nn = 0;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%
+% generate data
+
+init_robot_pos = [0 0]';
+true_robot_pos = zeros(2, T);
+true_rel_dist = zeros(2, T);
+for t=1:T
+  if t>1
+    true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
+  else
+    true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
+  end
+  nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
+  if use_nn
+    true_data_assoc(t) = nn;
+  end
+  true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
+end
+
+
+R = 1e-3*eye(2); % noise added to observation
+Q = 1e-3*eye(2); % noise added to robot motion
+
+% Create data set
+obs_noise_seq = sample_gaussian([0 0]', R, T)';
+obs_rel_pos = true_rel_dist + obs_noise_seq;
+%obs_rel_pos = true_rel_dist;
+
+%%%%%%%%%%%%%%%%%%
+% Create params
+
+
+% X(t) = A X(t-1) + B U(t) + noise(Q) 
+
+% [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
+% [L2]    [  1   ]    [L2]         [0]          [ 0  ]
+% [R ]t   [     1]    [R ]t-1      [1]          [   Q]
+
+% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
+% Yt|St=1 = [1 0 -1]  * [L1]  + R
+%                       [L2]    
+%                       [R ]    
+
+% Create indices into block structure
+bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
+robot_block =  block(nlandmarks+1, bs);
+for i=1:nlandmarks
+  landmark_block(:,i) = block(i, bs)';
+end
+Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
+Ysz = 2; % observe relative location
+Usz = 2; % input is (dx, dy)
+
+
+% create block-diagonal trans matrix for each switch
+A = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  A(bi, bi) = eye(2);
+end
+bi = robot_block;
+A(bi, bi) = eye(2);
+A = repmat(A, [1 1 nlandmarks]); % same for all switch values
+
+% create block-diagonal system cov
+
+
+Qbig = zeros(Xsz, Xsz);
+bi = robot_block;
+Qbig(bi,bi) = Q; % only add noise to robot motion
+Qbig = repmat(Qbig, [1 1 nlandmarks]);
+
+% create input matrix
+B = zeros(Xsz, Usz);
+B(robot_block,:) = eye(2); % only add input to robot position
+B = repmat(B, [1 1 nlandmarks]);
+
+% create observation matrix for each value of the switch node
+% C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
+% This computes L(i) - R
+C = zeros(Ysz, Xsz, nlandmarks);
+for i=1:nlandmarks
+  C(:, landmark_block(:,i), i) = eye(2); 
+  C(:, robot_block, i) = -eye(2);
+end
+
+% create observation cov for each value of the switch node
+Rbig = repmat(R, [1 1 nlandmarks]);
+
+% initial conditions
+init_x = zeros(Xsz, 1);
+init_v = zeros(Xsz, Xsz);
+bi = robot_block;
+init_x(bi) = init_robot_pos;
+%init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
+init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
+  %init_x(bi) = true_landmark_pos(:,i);
+  %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
+end
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_kf.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_kf.m
new file mode 100644
index 00000000..9844352b
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_kf.m
@@ -0,0 +1,78 @@
+% Plot how precision matrix changes over time for KF solution
+
+seed = 0;
+rand('state', seed);
+randn('state', seed);
+
+[A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
+	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
+	  obs_rel_pos, ctrl_signal] = mk_linear_slam(...
+	      'nlandmarks', 6, 'T', 12, 'ctrl', 'leftright', 'data-assoc', 'cycle');
+
+figure(1); clf
+hold on
+for i=1:nlandmarks
+  %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
+  plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
+end
+hold off
+
+
+[x, V] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+est_robot_pos = x(robot_block, :);
+est_robot_pos_cov = V(robot_block, robot_block, :);
+
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  est_landmark_pos(:,i) = x(bi, T);
+  est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
+end
+
+
+if 0
+figure(1); hold on
+for i=1:nlandmarks
+  h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));
+  set(h, 'color', 'r')
+end
+hold off
+
+hold on
+for t=1:T
+  h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t));
+  set(h,'color','r')
+  h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t));
+  set(h,'color','r')
+end
+hold off
+end
+
+
+P = zeros(size(V));
+for t=1:T
+  P(:,:,t) = inv(V(:,:,t));
+end
+
+if 0
+  figure(2)
+  for t=1:T
+    subplot(T/2,2,t)
+    imagesc(P(1:2:end,1:2:end, t))
+    colorbar
+  end
+else
+  figure(2)
+  for t=1:T
+    subplot(T/2,2,t)
+    imagesc(V(1:2:end,1:2:end, t))
+    colorbar
+  end
+end
+
+% marginalize out robot position and then check structure
+bi = landmark_block(:);
+V = V(bi,bi,T); 
+P = inv(V);
+P(1:2:end,1:2:end)
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_offline_loopy.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_offline_loopy.m
new file mode 100644
index 00000000..6abc0fe0
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_offline_loopy.m
@@ -0,0 +1,59 @@
+% Compare Kalman smoother with loopy
+
+seed = 0;
+rand('state', seed);
+randn('state', seed);
+nlandmarks = 6;
+T = 12;
+
+[A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
+	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
+	  obs_rel_pos, ctrl_signal] = mk_linear_slam(...
+	      'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle');
+
+[xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+est_robot_pos = xsmooth(robot_block, :);
+est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
+
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  est_landmark_pos(:,i) = xsmooth(bi, T);
+  est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
+end
+
+
+if 1
+  [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
+      mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
+  engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ...
+					 '/home/eecs/murphyk/matlab/loopyslam.txt');
+else
+  [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ...
+      mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
+  engine = jtree_dbn_inf_engine(bnet);
+end
+
+nnodes = bnet.nnodes_per_slice;
+evidence = cell(nnodes, T);
+evidence(Ynode, :) = num2cell(obs_rel_pos, 1);
+evidence(Unode, :) = num2cell(ctrl_signal, 1);
+evidence(Snode, :) = num2cell(true_data_assoc);
+
+[engine, ll, niter] = enter_evidence(engine, evidence);
+niter
+
+loopy_est_robot_pos = zeros(2, T);
+for t=1:T
+  m = marginal_nodes(engine, Rnode, t);
+  loopy_est_robot_pos(:,t) = m.mu;
+end
+
+for i=1:nlandmarks
+  m = marginal_nodes(engine, Lnodes(i), T);
+  loopy_est_landmark_pos(:,i) = m.mu;
+  loopy_est_landmark_pos_cov(:,:,i) = m.Sigma;
+end
+
+
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_partial_kf.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_partial_kf.m
new file mode 100644
index 00000000..3fe998be
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_partial_kf.m
@@ -0,0 +1,107 @@
+% See how well partial Kalman filter updates work
+
+seed = 0;
+rand('state', seed);
+randn('state', seed);
+nlandmarks = 6;
+T = 12;
+
+[A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
+	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
+	  obs_rel_pos, ctrl_signal] = mk_linear_slam(...
+	      'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle');
+
+% exact
+[xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
+
+
+% approx
+%k = nlandmarks-1; % exact
+k = 3;
+ndx = {};
+for t=1:T
+  landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
+  tmp = [landmark_block(:, landmarks) robot_block'];
+  ndx{t} = tmp(:);
+end
+
+[xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
+			 'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
+		       'ndx', ndx);
+
+
+
+nrows = 10;
+stepsize = T/(2*nrows);
+ts = 1:stepsize:T;
+
+if 1 % plot
+  
+clim = [0 max(max(Va(:,:,end)))];
+
+figure(2)
+if 0
+  imagesc(Ve(1:2:end,1:2:end, T))
+  clim = get(gca,'clim');
+else
+  i = 1;
+  for t=ts(:)'
+    subplot(nrows,2,i)
+    i = i + 1;
+    imagesc(Ve(1:2:end,1:2:end, t))
+    set(gca, 'clim', clim)
+    colorbar
+  end
+end
+suptitle('exact')
+
+
+figure(3)
+if 0
+  imagesc(Va(1:2:end,1:2:end, T))
+  set(gca,'clim', clim)
+else
+  i = 1;
+  for t=ts(:)'
+    subplot(nrows,2,i)
+    i = i+1;
+    imagesc(Va(1:2:end,1:2:end, t))
+    set(gca, 'clim', clim)
+    colorbar
+  end
+end
+suptitle('approx')
+
+
+figure(4)
+i = 1;
+for t=ts(:)'
+  subplot(nrows,2,i)
+  i = i+1;
+  Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
+  imagesc(Vd)
+  set(gca, 'clim', clim)
+  colorbar
+end
+suptitle('diff')
+
+end % all plot
+
+
+for t=1:T
+  %err(t)=rms(xa(:,t), xe(:,t));
+  err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
+end
+figure(5);plot(err)
+title('rms mean pos')
+
+
+for t=1:T
+  i = 1:2*nlandmarks;
+  denom = Ve(i,i,t) + (Ve(i,i,t)==0);
+  Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
+  Verr(t) = max(Vd(:));
+end
+figure(6); plot(Verr)
+title('max relative Verr')
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_stationary_loopy.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_stationary_loopy.m
new file mode 100644
index 00000000..0272d3f6
--- /dev/null
+++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/slam_stationary_loopy.m
@@ -0,0 +1,155 @@
+% This is like skf_data_assoc_gmux, except the objects don't move.
+% We are uncertain of their initial positions, and get more and more observations
+% over time. The goal is to test deterministic links (0 covariance).
+% This is like robot1, except the robot doesn't move and is always at [0 0],
+% so the relative location is simply L(s).
+
+nobj = 2;
+N = nobj+2;
+Xs = 1:nobj;
+S = nobj+1;
+Y = nobj+2;
+
+intra = zeros(N,N);
+inter = zeros(N,N);
+intra([Xs S], Y) =1;
+for i=1:nobj
+  inter(Xs(i), Xs(i))=1;
+end
+
+Xsz = 2; % state space = (x y)
+Ysz = 2;
+ns = zeros(1,N);
+ns(Xs) = Xsz;
+ns(Y) = Ysz;
+ns(S) = nobj;
+
+bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);
+
+% For each object, we have
+% X(t+1) = F X(t) + noise(Q)
+% Y(t) = H X(t) + noise(R)
+F = eye(2);
+H = eye(2);
+Q = 0*eye(Xsz); % no noise in dynamics
+R = eye(Ysz);
+
+init_state{1} = [10 10]';
+init_state{2} = [10 -10]';
+init_cov = eye(2);
+
+% Uncertain of initial state (position)
+for i=1:nobj
+  bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', init_cov);
+end
+bnet.CPD{S} = root_CPD(bnet, S); % always observed
+bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
+% slice 2
+eclass = bnet.equiv_class;
+for i=1:nobj
+  bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Create LDS params 
+
+% X(t) = A X(t-1) + B U(t) + noise(Q)
+
+% [L11]  = [1  ]  * [L1]       +   [Q ]
+% [L2]     [  1]    [L2]           [ Q]
+
+% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
+% Yt|St=1 = [1 0]  * [L1]  + R
+%                    [L2]    
+
+nlandmarks = nobj;
+
+% Create indices into block structure
+bs = 2*ones(1, nobj); % sizes of blocks in state space
+for i=1:nlandmarks
+  landmark_block(:,i) = block(i, bs)';
+end
+Xsz = 2*(nlandmarks); % 2 values for each landmark plus robot
+Ysz = 2; % observe relative location
+
+% create block-diagonal trans matrix for each switch
+A = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  A(bi, bi) = eye(2);
+end
+A = repmat(A, [1 1 nlandmarks]); % same for all switch values
+
+% create block-diagonal system cov
+Qbig = zeros(Xsz, Xsz);
+Qbig = repmat(Qbig, [1 1 nlandmarks]);
+
+
+% create observation matrix for each value of the switch node
+% C(:,:,i) = (0 ... I ...) where the I is in the i'th posn.
+C = zeros(Ysz, Xsz, nlandmarks);
+for i=1:nlandmarks
+  C(:, landmark_block(:,i), i) = eye(2);
+end
+
+% create observation cov for each value of the switch node
+Rbig = repmat(R, [1 1 nlandmarks]);
+
+% initial conditions
+init_x = [init_state{1}; init_state{2}];
+init_V = zeros(Xsz, Xsz);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  init_V(bi,bi) = init_cov;
+end
+
+
+
+%%%%%%%%%%%%%%%%
+% Observe objects at random
+T = 10;
+evidence = cell(N, T);
+data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
+evidence(S,:) = num2cell(data_assoc);
+evidence = sample_dbn(bnet, 'evidence', evidence);
+
+
+% Inference
+ev = cell(N,T);
+ev(bnet.observed,:) = evidence(bnet.observed, :);
+y = cell2num(evidence(Y,:));
+
+engine = pearl_unrolled_dbn_inf_engine(bnet);
+engine = enter_evidence(engine, ev);
+
+loopy_est_pos = zeros(2, nlandmarks);
+loopy_est_pos_cov = zeros(2, 2, nlandmarks);
+for i=1:nobj
+  m = marginal_nodes(engine, Xs(i), T);
+  loopy_est_pos(:,i) = m.mu;
+  loopy_est_pos_cov(:,:,i) = m.Sigma;
+end
+
+
+[xsmooth, Vsmooth] = kalman_smoother(y, A, C, Qbig, Rbig, init_x, init_V, 'model', data_assoc);
+
+kf_est_pos = zeros(2, nlandmarks);
+kf_est_pos_cov = zeros(2, 2, nlandmarks);
+for i=1:nlandmarks
+  bi = landmark_block(:,i);
+  kf_est_pos(:,i) = xsmooth(bi, T);
+  kf_est_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
+end
+
+
+kf_est_pos
+loopy_est_pos
+
+kf_est_pos_time = zeros(2, nlandmarks, T);
+for t=1:T
+  for i=1:nlandmarks
+    bi = landmark_block(:,i);
+    kf_est_pos_time(:,i,t) = xsmooth(bi, t);
+  end
+end
+kf_est_pos_time % same for all t since smoothed