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

7 files changed, 803 insertions, 0 deletions

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)