Mercurial > hg > camir-aes2014

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

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
first hg version after svn
author wolffd
date Tue, 10 Feb 2015 15:05:51 +0000
parents
children
rev   line source
wolffd@0 1 % This is like robot1, except we only use a Kalman filter.
wolffd@0 2 % The goal is to study how the precision matrix changes.
wolffd@0 3
wolffd@0 4 seed = 0;
wolffd@0 5 rand('state', seed);
wolffd@0 6 randn('state', seed);
wolffd@0 7
wolffd@0 8 if 0
wolffd@0 9 T = 20;
wolffd@0 10 ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
wolffd@0 11 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
wolffd@0 12 else
wolffd@0 13 T = 12;
wolffd@0 14 ctrl_signal = repmat([1 0]', 1, T);
wolffd@0 15 end
wolffd@0 16
wolffd@0 17 nlandmarks = 6;
wolffd@0 18 if 0
wolffd@0 19 true_landmark_pos = [1 1;
wolffd@0 20 4 1;
wolffd@0 21 4 4;
wolffd@0 22 1 4]';
wolffd@0 23 else
wolffd@0 24 true_landmark_pos = 10*rand(2,nlandmarks);
wolffd@0 25 end
wolffd@0 26 figure(1); clf
wolffd@0 27 hold on
wolffd@0 28 for i=1:nlandmarks
wolffd@0 29 %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
wolffd@0 30 plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
wolffd@0 31 end
wolffd@0 32 hold off
wolffd@0 33
wolffd@0 34 init_robot_pos = [0 0]';
wolffd@0 35
wolffd@0 36 true_robot_pos = zeros(2, T);
wolffd@0 37 true_data_assoc = zeros(1, T);
wolffd@0 38 true_rel_dist = zeros(2, T);
wolffd@0 39 for t=1:T
wolffd@0 40 if t>1
wolffd@0 41 true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
wolffd@0 42 else
wolffd@0 43 true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
wolffd@0 44 end
wolffd@0 45 %nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
wolffd@0 46 nn = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ...
wolffd@0 47 true_data_assoc(t) = nn;
wolffd@0 48 true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
wolffd@0 49 end
wolffd@0 50
wolffd@0 51 R = 1e-3*eye(2); % noise added to observation
wolffd@0 52 Q = 1e-3*eye(2); % noise added to robot motion
wolffd@0 53
wolffd@0 54 % Create data set
wolffd@0 55 obs_noise_seq = sample_gaussian([0 0]', R, T)';
wolffd@0 56 obs_rel_pos = true_rel_dist + obs_noise_seq;
wolffd@0 57 %obs_rel_pos = true_rel_dist;
wolffd@0 58
wolffd@0 59
wolffd@0 60 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wolffd@0 61 % Create params for inference
wolffd@0 62
wolffd@0 63 % X(t) = A X(t-1) + B U(t) + noise(Q)
wolffd@0 64
wolffd@0 65 % [L1] = [1 ] * [L1] + [0] * Ut + [0 ]
wolffd@0 66 % [L2] [ 1 ] [L2] [0] [ 0 ]
wolffd@0 67 % [R ]t [ 1] [R ]t-1 [1] [ Q]
wolffd@0 68
wolffd@0 69 % Y(t)|S(t)=s = C(s) X(t) + noise(R)
wolffd@0 70 % Yt|St=1 = [1 0 -1] * [L1] + R
wolffd@0 71 % [L2]
wolffd@0 72 % [R ]
wolffd@0 73
wolffd@0 74 % Create indices into block structure
wolffd@0 75 bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
wolffd@0 76 robot_block = block(nlandmarks+1, bs);
wolffd@0 77 for i=1:nlandmarks
wolffd@0 78 landmark_block(:,i) = block(i, bs)';
wolffd@0 79 end
wolffd@0 80 Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
wolffd@0 81 Ysz = 2; % observe relative location
wolffd@0 82 Usz = 2; % input is (dx, dy)
wolffd@0 83
wolffd@0 84
wolffd@0 85 % create block-diagonal trans matrix for each switch
wolffd@0 86 A = zeros(Xsz, Xsz);
wolffd@0 87 for i=1:nlandmarks
wolffd@0 88 bi = landmark_block(:,i);
wolffd@0 89 A(bi, bi) = eye(2);
wolffd@0 90 end
wolffd@0 91 bi = robot_block;
wolffd@0 92 A(bi, bi) = eye(2);
wolffd@0 93 A = repmat(A, [1 1 nlandmarks]); % same for all switch values
wolffd@0 94
wolffd@0 95 % create block-diagonal system cov
wolffd@0 96
wolffd@0 97
wolffd@0 98 Qbig = zeros(Xsz, Xsz);
wolffd@0 99 bi = robot_block;
wolffd@0 100 Qbig(bi,bi) = Q; % only add noise to robot motion
wolffd@0 101 Qbig = repmat(Qbig, [1 1 nlandmarks]);
wolffd@0 102
wolffd@0 103 % create input matrix
wolffd@0 104 B = zeros(Xsz, Usz);
wolffd@0 105 B(robot_block,:) = eye(2); % only add input to robot position
wolffd@0 106 B = repmat(B, [1 1 nlandmarks]);
wolffd@0 107
wolffd@0 108 % create observation matrix for each value of the switch node
wolffd@0 109 % C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
wolffd@0 110 % This computes L(i) - R
wolffd@0 111 C = zeros(Ysz, Xsz, nlandmarks);
wolffd@0 112 for i=1:nlandmarks
wolffd@0 113 C(:, landmark_block(:,i), i) = eye(2);
wolffd@0 114 C(:, robot_block, i) = -eye(2);
wolffd@0 115 end
wolffd@0 116
wolffd@0 117 % create observation cov for each value of the switch node
wolffd@0 118 Rbig = repmat(R, [1 1 nlandmarks]);
wolffd@0 119
wolffd@0 120 % initial conditions
wolffd@0 121 init_x = zeros(Xsz, 1);
wolffd@0 122 init_v = zeros(Xsz, Xsz);
wolffd@0 123 bi = robot_block;
wolffd@0 124 init_x(bi) = init_robot_pos;
wolffd@0 125 init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
wolffd@0 126 for i=1:nlandmarks
wolffd@0 127 bi = landmark_block(:,i);
wolffd@0 128 init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
wolffd@0 129 %init_x(bi) = true_landmark_pos(:,i);
wolffd@0 130 %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
wolffd@0 131 end
wolffd@0 132
wolffd@0 133 [xsmooth, Vsmooth] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
wolffd@0 134 'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
wolffd@0 135
wolffd@0 136 est_robot_pos = xsmooth(robot_block, :);
wolffd@0 137 est_robot_pos_cov = Vsmooth(robot_block, robot_block, :);
wolffd@0 138
wolffd@0 139 for i=1:nlandmarks
wolffd@0 140 bi = landmark_block(:,i);
wolffd@0 141 est_landmark_pos(:,i) = xsmooth(bi, T);
wolffd@0 142 est_landmark_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
wolffd@0 143 end
wolffd@0 144
wolffd@0 145
wolffd@0 146
wolffd@0 147 P = zeros(size(Vsmooth));
wolffd@0 148 for t=1:T
wolffd@0 149 P(:,:,t) = inv(Vsmooth(:,:,t));
wolffd@0 150 end
wolffd@0 151
wolffd@0 152 figure(1)
wolffd@0 153 for t=1:T
wolffd@0 154 subplot(T/2,2,t)
wolffd@0 155 imagesc(P(1:2:end,1:2:end, t))
wolffd@0 156 colorbar
wolffd@0 157 end
wolffd@0 158
wolffd@0 159 figure(2)
wolffd@0 160 for t=1:T
wolffd@0 161 subplot(T/2,2,t)
wolffd@0 162 imagesc(Vsmooth(1:2:end,1:2:end, t))
wolffd@0 163 colorbar
wolffd@0 164 end
wolffd@0 165
wolffd@0 166
wolffd@0 167
wolffd@0 168 % marginalize out robot position and then check structure
wolffd@0 169 bi = landmark_block(:);
wolffd@0 170 V = Vsmooth(bi,bi,T);
wolffd@0 171 P = inv(V);
wolffd@0 172 P(1:2:end,1:2:end)