% 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 setseed = 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);endnlandmarks = 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);endfigure(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));endfor t=1:T  text(true_robot_pos(1,t), true_robot_pos(2,t), sprintf('%d',t));endhold offaxis([-1 6 -1 6])R = 1e-3*eye(2); % noise added to observationQ = 1e-3*eye(2); % noise added to robot motion% Create data setobs_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 structurebs = 2*ones(1, nlandmarks+1); % sizes of blocks in state spacerobot_block =  block(nlandmarks+1, bs);for i=1:nlandmarks  landmark_block(:,i) = block(i, bs)';endXsz = 2*(nlandmarks+1); % 2 values for each landmark plus robotYsz = 2; % observe relative locationUsz = 2; % input is (dx, dy)% create block-diagonal trans matrix for each switchA = zeros(Xsz, Xsz);for i=1:nlandmarks  bi = landmark_block(:,i);  A(bi, bi) = eye(2);endbi = robot_block;A(bi, bi) = eye(2);A = repmat(A, [1 1 nlandmarks]); % same for all switch values% create block-diagonal system covQbig = zeros(Xsz, Xsz);bi = robot_block;Qbig(bi,bi) = Q; % only add noise to robot motionQbig = repmat(Qbig, [1 1 nlandmarks]);% create input matrixB = zeros(Xsz, Usz);B(robot_block,:) = eye(2); % only add input to robot positionB = 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) - RC = 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 nodeRbig = repmat(R, [1 1 nlandmarks]);% initial conditionsinit_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 posnfor 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 psosnsend%%%%%%%%%%%%%%%%%%%%%% Inferenceif 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);endendif 0figure(1); hold onfor i=1:nlandmarks  h=plotgauss2d(est_landmark_pos(:,i), est_landmark_pos_cov(:,:,i));  set(h, 'color', 'r')endhold offhold onfor 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')endhold offendif 0figure(3)if 0  for t=1:T    imagesc(inv(Vsmooth(:,:,t)))    colorbar    fprintf('t=%d; press key to continue\n', t);    pause  endelse  for t=1:T    subplot(5,4,t)    imagesc(inv(Vsmooth(:,:,t)))  endendend%%%%%%%%%%%%%%%%%% DBN inferenceif 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);endnnodes = 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);niterloopy_est_robot_pos = zeros(2, T);for t=1:T  m = marginal_nodes(engine, Rnode, t);  loopy_est_robot_pos(:,t) = m.mu;endfor 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