diff options
Diffstat (limited to 'sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old')
7 files changed, 803 insertions, 0 deletions
diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Entries b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Entries new file mode 100644 index 00000000..37fe6bb1 --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Entries @@ -0,0 +1,5 @@ +/offline_loopy_slam.m/1.1.1.1/Wed May 29 15:59:54 2002// +/paskin1.m/1.1.1.1/Wed May 29 15:59:54 2002// +/skf_data_assoc_gmux2.m/1.1.1.1/Wed May 29 15:59:54 2002// +/slam_kf.m/1.1.1.1/Wed May 29 15:59:54 2002// +D diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Repository b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Repository new file mode 100644 index 00000000..1bae1a70 --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Repository @@ -0,0 +1 @@ +FullBNT/BNT/examples/dynamic/SLAM/Old diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Root b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Root new file mode 100644 index 00000000..f3bd14a6 --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/CVS/Root @@ -0,0 +1 @@ +:ext:nsaunier@bnt.cvs.sourceforge.net:/cvsroot/bnt diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/offline_loopy_slam.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/offline_loopy_slam.m new file mode 100644 index 00000000..377a4659 --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/offline_loopy_slam.m @@ -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 + + diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/paskin1.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/paskin1.m new file mode 100644 index 00000000..286793d3 --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/paskin1.m @@ -0,0 +1,238 @@ +% This is like robot1, except we only use a Kalman filter. +% The goal is to study how the precision matrix changes. + +seed = 1; +rand('state', seed); +randn('state', seed); + +if 0 + 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 = 60; + ctrl_signal = repmat([1 0]', 1, T); +end + +nlandmarks = 6; +if 0 + true_landmark_pos = [1 1; + 4 1; + 4 4; + 1 4]'; +else + true_landmark_pos = 10*rand(2,nlandmarks); +end +if 0 +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 +end + +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')); + %true_data_assoc(t) = nn; + %true_data_assoc = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ... + true_data_assoc = sample_discrete(normalise(ones(1,nlandmarks)),1,T); + true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t); +end + +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 +init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step +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 + +%k = nlandmarks-1; % exact +k = 3; +ndx = {}; +for t=1:T + landmarks = unique(true_data_assoc(t:-1:max(t-k,1))); + tmp = [landmark_block(:, landmarks) robot_block']; + ndx{t} = tmp(:); +end + +[xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ... + 'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ... + 'ndx', ndx); + +[xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ... + 'model', true_data_assoc, 'u', ctrl_signal, 'B', B); + + +if 0 +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 +end + + + +nrows = 10; +stepsize = T/(2*nrows); +ts = 1:stepsize:T; + +if 1 % plot + +clim = [0 max(max(Va(:,:,end)))]; + +figure(2) +if 0 + imagesc(Ve(1:2:end,1:2:end, T)) + clim = get(gca,'clim'); +else + i = 1; + for t=ts(:)' + subplot(nrows,2,i) + i = i + 1; + imagesc(Ve(1:2:end,1:2:end, t)) + set(gca, 'clim', clim) + colorbar + end +end +suptitle('exact') + + +figure(3) +if 0 + imagesc(Va(1:2:end,1:2:end, T)) + set(gca,'clim', clim) +else + i = 1; + for t=ts(:)' + subplot(nrows,2,i) + i = i+1; + imagesc(Va(1:2:end,1:2:end, t)) + set(gca, 'clim', clim) + colorbar + end +end +suptitle('approx') + + +figure(4) +i = 1; +for t=ts(:)' + subplot(nrows,2,i) + i = i+1; + Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t); + imagesc(Vd) + set(gca, 'clim', clim) + colorbar +end +suptitle('diff') + +end % all plot + + +for t=1:T + i = 1:2*nlandmarks; + denom = Ve(i,i,t) + (Ve(i,i,t)==0); + Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom; + Verr(t) = max(Vd(:)); +end +figure(6); plot(Verr) +title('max relative Verr') + +for t=1:T + %err(t)=rms(xa(:,t), xe(:,t)); + err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot +end +figure(5);plot(err) +title('rms mean pos') diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.m new file mode 100644 index 00000000..0272d3f6 --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/skf_data_assoc_gmux2.m @@ -0,0 +1,155 @@ +% This is like skf_data_assoc_gmux, except the objects don't move. +% We are uncertain of their initial positions, and get more and more observations +% over time. The goal is to test deterministic links (0 covariance). +% This is like robot1, except the robot doesn't move and is always at [0 0], +% so the relative location is simply L(s). + +nobj = 2; +N = nobj+2; +Xs = 1:nobj; +S = nobj+1; +Y = nobj+2; + +intra = zeros(N,N); +inter = zeros(N,N); +intra([Xs S], Y) =1; +for i=1:nobj + inter(Xs(i), Xs(i))=1; +end + +Xsz = 2; % state space = (x y) +Ysz = 2; +ns = zeros(1,N); +ns(Xs) = Xsz; +ns(Y) = Ysz; +ns(S) = nobj; + +bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]); + +% For each object, we have +% X(t+1) = F X(t) + noise(Q) +% Y(t) = H X(t) + noise(R) +F = eye(2); +H = eye(2); +Q = 0*eye(Xsz); % no noise in dynamics +R = eye(Ysz); + +init_state{1} = [10 10]'; +init_state{2} = [10 -10]'; +init_cov = eye(2); + +% Uncertain of initial state (position) +for i=1:nobj + bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', init_cov); +end +bnet.CPD{S} = root_CPD(bnet, S); % always observed +bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj])); +% slice 2 +eclass = bnet.equiv_class; +for i=1:nobj + bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F); +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Create LDS params + +% X(t) = A X(t-1) + B U(t) + noise(Q) + +% [L11] = [1 ] * [L1] + [Q ] +% [L2] [ 1] [L2] [ Q] + +% Y(t)|S(t)=s = C(s) X(t) + noise(R) +% Yt|St=1 = [1 0] * [L1] + R +% [L2] + +nlandmarks = nobj; + +% Create indices into block structure +bs = 2*ones(1, nobj); % sizes of blocks in state space +for i=1:nlandmarks + landmark_block(:,i) = block(i, bs)'; +end +Xsz = 2*(nlandmarks); % 2 values for each landmark plus robot +Ysz = 2; % observe relative location + +% 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 +A = repmat(A, [1 1 nlandmarks]); % same for all switch values + +% create block-diagonal system cov +Qbig = zeros(Xsz, Xsz); +Qbig = repmat(Qbig, [1 1 nlandmarks]); + + +% create observation matrix for each value of the switch node +% C(:,:,i) = (0 ... I ...) where the I is in the i'th posn. +C = zeros(Ysz, Xsz, nlandmarks); +for i=1:nlandmarks + C(:, landmark_block(:,i), i) = eye(2); +end + +% create observation cov for each value of the switch node +Rbig = repmat(R, [1 1 nlandmarks]); + +% initial conditions +init_x = [init_state{1}; init_state{2}]; +init_V = zeros(Xsz, Xsz); +for i=1:nlandmarks + bi = landmark_block(:,i); + init_V(bi,bi) = init_cov; +end + + + +%%%%%%%%%%%%%%%% +% Observe objects at random +T = 10; +evidence = cell(N, T); +data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T); +evidence(S,:) = num2cell(data_assoc); +evidence = sample_dbn(bnet, 'evidence', evidence); + + +% Inference +ev = cell(N,T); +ev(bnet.observed,:) = evidence(bnet.observed, :); +y = cell2num(evidence(Y,:)); + +engine = pearl_unrolled_dbn_inf_engine(bnet); +engine = enter_evidence(engine, ev); + +loopy_est_pos = zeros(2, nlandmarks); +loopy_est_pos_cov = zeros(2, 2, nlandmarks); +for i=1:nobj + m = marginal_nodes(engine, Xs(i), T); + loopy_est_pos(:,i) = m.mu; + loopy_est_pos_cov(:,:,i) = m.Sigma; +end + + +[xsmooth, Vsmooth] = kalman_smoother(y, A, C, Qbig, Rbig, init_x, init_V, 'model', data_assoc); + +kf_est_pos = zeros(2, nlandmarks); +kf_est_pos_cov = zeros(2, 2, nlandmarks); +for i=1:nlandmarks + bi = landmark_block(:,i); + kf_est_pos(:,i) = xsmooth(bi, T); + kf_est_pos_cov(:,:,i) = Vsmooth(bi, bi, T); +end + + +kf_est_pos +loopy_est_pos + +kf_est_pos_time = zeros(2, nlandmarks, T); +for t=1:T + for i=1:nlandmarks + bi = landmark_block(:,i); + kf_est_pos_time(:,i,t) = xsmooth(bi, t); + end +end +kf_est_pos_time % same for all t since smoothed diff --git a/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/slam_kf.m b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/slam_kf.m new file mode 100644 index 00000000..ba98140f --- /dev/null +++ b/sourcecodes/bnt-master/BNT/examples/dynamic/SLAM/Old/slam_kf.m @@ -0,0 +1,172 @@ +% This is like robot1, except we only use a Kalman filter. +% The goal is to study how the precision matrix changes. + +seed = 0; +rand('state', seed); +randn('state', seed); + +if 0 + 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 = 12; + ctrl_signal = repmat([1 0]', 1, T); +end + +nlandmarks = 6; +if 0 + true_landmark_pos = [1 1; + 4 1; + 4 4; + 1 4]'; +else + true_landmark_pos = 10*rand(2,nlandmarks); +end +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 + +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 = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ... + true_data_assoc(t) = nn; + true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t); +end + +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 + +[xsmooth, Vsmooth] = 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 = 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 + + + +P = zeros(size(Vsmooth)); +for t=1:T + P(:,:,t) = inv(Vsmooth(:,:,t)); +end + +figure(1) +for t=1:T + subplot(T/2,2,t) + imagesc(P(1:2:end,1:2:end, t)) + colorbar +end + +figure(2) +for t=1:T + subplot(T/2,2,t) + imagesc(Vsmooth(1:2:end,1:2:end, t)) + colorbar +end + + + +% marginalize out robot position and then check structure +bi = landmark_block(:); +V = Vsmooth(bi,bi,T); +P = inv(V); +P(1:2:end,1:2:end) |