Daniel@0: % We navigate a robot around a square using a fixed control policy and no noise.
Daniel@0: % We assume the robot observes the relative distance to the nearest landmark.
Daniel@0: % Everything is linear-Gaussian.
Daniel@0: 
Daniel@0: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Daniel@0: % Create toy data set
Daniel@0: 
Daniel@0: seed = 0;
Daniel@0: rand('state', seed);
Daniel@0: randn('state', seed);
Daniel@0: 
Daniel@0: if 1
Daniel@0:   T = 20;
Daniel@0:   ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
Daniel@0: 		 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
Daniel@0: else
Daniel@0:   T = 5;
Daniel@0:   ctrl_signal = repmat([1 0]', 1, T);
Daniel@0: end
Daniel@0: 
Daniel@0: nlandmarks = 4;
Daniel@0: true_landmark_pos = [1 1;
Daniel@0: 		     4 1;
Daniel@0: 		     4 4;
Daniel@0: 		     1 4]';
Daniel@0: init_robot_pos = [0 0]';
Daniel@0: 
Daniel@0: true_robot_pos = zeros(2, T);
Daniel@0: true_data_assoc = zeros(1, T);
Daniel@0: true_rel_dist = zeros(2, T);
Daniel@0: for t=1:T
Daniel@0:   if t>1
Daniel@0:     true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
Daniel@0:   else
Daniel@0:     true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
Daniel@0:   end
Daniel@0:   nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
Daniel@0:   %nn = t; % observe 1, 2, 3
Daniel@0:   true_data_assoc(t) = nn;
Daniel@0:   true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
Daniel@0: end
Daniel@0: 
Daniel@0: figure(1);
Daniel@0: %clf; 
Daniel@0: hold on
Daniel@0: %plot(true_landmark_pos(1,:), true_landmark_pos(2,:), '*');
Daniel@0: for i=1:nlandmarks
Daniel@0:   text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
Daniel@0: end
Daniel@0: for t=1:T
Daniel@0:   text(true_robot_pos(1,t), true_robot_pos(2,t), sprintf('%d',t));
Daniel@0: end
Daniel@0: hold off
Daniel@0: axis([-1 6 -1 6])
Daniel@0: 
Daniel@0: R = 1e-3*eye(2); % noise added to observation
Daniel@0: Q = 1e-3*eye(2); % noise added to robot motion
Daniel@0: 
Daniel@0: % Create data set
Daniel@0: obs_noise_seq = sample_gaussian([0 0]', R, T)';
Daniel@0: obs_rel_pos = true_rel_dist + obs_noise_seq;
Daniel@0: %obs_rel_pos = true_rel_dist;
Daniel@0: 
Daniel@0: 
Daniel@0: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Daniel@0: % Create params for inference
Daniel@0: 
Daniel@0: % X(t) = A X(t-1) + B U(t) + noise(Q)
Daniel@0: 
Daniel@0: % [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
Daniel@0: % [L2]    [  1   ]    [L2]         [0]          [ 0  ]
Daniel@0: % [R ]t   [     1]    [R ]t-1      [1]          [   Q]
Daniel@0: 
Daniel@0: % Y(t)|S(t)=s  = C(s) X(t) + noise(R)
Daniel@0: % Yt|St=1 = [1 0 -1]  * [L1]  + R
Daniel@0: %                       [L2]    
Daniel@0: %                       [R ]    
Daniel@0: 
Daniel@0: % Create indices into block structure
Daniel@0: bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
Daniel@0: robot_block =  block(nlandmarks+1, bs);
Daniel@0: for i=1:nlandmarks
Daniel@0:   landmark_block(:,i) = block(i, bs)';
Daniel@0: end
Daniel@0: Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
Daniel@0: Ysz = 2; % observe relative location
Daniel@0: Usz = 2; % input is (dx, dy)
Daniel@0: 
Daniel@0: 
Daniel@0: % create block-diagonal trans matrix for each switch
Daniel@0: A = zeros(Xsz, Xsz);
Daniel@0: for i=1:nlandmarks
Daniel@0:   bi = landmark_block(:,i);
Daniel@0:   A(bi, bi) = eye(2);
Daniel@0: end
Daniel@0: bi = robot_block;
Daniel@0: A(bi, bi) = eye(2);
Daniel@0: A = repmat(A, [1 1 nlandmarks]); % same for all switch values
Daniel@0: 
Daniel@0: % create block-diagonal system cov
Daniel@0: 
Daniel@0: 
Daniel@0: Qbig = zeros(Xsz, Xsz);
Daniel@0: bi = robot_block;
Daniel@0: Qbig(bi,bi) = Q; % only add noise to robot motion
Daniel@0: Qbig = repmat(Qbig, [1 1 nlandmarks]);
Daniel@0: 
Daniel@0: % create input matrix
Daniel@0: B = zeros(Xsz, Usz);
Daniel@0: B(robot_block,:) = eye(2); % only add input to robot position
Daniel@0: B = repmat(B, [1 1 nlandmarks]);
Daniel@0: 
Daniel@0: % create observation matrix for each value of the switch node
Daniel@0: % C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
Daniel@0: % This computes L(i) - R
Daniel@0: C = zeros(Ysz, Xsz, nlandmarks);
Daniel@0: for i=1:nlandmarks
Daniel@0:   C(:, landmark_block(:,i), i) = eye(2); 
Daniel@0:   C(:, robot_block, i) = -eye(2);
Daniel@0: end
Daniel@0: 
Daniel@0: % create observation cov for each value of the switch node
Daniel@0: Rbig = repmat(R, [1 1 nlandmarks]);
Daniel@0: 
Daniel@0: % initial conditions
Daniel@0: init_x = zeros(Xsz, 1);
Daniel@0: init_v = zeros(Xsz, Xsz);
Daniel@0: bi = robot_block;
Daniel@0: init_x(bi) = init_robot_pos;
Daniel@0: init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
Daniel@0: for i=1:nlandmarks
Daniel@0:   bi = landmark_block(:,i);
Daniel@0:   init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
Daniel@0:   %init_x(bi) = true_landmark_pos(:,i);
Daniel@0:   %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
Daniel@0: end
Daniel@0: 
Daniel@0: %%%%%%%%%%%%%%%%%%%%%
Daniel@0: % Inference
Daniel@0: if 1
Daniel@0: [xsmooth, Vsmooth] = kalman_smoother(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
Daniel@0: 				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
Daniel@0: 
Daniel@0: est_robot_pos = xsmooth(robot_block, :);
Daniel@0: est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
Daniel@0: 
Daniel@0: for i=1:nlandmarks
Daniel@0:   bi = landmark_block(:,i);
Daniel@0:   est_landmark_pos(:,i) = xsmooth(bi, T);
Daniel@0:   est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
Daniel@0: end
Daniel@0: end
Daniel@0: 
Daniel@0: 
Daniel@0: if 0
Daniel@0: figure(1); hold on
Daniel@0: for i=1:nlandmarks
Daniel@0:   h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));
Daniel@0:   set(h, 'color', 'r')
Daniel@0: end
Daniel@0: hold off
Daniel@0: 
Daniel@0: hold on
Daniel@0: for t=1:T
Daniel@0:   h=plotgauss2d(est_robot_pos(:,t), est_robot_pos_cov(:,:,t));
Daniel@0:   set(h,'color','r')
Daniel@0:   h=text(est_robot_pos(1,t), est_robot_pos(2,2), sprintf('R%d', t));
Daniel@0:   set(h,'color','r')
Daniel@0: end
Daniel@0: hold off
Daniel@0: end
Daniel@0: 
Daniel@0: 
Daniel@0: if 0
Daniel@0: figure(3)
Daniel@0: if 0
Daniel@0:   for t=1:T
Daniel@0:     imagesc(inv(Vsmooth(:,:,t)))
Daniel@0:     colorbar
Daniel@0:     fprintf('t=%d; press key to continue\n', t);
Daniel@0:     pause
Daniel@0:   end
Daniel@0: else
Daniel@0:   for t=1:T
Daniel@0:     subplot(5,4,t)
Daniel@0:     imagesc(inv(Vsmooth(:,:,t)))
Daniel@0:   end
Daniel@0: end
Daniel@0: end
Daniel@0: 
Daniel@0: 
Daniel@0: 
Daniel@0: 
Daniel@0: 
Daniel@0: %%%%%%%%%%%%%%%%%
Daniel@0: % DBN inference
Daniel@0: 
Daniel@0: if 1
Daniel@0:   [bnet, Unode, Snode, Lnodes, Rnode, Ynode, Lsnode] = ...
Daniel@0:       mk_gmux_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
Daniel@0:   engine = pearl_unrolled_dbn_inf_engine(bnet, 'max_iter', 50, 'filename', ...
Daniel@0: 					 '/home/eecs/murphyk/matlab/loopyslam.txt');
Daniel@0: else
Daniel@0:   [bnet, Unode, Snode, Lnodes, Rnode, Ynode] = ...
Daniel@0:       mk_gmux2_robot_dbn(nlandmarks, Q, R, init_x, init_V, robot_block, landmark_block);
Daniel@0:   engine = jtree_dbn_inf_engine(bnet);
Daniel@0: end
Daniel@0: 
Daniel@0: nnodes = bnet.nnodes_per_slice;
Daniel@0: evidence = cell(nnodes, T);
Daniel@0: evidence(Ynode, :) = num2cell(obs_rel_pos, 1);
Daniel@0: evidence(Unode, :) = num2cell(ctrl_signal, 1);
Daniel@0: evidence(Snode, :) = num2cell(true_data_assoc);
Daniel@0: 
Daniel@0: 
Daniel@0: [engine, ll, niter] = enter_evidence(engine, evidence);
Daniel@0: niter
Daniel@0: 
Daniel@0: loopy_est_robot_pos = zeros(2, T);
Daniel@0: for t=1:T
Daniel@0:   m = marginal_nodes(engine, Rnode, t);
Daniel@0:   loopy_est_robot_pos(:,t) = m.mu;
Daniel@0: end
Daniel@0: 
Daniel@0: for i=1:nlandmarks
Daniel@0:   m = marginal_nodes(engine, Lnodes(i), T);
Daniel@0:   loopy_est_landmark_pos(:,i) = m.mu;
Daniel@0:   loopy_est_landmark_pos_cov(:,:,i) = m.Sigma;
Daniel@0: end
Daniel@0: 
Daniel@0: