Professional Documents
Culture Documents
Neural Network-Aided Extended Kalman Filter For SLAM Problem
Neural Network-Aided Extended Kalman Filter For SLAM Problem
5
Robotics and Automation
Roma, Italy, 10-14 April 2007
Abstract This paper addresses the problem of Simultane- efficient than classical techniques because neural network is
ous Localization and Map Building (SLAM) using a Neural highly tolerant to noises [7]. As a relevant researches in [8],
Network aided Extended Kalman Filter (NNEKF) algorithm. [9], the extended Kalman filter has been used to train the
Since the EKF is based on the white noise assumption, if there
are colored noise or systematic bias error in the system, EKF multilayer perception network by treating the weights of a
inevitably diverges. The neural network in this algorithm is network as the state of the nonlinear dynamic system. In
used to approximate the uncertainty of the system model due to [13] dual extended Kalman filter is presented for removing
mismodeling and extreme nonlinearities. Simulation results are nonstationary and colored noise from speech. More recently,
presented to illustrate the proposed algorithm NNEKF is very Zhan and Wan [14] developed NN-aided adaptive unscented
effective compared with the standard EKF algorithm under the
practical condition where the mobile robot has bias error in Kalman filter for nonlinear state estimation.
its modeling and environment has strong uncertainties. In this Even though the above works studied for state estimation
paper, we propose an algorithm which enables a biased control problem, no work have been reported regarding the SLAM
input in vehicle model using neural network. problem with the aid of neural network. Motivated by the
above considerations, the goal of this paper is to introduce
I. INTRODUCTION
a neural network based extended Kalman filter to SLAM
In robotics, the problem of simultaneous localization and problem to deal with systematic error. To the best of authors
map building is that of estimating both a robots position Knowledge, this is possibly the first attempt to this problem.
and a map of its surrounding environment. In general, it is a The NNEKF is a combination of system identification and
complex problem because noise in the estimate of the robots standard EKF. In [9], two EKFs are coupled so that a single
pose leads to uncertainty in the estimate of the map and EKF will be used simultaneously to estimate the state and
vice versa. The Extended Kalman Filter (EKF) has become train the weights of the NN. For simplicity, in this paper we
a standard technique used in simultaneous localization and consider a two hidden layer neural network.
map building problem. During the past few years significant This paper is organized as follows. In section II, a neural
progress have been made towards the solution for the SLAM network aided extended Kalman filter is explained briefly.
problem [3], [4], [6] with the aid of EKF. Section III describes about SLAM problem using NN-aided
EKF assumes a white Gaussian noise in prediction and EKF. In Section IV, simulation results are provided to
measurement model. In real situation, however, there are demonstrate the effectiveness of the proposed algorithm.
biased systematic error in mobile robot even after appropriate Finally, Section V contains some concluding remarks.
compensation [5]. If there are also some colored noise
in control input or drifting errors in dead-reckoning, they
could affect the quality of SLAM directly. In addition to II. NN-AIDED EXTENDED KALMAN FILTER
these, a priori knowledge of the plant model is required to
compute both the prediction of the state estimate and its A. Neural Network
Jacobian. Often, the plant model is not completely known
due to mismodeling, extreme nonlinearities and changes Neural network is a network of single perceptron. The
in system parameters. To perform state estimation when single perceptron is represented as in Fig. 1. According to
such conditions occur, Martinelli et al [10] introduced an inputs xi and weights aij , output yj is changed. The function
augmented Kalman filter which simultaneously estimates the f () is referred to as the squashing function, one of which a
robot configuration and the parameters characterizing the sigmoid type function is widely used since its derivative is
systematic error. In this paper, we propose to implement easily obtained.
an EKF whose plant model is augmented by an Neural
Network(NN). The NN will capture the unmodeled dynamics
by learning on line. The function describes the difference
between the true plant and the best model. The processing
of imprecise or noisy data by the neural networks is more
Minyong Choi, R. Sakthivel, and Wan Kyun Chung are with Robotics
and Bio-mechatronics Lab., Department of Mechanical Engineering, Po-
hang University of Science and Technology (POSTECH), Pohang, Korea
{minyong,Sakthi,wkchung}@postech.ac.kr Fig. 1. Single Perceptron
1687
ThA1.5
where the diagonal sub-matrices are covariance of vehicle 5) New Feature Augmentation
and each feature, and off-diagonal sub-matrices are corre- IV. SIMULATION RESULT
lation of them. A detailed SLAM algorithm using standard
In this section, we apply the proposed NNEKF algorithm
EKF is presented in [6].
to SLAM problem and compare it with the standard EKF
B. SLAM using NNEKF algorithm. To test the performance of our algorithm, we use
the Ackerman vehicle model, in which control inputs are
In this section, the NNEKF is used to study the SLAM linear velocity and steering angle.
problem. A main advantage of the NNEKF is that it is used
simultaneously for state estimation (both vehicles position xk+1 xk + tVk+1 cos(k )
and feature position) and neural network training. If we yk+1 = yk + tVk+1 sin(k ) (11)
represent the augmented state vector as x = [xTv xTa xTf ]T k+1 k + (tVk+1 tan(k+1 ))/L
then (6) can be redefined as where V is linear velocity, is steering angle, L = 0.5 m,
and t = 0.025 s.
xv,k+1 fv (xv,k , uk+1 ) + g(xv,k , xa,k , uk+1 )
xa,k+1 = xa,k Matlab simulation code developed by Bailey et al. [2] for
xf ,k+1 xf ,k SLAM which is opened on the web-site [15]. We modified
their simulation code according to our work and used to
where xa is neural networks weight. verify the algorithm.
In general, features position in SLAM problem is assumed The vehicles unbiased control input has maximum linear
to be stationary. It is noted that in the prediction model, velocity as 2.0 m/s, and maximum steering angle as 30 .
neural network weights are not changed as like features Noise of control input is assumed zero mean Gaussian with
position. The plant Jacobian with respect to states is given V = 0.1 m/s, and = 1 . To simulate biased control
by input, we add linear velocity by 0.05 m/s, and steering
xv fv + xv g xa g 0 angle by 0.2 . This comes from the following reasoning.
x f = 0 If 0 (9) Suppose both wheel diameters are 20 cm, the change of 0.5
0 0 Ia cm of the wheel diameter can make the 0.05 m/s biases in
linear velocity. As the wheel tread is 40 cm and the difference
In the above Jacobian xv g, and xa g are depending on
between two wheel diameters is 0.2 cm, nearly 0.2 biases
neural network structure. The plant Jacobian with respect to
could be occurred in steering angle.
control inputs is given by
The environment is relatively large about 100 m 100
u f + u g m. Since the sufficient number of features are required to
u f = 0 (10) guarantee observability, we used about 100 features and there
0 is one feature in each 10 m 10 m area.
In the observation model, range-bearing sensor model are
The augmented state vector estimates the state estimate
used to measure the feature position and vehicle pose related
and weights of the neural networks simultaneously. The
to its map, whose noise level is 0.1 m in range, 1 in bearing.
NNEKF algorithm for SLAM problem is described.
The sensor range is restricted under 20 m, which can detect
1) Initialize all features in front of vehicle.
5-3-3-3 multilayered neural network having two hidden
xv = 0, Pvv = 0
layers with sigmoid function is used in the simulation. Since
xa = , Paa = I the input of the neural network is the augmented vector of
1688
ThA1.5
x error x error
1.5 1.5
40 1 1
2 bound 2 bound
0.5 0.5
30
meters
meters
0 0
20
0.5 0.5
10
1 1
m
0 1.5
0 500 1000 1500 2000 2500 3000 3500 4000 4500
1.5
0 500 1000 1500 2000 2500 3000 3500 4000 4500
time step time step
0.8 0.8
2 bound 2 bound
30 0.6 0.6
0.4 0.4
40 0.2 0.2
meters
meters
0 0
0.2 0.2
60 40 20 0 20 40 60
m 0.4 0.4
0.6 0.6
Fig. 4. Simulation Result in Unbiased Case; black dash-dot line: reference 0.8 0.8
trajectory, black (+): true features; blue solid line: estimated trajectory by 1
0 500 1000 1500 2000 2500
time step
3000 3500 4000
1
0 500 1000 1500 2000 2500
time step
3000 3500 4000
0.02 0.02
0.01 0.01
radians
radians
the vehicle pose and control inputs, the number of input 0 0
0.02 0.02
1689
ThA1.5
x error x error
7 7
6 6
40
5 5
4 4
30 2 bound 2 bound
3 3
meters
meters
2 2
20
1 1
0 0
10
1 1
2 2
m
0 3 3
0 500 1000 1500 2000 2500 3000 3500 4000 0 500 1000 1500 2000 2500 3000 3500 4000
time step time step
10
(a) x error (EKF) (b) x error (NNEKF)
20 y error y error
2 2
30 1 1
0 0
40
1 1
meters
meters
60 40 20 0 20 40 60 2 2
m
3 3
(a) NNEKF
4 4
2 bound 2 bound
5 5
0 500 1000 1500 2000 2500 3000 3500 4000 0 500 1000 1500 2000 2500 3000 3500 4000
time step time step
40
(c) y error (EKF) (d) y error (NNEKF)
30
error error
0.14 0.14
0.12 0.12
20
0.1 0.1
0.08 0.08
10
0.06 0.06
radians
radians
0.04 0.04
m
0
0.02 0.02
0 0
10 0.02 0.02
0.04 0.04
2 bound 2 bound
20 0.06 0.06
0 500 1000 1500 2000 2500 3000 3500 4000 0 500 1000 1500 2000 2500 3000 3500 4000
time step time step
1690