Mercurial > hg > camir-aes2014

annotate toolboxes/FullBNT-1.0.7/bnt/examples/dynamic/SLAM/Old/paskin1.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 = 1;
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 = 60;
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 if 0
wolffd@0 27 figure(1); clf
wolffd@0 28 hold on
wolffd@0 29 for i=1:nlandmarks
wolffd@0 30 %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
wolffd@0 31 plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
wolffd@0 32 end
wolffd@0 33 hold off
wolffd@0 34 end
wolffd@0 35
wolffd@0 36 init_robot_pos = [0 0]';
wolffd@0 37
wolffd@0 38 true_robot_pos = zeros(2, T);
wolffd@0 39 true_data_assoc = zeros(1, T);
wolffd@0 40 true_rel_dist = zeros(2, T);
wolffd@0 41 for t=1:T
wolffd@0 42 if t>1
wolffd@0 43 true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
wolffd@0 44 else
wolffd@0 45 true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
wolffd@0 46 end
wolffd@0 47 nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
wolffd@0 48 %true_data_assoc(t) = nn;
wolffd@0 49 %true_data_assoc = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ...
wolffd@0 50 true_data_assoc = sample_discrete(normalise(ones(1,nlandmarks)),1,T);
wolffd@0 51 true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
wolffd@0 52 end
wolffd@0 53
wolffd@0 54 R = 1e-3*eye(2); % noise added to observation
wolffd@0 55 Q = 1e-3*eye(2); % noise added to robot motion
wolffd@0 56
wolffd@0 57 % Create data set
wolffd@0 58 obs_noise_seq = sample_gaussian([0 0]', R, T)';
wolffd@0 59 obs_rel_pos = true_rel_dist + obs_noise_seq;
wolffd@0 60 %obs_rel_pos = true_rel_dist;
wolffd@0 61
wolffd@0 62
wolffd@0 63 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wolffd@0 64 % Create params for inference
wolffd@0 65
wolffd@0 66 % X(t) = A X(t-1) + B U(t) + noise(Q)
wolffd@0 67
wolffd@0 68 % [L1] = [1 ] * [L1] + [0] * Ut + [0 ]
wolffd@0 69 % [L2] [ 1 ] [L2] [0] [ 0 ]
wolffd@0 70 % [R ]t [ 1] [R ]t-1 [1] [ Q]
wolffd@0 71
wolffd@0 72 % Y(t)|S(t)=s = C(s) X(t) + noise(R)
wolffd@0 73 % Yt|St=1 = [1 0 -1] * [L1] + R
wolffd@0 74 % [L2]
wolffd@0 75 % [R ]
wolffd@0 76
wolffd@0 77 % Create indices into block structure
wolffd@0 78 bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
wolffd@0 79 robot_block = block(nlandmarks+1, bs);
wolffd@0 80 for i=1:nlandmarks
wolffd@0 81 landmark_block(:,i) = block(i, bs)';
wolffd@0 82 end
wolffd@0 83 Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
wolffd@0 84 Ysz = 2; % observe relative location
wolffd@0 85 Usz = 2; % input is (dx, dy)
wolffd@0 86
wolffd@0 87
wolffd@0 88 % create block-diagonal trans matrix for each switch
wolffd@0 89 A = zeros(Xsz, Xsz);
wolffd@0 90 for i=1:nlandmarks
wolffd@0 91 bi = landmark_block(:,i);
wolffd@0 92 A(bi, bi) = eye(2);
wolffd@0 93 end
wolffd@0 94 bi = robot_block;
wolffd@0 95 A(bi, bi) = eye(2);
wolffd@0 96 A = repmat(A, [1 1 nlandmarks]); % same for all switch values
wolffd@0 97
wolffd@0 98 % create block-diagonal system cov
wolffd@0 99
wolffd@0 100
wolffd@0 101 Qbig = zeros(Xsz, Xsz);
wolffd@0 102 bi = robot_block;
wolffd@0 103 Qbig(bi,bi) = Q; % only add noise to robot motion
wolffd@0 104 Qbig = repmat(Qbig, [1 1 nlandmarks]);
wolffd@0 105
wolffd@0 106 % create input matrix
wolffd@0 107 B = zeros(Xsz, Usz);
wolffd@0 108 B(robot_block,:) = eye(2); % only add input to robot position
wolffd@0 109 B = repmat(B, [1 1 nlandmarks]);
wolffd@0 110
wolffd@0 111 % create observation matrix for each value of the switch node
wolffd@0 112 % C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
wolffd@0 113 % This computes L(i) - R
wolffd@0 114 C = zeros(Ysz, Xsz, nlandmarks);
wolffd@0 115 for i=1:nlandmarks
wolffd@0 116 C(:, landmark_block(:,i), i) = eye(2);
wolffd@0 117 C(:, robot_block, i) = -eye(2);
wolffd@0 118 end
wolffd@0 119
wolffd@0 120 % create observation cov for each value of the switch node
wolffd@0 121 Rbig = repmat(R, [1 1 nlandmarks]);
wolffd@0 122
wolffd@0 123 % initial conditions
wolffd@0 124 init_x = zeros(Xsz, 1);
wolffd@0 125 init_v = zeros(Xsz, Xsz);
wolffd@0 126 bi = robot_block;
wolffd@0 127 init_x(bi) = init_robot_pos;
wolffd@0 128 %init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
wolffd@0 129 init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step
wolffd@0 130 for i=1:nlandmarks
wolffd@0 131 bi = landmark_block(:,i);
wolffd@0 132 init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
wolffd@0 133 %init_x(bi) = true_landmark_pos(:,i);
wolffd@0 134 %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
wolffd@0 135 end
wolffd@0 136
wolffd@0 137 %k = nlandmarks-1; % exact
wolffd@0 138 k = 3;
wolffd@0 139 ndx = {};
wolffd@0 140 for t=1:T
wolffd@0 141 landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
wolffd@0 142 tmp = [landmark_block(:, landmarks) robot_block'];
wolffd@0 143 ndx{t} = tmp(:);
wolffd@0 144 end
wolffd@0 145
wolffd@0 146 [xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
wolffd@0 147 'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
wolffd@0 148 'ndx', ndx);
wolffd@0 149
wolffd@0 150 [xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
wolffd@0 151 'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
wolffd@0 152
wolffd@0 153
wolffd@0 154 if 0
wolffd@0 155 est_robot_pos = x(robot_block, :);
wolffd@0 156 est_robot_pos_cov = V(robot_block, robot_block, :);
wolffd@0 157
wolffd@0 158 for i=1:nlandmarks
wolffd@0 159 bi = landmark_block(:,i);
wolffd@0 160 est_landmark_pos(:,i) = x(bi, T);
wolffd@0 161 est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
wolffd@0 162 end
wolffd@0 163 end
wolffd@0 164
wolffd@0 165
wolffd@0 166
wolffd@0 167 nrows = 10;
wolffd@0 168 stepsize = T/(2*nrows);
wolffd@0 169 ts = 1:stepsize:T;
wolffd@0 170
wolffd@0 171 if 1 % plot
wolffd@0 172
wolffd@0 173 clim = [0 max(max(Va(:,:,end)))];
wolffd@0 174
wolffd@0 175 figure(2)
wolffd@0 176 if 0
wolffd@0 177 imagesc(Ve(1:2:end,1:2:end, T))
wolffd@0 178 clim = get(gca,'clim');
wolffd@0 179 else
wolffd@0 180 i = 1;
wolffd@0 181 for t=ts(:)'
wolffd@0 182 subplot(nrows,2,i)
wolffd@0 183 i = i + 1;
wolffd@0 184 imagesc(Ve(1:2:end,1:2:end, t))
wolffd@0 185 set(gca, 'clim', clim)
wolffd@0 186 colorbar
wolffd@0 187 end
wolffd@0 188 end
wolffd@0 189 suptitle('exact')
wolffd@0 190
wolffd@0 191
wolffd@0 192 figure(3)
wolffd@0 193 if 0
wolffd@0 194 imagesc(Va(1:2:end,1:2:end, T))
wolffd@0 195 set(gca,'clim', clim)
wolffd@0 196 else
wolffd@0 197 i = 1;
wolffd@0 198 for t=ts(:)'
wolffd@0 199 subplot(nrows,2,i)
wolffd@0 200 i = i+1;
wolffd@0 201 imagesc(Va(1:2:end,1:2:end, t))
wolffd@0 202 set(gca, 'clim', clim)
wolffd@0 203 colorbar
wolffd@0 204 end
wolffd@0 205 end
wolffd@0 206 suptitle('approx')
wolffd@0 207
wolffd@0 208
wolffd@0 209 figure(4)
wolffd@0 210 i = 1;
wolffd@0 211 for t=ts(:)'
wolffd@0 212 subplot(nrows,2,i)
wolffd@0 213 i = i+1;
wolffd@0 214 Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
wolffd@0 215 imagesc(Vd)
wolffd@0 216 set(gca, 'clim', clim)
wolffd@0 217 colorbar
wolffd@0 218 end
wolffd@0 219 suptitle('diff')
wolffd@0 220
wolffd@0 221 end % all plot
wolffd@0 222
wolffd@0 223
wolffd@0 224 for t=1:T
wolffd@0 225 i = 1:2*nlandmarks;
wolffd@0 226 denom = Ve(i,i,t) + (Ve(i,i,t)==0);
wolffd@0 227 Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
wolffd@0 228 Verr(t) = max(Vd(:));
wolffd@0 229 end
wolffd@0 230 figure(6); plot(Verr)
wolffd@0 231 title('max relative Verr')
wolffd@0 232
wolffd@0 233 for t=1:T
wolffd@0 234 %err(t)=rms(xa(:,t), xe(:,t));
wolffd@0 235 err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
wolffd@0 236 end
wolffd@0 237 figure(5);plot(err)
wolffd@0 238 title('rms mean pos')