view toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/slam_kf.m @ 0:e9a9cd732c1e tip

first hg version after svn

% 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)