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