Page 350 - Autonomous Mobile Robots

340 Autonomous Mobile Robots

B
Vector ˆ x contains the estimated location of the vehicle R and the environment
features F 1 ... F n , all with respect to a base reference B. In the case of
T
B
the vehicle, its location vector ˆ x = (x, y, φ) describes the transformation
R
from B to R. In the case of an environment feature j, the parameters that
compose its location vector ˆ x B depend on the feature type, for example,
F j B
B
T
ˆ x = (x j , y j ) for point features. The diagonal elements of the matrix P
F j
represent the estimated error covariance of the different features of the state
vector and that of the vehicle location; its off-diagonal elements represent the
cross-covariance matrices between the estimated locations of the corresponding
features.
Recursive estimation of the first two moments of the probability density
B
function of x is performed following Algorithm 9.1. There, the map is ini-
tialized using the current vehicle location as base reference, and thus with
perfect knowledge of the vehicle location. Sensing and feature initialization
is also performed before the first vehicle motion, to maximize the precision
of the resulting map. Prediction of the vehicle motion using odometry and
update of the map using onboard sensor measurements are then iteratively
carried out.

9.2.1 Initialization
In the creation of a new stochastic map at step 0, a base reference B must
be selected. It is common practice to build a map relative to a fixed base
reference different from the initial vehicle location. This normally requires the

ALGORITHM 9.1
EKF–SLAM
B
B
x = 0; P = 0 {Map initialization}
0 0
[z 0 , R 0 ] = get_measurements
B
B
B
B
[x , P ] = add_new_features(x , P , z 0 , R 0 )
0 0 0 0
for k = 1 to steps do
[x R k−1 , Q k ] = get_odometry
R k
[x B ,P B ]=compute_motion(x B , P B ,x R k−1 ,Q k ){EKFpredict.}
k|k−1 k|k−1 k−1 k−1 R k
[z k , R k ] = get_measurements
H k = data_association(x B , P B , z k , R k )
k|k−1 k|k−1
B
B
[x , P ] = update_map(x B , P B , z k , R k , H k ){EKF update}
k k k|k−1 k|k−1
B
B
B
B
[x , P ] = add_new_features(x , P , z k , R k , H k )
k k k k
end for
© 2006 by Taylor & Francis Group, LLC
FRANKL: “dk6033_c009” — 2006/3/31 — 16:43 — page 340 — #10