Page 174 - Autonomous Mobile Robots

158 Autonomous Mobile Robots

For the laser scanner, the observation model is

b yi − y(k)
h(B i , u(k)) = arctan − θ(k) (4.7)
b xi − x(k)

Since the models (4.3) and (4.4) are nonlinear, the EKF [17] must be used here
to integrate the laser measurements and encoder readings. Note that the EKF is
recursively implemented as follows:

Step 1: Prediction — It predicts the next position of the robot using
odometry.

x(k + 1/k) = f(x(k), u(k)) (4.8)
T
p(k + 1/k) =∇fP(k/k)∇f + Q(k) (4.9)

where ∇f is the Jacobean matrix of the transition function, and is
obtained by linearization

 
1 0 − d(k) sin θ(k)
∇f =  0 1 d(k) cos θ(k)  (4.10)
0 0 1

Step 2: Observation — It makes actual measurements.
The measurement of the laser scanner is

z(k + 1) = h(B i , x(k)) (4.11)

The predicted angular measurement is

ˆ z(k + 1) = h(B i , ˆ x(k + 1/k)) (4.12)

Step 3: Matching— It compares the real measurement with the predicted
measurement.
To calculate the innovation, use

v(k + 1) = z(k + 1) − ˆ z(k + 1) (4.13)

The innovation covariance is:

T
S(k + 1) =∇hP(k + 1)∇h + R(k + 1) (4.14)

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c004” — 2006/3/31 — 16:42 — page 158 — #10