|
wolffd@0
|
1 function [A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
|
|
wolffd@0
|
2 true_landmark_pos, true_robot_pos, true_data_assoc, ...
|
|
wolffd@0
|
3 obs_rel_pos, ctrl_signal] = mk_linear_slam(varargin)
|
|
wolffd@0
|
4
|
|
wolffd@0
|
5 % We create data from a linear system for testing SLAM algorithms.
|
|
wolffd@0
|
6 % i.e. , new robot pos = old robot pos + ctrl_signal, which is just a displacement vector.
|
|
wolffd@0
|
7 % and observation = landmark_pos - robot_pos, which is just a displacement vector.
|
|
wolffd@0
|
8 %
|
|
wolffd@0
|
9 % The behavior is determined by the following optional arguments:
|
|
wolffd@0
|
10 %
|
|
wolffd@0
|
11 % 'nlandmarks' - num. landmarks
|
|
wolffd@0
|
12 % 'landmarks' - 'rnd' means random locations in the unit sqyare
|
|
wolffd@0
|
13 % 'square' means at [1 1], [4 1], [4 4] and [1 4]
|
|
wolffd@0
|
14 % 'T' - num steps to run
|
|
wolffd@0
|
15 % 'ctrl' - 'stationary' means the robot remains at [0 0],
|
|
wolffd@0
|
16 % 'leftright' means the robot receives a constant contol of [1 0],
|
|
wolffd@0
|
17 % 'square' means we navigate the robot around the square
|
|
wolffd@0
|
18 % 'data-assoc' - 'rnd' means we observe landmarks at random
|
|
wolffd@0
|
19 % 'nn' means we observe the nearest neighbor landmark
|
|
wolffd@0
|
20 % 'cycle' means we observe landmarks in order 1,2,.., 1, 2, ...
|
|
wolffd@0
|
21
|
|
wolffd@0
|
22 args = varargin;
|
|
wolffd@0
|
23 % get mandatory params
|
|
wolffd@0
|
24 for i=1:2:length(args)
|
|
wolffd@0
|
25 switch args{i},
|
|
wolffd@0
|
26 case 'nlandmarks', nlandmarks = args{i+1};
|
|
wolffd@0
|
27 case 'T', T = args{i+1};
|
|
wolffd@0
|
28 end
|
|
wolffd@0
|
29 end
|
|
wolffd@0
|
30
|
|
wolffd@0
|
31 % set defaults
|
|
wolffd@0
|
32 true_landmark_pos = rand(2,nlandmarks);
|
|
wolffd@0
|
33 true_data_assoc = [];
|
|
wolffd@0
|
34
|
|
wolffd@0
|
35 % get args
|
|
wolffd@0
|
36 for i=1:2:length(args)
|
|
wolffd@0
|
37 switch args{i},
|
|
wolffd@0
|
38 case 'landmarks',
|
|
wolffd@0
|
39 switch args{i+1},
|
|
wolffd@0
|
40 case 'rnd', true_landmark_pos = rand(2,nlandmarks);
|
|
wolffd@0
|
41 case 'square', true_landmark_pos = [1 1; 4 1; 4 4; 1 4]';
|
|
wolffd@0
|
42 end
|
|
wolffd@0
|
43 case 'ctrl',
|
|
wolffd@0
|
44 switch args{i+1},
|
|
wolffd@0
|
45 case 'stationary', ctrl_signal = repmat([0 0]', 1, T);
|
|
wolffd@0
|
46 case 'leftright', ctrl_signal = repmat([1 0]', 1, T);
|
|
wolffd@0
|
47 case 'square', ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
|
|
wolffd@0
|
48 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
|
|
wolffd@0
|
49 end
|
|
wolffd@0
|
50 case 'data-assoc',
|
|
wolffd@0
|
51 switch args{i+1},
|
|
wolffd@0
|
52 case 'rnd', true_data_assoc = sample_discrete(normalise(ones(1,nlandmarks)),1,T);
|
|
wolffd@0
|
53 case 'cycle', true_data_assoc = wrap(1:T, nlandmarks);
|
|
wolffd@0
|
54 end
|
|
wolffd@0
|
55 end
|
|
wolffd@0
|
56 end
|
|
wolffd@0
|
57 if isempty(true_data_assoc)
|
|
wolffd@0
|
58 use_nn = 1;
|
|
wolffd@0
|
59 else
|
|
wolffd@0
|
60 use_nn = 0;
|
|
wolffd@0
|
61 end
|
|
wolffd@0
|
62
|
|
wolffd@0
|
63 %%%%%%%%%%%%%%%%%%%%%%%%
|
|
wolffd@0
|
64 % generate data
|
|
wolffd@0
|
65
|
|
wolffd@0
|
66 init_robot_pos = [0 0]';
|
|
wolffd@0
|
67 true_robot_pos = zeros(2, T);
|
|
wolffd@0
|
68 true_rel_dist = zeros(2, T);
|
|
wolffd@0
|
69 for t=1:T
|
|
wolffd@0
|
70 if t>1
|
|
wolffd@0
|
71 true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
|
|
wolffd@0
|
72 else
|
|
wolffd@0
|
73 true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
|
|
wolffd@0
|
74 end
|
|
wolffd@0
|
75 nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
|
|
wolffd@0
|
76 if use_nn
|
|
wolffd@0
|
77 true_data_assoc(t) = nn;
|
|
wolffd@0
|
78 end
|
|
wolffd@0
|
79 true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
|
|
wolffd@0
|
80 end
|
|
wolffd@0
|
81
|
|
wolffd@0
|
82
|
|
wolffd@0
|
83 R = 1e-3*eye(2); % noise added to observation
|
|
wolffd@0
|
84 Q = 1e-3*eye(2); % noise added to robot motion
|
|
wolffd@0
|
85
|
|
wolffd@0
|
86 % Create data set
|
|
wolffd@0
|
87 obs_noise_seq = sample_gaussian([0 0]', R, T)';
|
|
wolffd@0
|
88 obs_rel_pos = true_rel_dist + obs_noise_seq;
|
|
wolffd@0
|
89 %obs_rel_pos = true_rel_dist;
|
|
wolffd@0
|
90
|
|
wolffd@0
|
91 %%%%%%%%%%%%%%%%%%
|
|
wolffd@0
|
92 % Create params
|
|
wolffd@0
|
93
|
|
wolffd@0
|
94
|
|
wolffd@0
|
95 % X(t) = A X(t-1) + B U(t) + noise(Q)
|
|
wolffd@0
|
96
|
|
wolffd@0
|
97 % [L1] = [1 ] * [L1] + [0] * Ut + [0 ]
|
|
wolffd@0
|
98 % [L2] [ 1 ] [L2] [0] [ 0 ]
|
|
wolffd@0
|
99 % [R ]t [ 1] [R ]t-1 [1] [ Q]
|
|
wolffd@0
|
100
|
|
wolffd@0
|
101 % Y(t)|S(t)=s = C(s) X(t) + noise(R)
|
|
wolffd@0
|
102 % Yt|St=1 = [1 0 -1] * [L1] + R
|
|
wolffd@0
|
103 % [L2]
|
|
wolffd@0
|
104 % [R ]
|
|
wolffd@0
|
105
|
|
wolffd@0
|
106 % Create indices into block structure
|
|
wolffd@0
|
107 bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
|
|
wolffd@0
|
108 robot_block = block(nlandmarks+1, bs);
|
|
wolffd@0
|
109 for i=1:nlandmarks
|
|
wolffd@0
|
110 landmark_block(:,i) = block(i, bs)';
|
|
wolffd@0
|
111 end
|
|
wolffd@0
|
112 Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
|
|
wolffd@0
|
113 Ysz = 2; % observe relative location
|
|
wolffd@0
|
114 Usz = 2; % input is (dx, dy)
|
|
wolffd@0
|
115
|
|
wolffd@0
|
116
|
|
wolffd@0
|
117 % create block-diagonal trans matrix for each switch
|
|
wolffd@0
|
118 A = zeros(Xsz, Xsz);
|
|
wolffd@0
|
119 for i=1:nlandmarks
|
|
wolffd@0
|
120 bi = landmark_block(:,i);
|
|
wolffd@0
|
121 A(bi, bi) = eye(2);
|
|
wolffd@0
|
122 end
|
|
wolffd@0
|
123 bi = robot_block;
|
|
wolffd@0
|
124 A(bi, bi) = eye(2);
|
|
wolffd@0
|
125 A = repmat(A, [1 1 nlandmarks]); % same for all switch values
|
|
wolffd@0
|
126
|
|
wolffd@0
|
127 % create block-diagonal system cov
|
|
wolffd@0
|
128
|
|
wolffd@0
|
129
|
|
wolffd@0
|
130 Qbig = zeros(Xsz, Xsz);
|
|
wolffd@0
|
131 bi = robot_block;
|
|
wolffd@0
|
132 Qbig(bi,bi) = Q; % only add noise to robot motion
|
|
wolffd@0
|
133 Qbig = repmat(Qbig, [1 1 nlandmarks]);
|
|
wolffd@0
|
134
|
|
wolffd@0
|
135 % create input matrix
|
|
wolffd@0
|
136 B = zeros(Xsz, Usz);
|
|
wolffd@0
|
137 B(robot_block,:) = eye(2); % only add input to robot position
|
|
wolffd@0
|
138 B = repmat(B, [1 1 nlandmarks]);
|
|
wolffd@0
|
139
|
|
wolffd@0
|
140 % create observation matrix for each value of the switch node
|
|
wolffd@0
|
141 % C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
|
|
wolffd@0
|
142 % This computes L(i) - R
|
|
wolffd@0
|
143 C = zeros(Ysz, Xsz, nlandmarks);
|
|
wolffd@0
|
144 for i=1:nlandmarks
|
|
wolffd@0
|
145 C(:, landmark_block(:,i), i) = eye(2);
|
|
wolffd@0
|
146 C(:, robot_block, i) = -eye(2);
|
|
wolffd@0
|
147 end
|
|
wolffd@0
|
148
|
|
wolffd@0
|
149 % create observation cov for each value of the switch node
|
|
wolffd@0
|
150 Rbig = repmat(R, [1 1 nlandmarks]);
|
|
wolffd@0
|
151
|
|
wolffd@0
|
152 % initial conditions
|
|
wolffd@0
|
153 init_x = zeros(Xsz, 1);
|
|
wolffd@0
|
154 init_v = zeros(Xsz, Xsz);
|
|
wolffd@0
|
155 bi = robot_block;
|
|
wolffd@0
|
156 init_x(bi) = init_robot_pos;
|
|
wolffd@0
|
157 %init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
|
|
wolffd@0
|
158 init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step
|
|
wolffd@0
|
159 for i=1:nlandmarks
|
|
wolffd@0
|
160 bi = landmark_block(:,i);
|
|
wolffd@0
|
161 init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
|
|
wolffd@0
|
162 %init_x(bi) = true_landmark_pos(:,i);
|
|
wolffd@0
|
163 %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
|
|
wolffd@0
|
164 end
|
