wolffd@0: % Compare Kalman smoother with loopy
wolffd@0: 
wolffd@0: seed = 0;
wolffd@0: rand('state', seed);
wolffd@0: randn('state', seed);
wolffd@0: nlandmarks = 6;
wolffd@0: T = 12;
wolffd@0: 
wolffd@0: [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
wolffd@0: 	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
wolffd@0: 	  obs_rel_pos, ctrl_signal] = mk_linear_slam(...
wolffd@0: 	      'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle');
wolffd@0: 
wolffd@0: [xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
wolffd@0: 				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
wolffd@0: 
wolffd@0: est_robot_pos = xsmooth(robot_block, :);
wolffd@0: est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
wolffd@0: 
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   est_landmark_pos(:,i) = xsmooth(bi, T);
wolffd@0:   est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: if 1
wolffd@0:   [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:   engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ...
wolffd@0: 					 '/home/eecs/murphyk/matlab/loopyslam.txt');
wolffd@0: else
wolffd@0:   [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ...
wolffd@0:       mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
wolffd@0:   engine = jtree_dbn_inf_engine(bnet);
wolffd@0: end
wolffd@0: 
wolffd@0: nnodes = bnet.nnodes_per_slice;
wolffd@0: evidence = cell(nnodes, T);
wolffd@0: evidence(Ynode, :) = num2cell(obs_rel_pos, 1);
wolffd@0: evidence(Unode, :) = num2cell(ctrl_signal, 1);
wolffd@0: evidence(Snode, :) = num2cell(true_data_assoc);
wolffd@0: 
wolffd@0: [engine, ll, niter] = enter_evidence(engine, evidence);
wolffd@0: niter
wolffd@0: 
wolffd@0: loopy_est_robot_pos = zeros(2, T);
wolffd@0: for t=1:T
wolffd@0:   m = marginal_nodes(engine, Rnode, t);
wolffd@0:   loopy_est_robot_pos(:,t) = m.mu;
wolffd@0: end
wolffd@0: 
wolffd@0: for i=1:nlandmarks
wolffd@0:   m = marginal_nodes(engine, Lnodes(i), T);
wolffd@0:   loopy_est_landmark_pos(:,i) = m.mu;
wolffd@0:   loopy_est_landmark_pos_cov(:,:,i) = m.Sigma;
wolffd@0: end
wolffd@0: 
wolffd@0: