Mercurial > hg > camir-aes2014
diff toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/Old/offline_loopy_slam.m @ 0:e9a9cd732c1e tip
first hg version after svn
author | wolffd |
---|---|
date | Tue, 10 Feb 2015 15:05:51 +0000 |
parents | |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/Old/offline_loopy_slam.m Tue Feb 10 15:05:51 2015 +0000 @@ -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 + +