See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/3859405

Sensor data fusion using Kalman filter

Conference Paper · August 2000

DOI: 10.1109/IFIC.2000.859866 · Source: IEEE Xplore

CITATIONS READS

75 4,739

2 authors, including:

J. Z. Sasiadek
Carleton University
222 PUBLICATIONS   1,652 CITATIONS   

SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Space robotics View project

Navigation and control of a space robot capturing moving target View project

All content following this page was uploaded by J. Z. Sasiadek on 09 April 2015.

The user has requested enhancement of the downloaded file.

 Sensor Data Fusion Using Kalman Filter

J.Z. Sasiadek and P. Hartana

Department of Mechanical & Aerospace Engineering

Carleton University
1125 Colonel By Drive
Ottawa, Ontario, K1S 5B6, Canada
e-mail: [email protected]

Abstract - Autonomous Robots and Vehicles need variables on the vehicle. The examples of these sensors
accurate positioning and localization for their guidance, are encoders, gyroscopes, accelerometers and compasses.
navigation and control. Often, two or more different External sensors measure relationships between the robot
sensors are used to obtain reliable data useful for control and its environment, which can be natural or artificial
system. This paper presents the data fusion system for objects. The examples of external sensors are sonar
mobile robot navigation. Odometry and sonar signals are sensor, radars and laser range finders.
fused using Extended Kalman Filter (EKF) and Adaptive Both of these sensors have advantages and
Fuzzy Logic System (AFLS). The signals used during disadvantages. For short period of time, measurements
navigation cannot be always considered as white noise using internal sensors are quite accurate. However, for
signals. On the other hand, colored signals will cause the long-term estimation, the measurements usually produce a
EKF to diverge. The AFLS was used to adapt the gain and drift. On the contrary, external sensors do not produce the
therefore prevent the Kalman filter divergence. The fused drift, however, the measurements from these sensors are
signal is more accurate than any of the original signals usually not always available [7].
considered separately. The enhanced, more accurate To get the optimal result, both sensors are usually
signal is used to guide and navigate the robot. combined. Because the results of both measurements
contain errors, a special method has to be used to combine
Keywords: Autonomous Robots, Guidance, Navigation the results. The commonly used method is fusing those
and Control, Sensor fusion, Kalman Filter, Adaptive two measurements so it will produce the best desire
Fuzzy Logic System estimation by using the Extended Kalman Filter (EKF)
[6], [8], [9], and [10]. Internal sensors can be used to
estimate the position of the vehicle during a particular
period. External sensors are then implemented to correct
Introduction the errors that come from internal sensors.
To follow the designed path, an autonomous vehicle The type of internal sensors that are widely used in
has to be equipped with three systems: navigation, navigation is odometry sensor. It is mounted on the
guidance, and control system [1], [2]. Navigation system robot’s driving wheels and register angular movements of
is used to provide estimation of position and velocity of the wheels. These angular movements are then translated
the vehicle. Guidance system is used to determine the into linear movements. This process has limited accuracy,
optimal trajectory from a current position and velocity to a for example, if slip has occurred on the wheel, the
desired position and velocity. Control system is used to odometry would register the movement, but in fact, the
determine the commands for vehicle’s actuators to drive vehicle may stay on its position. In the long period, the
the actual velocity and attitude of the vehicle to the value incremental motion of odometry will cause the
commanded by the guidance scheme. accumulative error in positioning process. On the other
There are two basic position-estimation methods hand, the advantage of using odometry is that the
applied in navigation system, i.e. absolute and relative measurement signal is always available.
positioning. [3], [4], [5], [6]. Absolute positioning is based Beside the typical drift error in odometry, other
on navigation beacons, active or passive landmark, map errors can also occur in odometry sensors [11]. One
matching, or satellite-based navigation signal, where important error is systematic error. This error will cause
absolute positioning sensors interact with dynamic the bias in one direction of the movement of the vehicle,
environment. Relative positioning is usually based on so the final position of the vehicle will deviate from the
odometry sensors, or inertial sensors designed path. One method used to reduce this error is by
Internal and external sensors are usually used in conducting a benchmark experiment prior to regular
positioning problems. Internal sensors measure physical operation of the vehicle [3]. This experiment can find the
systematic error and, subsequently, this error is applied to πD n
correct the control system parameters. However, if the ∆S L , R = N L, R (1)
nC e
systematic errors occur frequently, this method may not be
sufficient. For example, if the vehicle uses elastic tires, the where
benchmarking process has to be performed each time the D n = nominal wheel diameter
unequal diameter occurs. It is beneficial if the error C e = encoder resolution
correction can be done in real time operation. n = gear ratio between the motor and drive wheel
Sonar sensor is one type of external sensor. It N = pulse increment shown by encoder.
measures absolute position of the vehicle based on pre-
defined environment. If the sonar sensor is implemented
on the vehicle with odometry sensor, it can be used to C’
correct the systematic error by fusing both measurements
B’
using EKF.
When using EKF to fuse the signal, it is widely A’
known that poorly designed mathematical model for the ∆S R
∆ S L ∆S
EKF will lead to the divergence. If the plant parameters
are subject to perturbations and dynamics of the system
are too complex to be characterized by an explicit
mathematical model, an adaptive scheme is needed.
∆θ
The adaptation scheme that usually used is based on O A B C
fuzzy logic. Many methods have been implemented using R d
this logic. Two recent paper that explain about this
method are the paper presented by Jetto [6] and Sasiadek Figure 1. Two-wheeled vehicle model.
[10]. Jetto used Fuzzy Logic Adapted Kalman Filter
(FLAKF) to prevent the filter from divergence when If the sampling interval of encoder is T , the linear
fusing measurement from odometry and sonar sensors. In and angular velocity of the incremental movement of the
this method, the ratio of innovation and covariance of vehicle can be shown as:
innovation is used as input to the fuzzy logic, and the
output is used to weight the process noise covariance in ∆S R + ∆S L
EKF. Sasiadek used exponential data weighting to prevent v= (2)
2T
the divergence. Mean value and covariance of innovation
are used as the input of the Fuzzy Logic Adaptive ∆S R − ∆S L
ω = (3)
Controller (FLAC). The output is then used to weight dT
process noise and measurement noise covariance in EKF. where d is the distance between the odometry encoder.
This FLAC is implemented on the flying vehicle If the sampling period is made to limit to zero, the
navigating in three-dimensional space. Both those kinematic model of this vehicle then can be described by
methods have shown improvement in the estimation of the the following equations:
vehicle position in comparison with the EKF only.
In this paper, the systematic error in odometry x (t ) = v(t ) cos θ (t ) (4)
sensor is corrected during real-time operation of the
vehicle by using measurements result from the sonar y (t ) = v(t ) sin θ (t ) (5)
sensor. EKF is applied to fuse those two signals to find the
best estimation of position. Adaptive Fuzzy Logic System θ(t ) = ω (t ) (6)
(AFLS) is used to prevent the filter from divergence. The If we denote the state variable of the vehicle as
model of vehicle used in this experiment is based on a x(t ) = [ x(t ) y (t ) θ (t )]T , and the vehicle control input
differential-drive. This type vehicle can be steered by
differentiating the wheels angular velocity. The objective as u(t ) = [v(t ) ω (t )]T , the kinematic model in Eqs. (4 -
of this paper is to develop an efficient method for signal 6) can be written in the form of stochastic differential
fusing to get accurate positioning. equation as:
x (t ) = f (x(t ), u (t )) + w (t ) (7)
Two-Wheeled Vehicle where w (t ) is a zero-mean Gaussian white noise with
The configuration of the two-wheeled vehicle is
covariance matrix Q(t ) , which represents the model
presented in Figure 1. This configuration has two opposed
drive wheels mounted on the left and right sides of the inaccuracies.
vehicle. The motion of the midpoint of the axis represents
the movement of the vehicle.
The incremental movement of the left and right
drive wheel can be formulated as:
Exponentially Weighted EKF y (x k , Π ) = [ x k yk θk d1k d 2k  d nk ]T (20)
Linearizing Eq. (7), and making the sampling period where d nk is the measurement of sonar nth at time k.
is small enough, the equation of EKF can be found. By
There are many methods that can be implemented to
assuming that during this time sampling, the linear and
fuse the signals using EKF. Four methods are described
angular velocities are constant, and that the vehicle is
here. The first method is direct pre-filtering method,
following an arc path (see Wang [12]), then, the equations
which was presented by Green and Sasiadek [13]. In this
for Extended Kalman Filter can be expressed by:
method, both measurement signals are filtered prior of
x −k +1 = x k + B k u k (8) comparison process. The error of those signals is then
used to correct the measurement signal. The scheme of
Pk−+1 = A k Pk A Tk + Q k (9) this method is shown in Figure 2.

K k +1 = Pk−+1C Tk +1 [C k +1 Pk−+1C Tk +1 + R k +1 ] −1 (10) Measurement

EKF
+ x̂
Signal 1 +
x k +1 = x k−+1 + K k +1 [y k +1 − C k +1 x k−+1 ] (11) −

Pk +1 = [I − K k +1C k +1 ]Pk−+1 (12) Measurement −

EKF
Signal 2
where:
Figure 2. Direct Pre-filtering scheme
x k = [ xk yk θ k ]T (13)

 The second method is by applying the total state,

 ∆θ k  
T cosθ k +  0 such as position and velocity, into the filter. The
  2   measurement signals are combined together before fed to
  ∆θ k   the EKF. The advantage of using this method is that
B k = T sin θ k +  0 (14)
  2  
although only one measurement signal is available, the
 0 1 correction still can be done. The more the measurement
  signals become available, the estimation will be more
  accurate. The disadvantage of this method is that one has
to know the dynamic model of the sensor in order find the
1 0 − v k T sin θ k 
predicted position. This method is sometime called direct
A k = 0 1 v k T cos θ k  (15) or total state space EKF [14]. The scheme of this method
0 0 1  is presented in Figure 3.

Q k = [Q1 Q2 Q3 ] (16) Measurement

Signal 1
Q11T + Q33 (T 3 3)v k2 sin 2 θ k  x̂
  EKF
Q1 =  − Q33 (T 3 3)v k2 sin θ k cos θ k  (17)
 − Q (T 2 2)v sin θ  Measurement
 33 k k  Signal 2

 − Q33 (T 3 3)v k2 sin θ k cos θ k  Figure 3. Direct EKF scheme

 
Q 2 = Q22T + Q33 (T 3 3)v k2 cos 2 θ k  (18)
  Measurement + x̂
Q33 (T 2 2)v k cos θ k
  Signal 1 +
−
− Q33 (T 2 2)v k sin θ k  EKF
  −
Q 3 =  Q33 (T 2 2)v k cos θ k  (19) Measurement
Signal 2
 Q33T 
 
Figure 4. Indirect feed forward EKF scheme
and, Q11 = σ x2 , Q 22 = σ y2 , and Q33 = σ z2 are diagonal
elements of covariance matrix Q(t ) of w (t ) in Eq. (7). The third method is indirect feed forward EKF. In
The measurement, in this case, will consist of the this method, the signals are compared before fed into the
measurement from odometry sensor and sonar sensor. The EKF. The estimation error is then fed forward into one of
size of the measurement vector depends on the number of the measured signal. The scheme of this method is
active sonar sensor. In general, this vector can be presented in Figure 4.
expressed as (See Jetto et. al. [7]):
 The last method is almost the same as the third one. Adaptive Fuzzy Logic System
In spite of feed the estimation error forward as in the third
method, in this method, the error is fed back into one of In Kalman filter model, both process noise w k and
the measured signal. Sasiadek and Wang [10] presented measurement noise v k are assumed zero-mean white
this method when fusing the signals, which come from noise sequence with covariance Q k and R k . If the model
INS and GPS. Using the last two methods, the dynamic
model of the sensor is less important, but, when one of EKF is tuned perfectly, the residual between actual and
measurement signal is not available, the correction cannot predicted measurement should be a zero-mean white noise
be performed. The last method is sometime referred as process.
indirect feedback EKF [14]. The scheme of this method is Often, we do not know all parameters of the model
shown in Figure 5. or we want to reduce the complexity of modeling.
Therefore, in real application, the exact value of Q k and
Measurement x̂ R k is not known. If the actual process and measurement
Signal 1 noises are not a zero-mean white noise, the residual in
Kalman filter will also not be a white noise. If this is
− happened, the Kalman filter would diverge or at best
+
converge to a large bound.
EKF Jetto et. al. [7] used fuzzy logic adapted Kalman
Measurement − filter to prevent the filter from divergence. The fuzzy logic
Signal 2 controller uses one input and one output. The ratio
between innovation and covariance of innovation process
Figure 5. Indirect feed back EKF scheme is used as an input. The output is a constant, which is used
to weight the process noise covariance. The controller
In this paper, the second method, that is the direct uses five fuzzy rules and is implemented in a wheeled
EKF method, is used. The measurement signals, which mobile robot equipped with odometry and sonar sensors.
come from odometry and sonar sensors, are combined into Sasiadek and Wang [10] used fuzzy logic adapted
one measurement vector. This measurement result is then controller (FLAC) to prevent the filter from divergence
fed into the EKF. when fusing signals coming from INS and GPS on flying
To prevent the filter from divergence, exponentially vehicle. Nine rules were used. There were two inputs,
weighted EKF is used (See Lewis [15]). Using this which are the mean value and covariance of innovation,
method, the equations of the EKF as explained in the and the output is a constant that is used to weight
beginning of this section have to be adjusted. For exponentially the model and measurement noise
exponentially weighted EKF, the weighted process and covariance.
measurement noise covariance can be written as: In the case of fusing signals that come from
odometry and sonar sensors, sometime only odometry
R k = Rα −2( k +1) (21) measurements are available. In this case, the innovation
will be a white noise as long as the process and
Q k = Qα −2( k +1) (22) measurement noises are assumed as a white noise. But
where α ≥ 1 . Q and R are constant matrices of process when the sonar measurements become available, and
and measurement noise covariance. For α > 1 , as time k combined with the odometry measurement, the innovation
might be not a white noise anymore. This will cause the
increases, Q k and R k will decrease, which means that
filter to diverge.
the most recent measurement is given higher weighting. When systematic error occurs in the vehicle, the
If the weighted estimation covariance is defined as: process and measurement noise actually are not a gaussian
white noise. This will deviate from the requirement of the
Pkα − = Pk−α 2 k (23)
EKF and the divergence in filter will occur. AFLS can be
then the EKF equations become: used to adapt the filter gain so that the divergence can be
avoided. The adaptation process used in this paper is
x k−+1 = x k + B k u k (24) based on exponential data weighting (Lewis [15]). The
adaptation process will change the Q and R , and
Pkα+−1 = α 2 A k Pkα A Tk + Q k (25) subsequently the Kalman gain K of the EKF. The scheme
of the adaptation process is shown in Figure 6.
R k +1
K k +1 = Pkα+−1C Tk +1 [C k +1 Pkα+−1C Tk +1 + ] −1 (26) The membership function used in this AFLS is
α2 displayed in Figure (7 – 9). The AFLS uses nine rules ,
which are summarized in Table 1.
x k +1 = x k−+1 + K k +1 [y k +1 − C k +1 x k−+1 ] (27)

Pkα+1 = [I − K k +1C k +1 ]Pkα+−1 (28)

 Estimated Systematic error in odometry measurement, which comes
Odometry AFLS
& Sonar α C
Position from unequal in wheel’s diameter, is also considered. The
measu- ŷ k vehicle is planned to follow sinus path in in-door
rement environment. The map of the in-door environment along
K Delay with the movement of the mobile vehicle that has
yk + zk + xk
systematic error is shown in Figure 10.
− + x k −1 Three simulation experiments have been performed.
The first experiment is to show the implementation of
C A EKF in the mobile robot using odometry sensor, where the
x −k sensor has systematic error. The result of this experiment
is shown in Figure 11. In this experiment, it shows that the
Figure 6. Adaptive Fuzzy Logic System (AFLS) scheme
implementation of EKF with only one measurement signal
is available, cannot be used to correct the systematic error.
The EKF in this case only filters the gaussian white noise
zero small large of the odometry measurement error. The systematic error
however, still present in the movement of the mobile
vehicle.
The second experiment is to use the EKF to fuse
measurement signals that come from odometry and sonar
sensor without using AFLS. This experiment result is
0 ( 4) 2 1.1( 4) 2 (m 2 ) shown in Figure 12. The present of sonar sensor, which
measures the relation of the mobile vehicle and its
Figure 7. MF of innovation process covariance environment, reduces the systematic error, and the mobile
vehicle can follow the designed path. However, the
movement of the mobile vehicle in this case is not smooth.
zero small large The result of sonar measurement in this experiment is not
used efficiently to improve the position estimation.
The third experiment is to use AFLS to adapt the
gain of EKF to prevent the filter from divergence. In this
experiment, when the sonar measurement becomes
available, the EKF uses this signal to improve its
0 2 20 (m) estimation. AFLS makes the position estimation smoother
than without AFLS. The result of this experiment is
Figure 8. MF of innovation process mean value shown in Figure 13.

zero small medium large C

D

(α )
5m
1 1.02 1.1 1.2

Figure 9. MF of α
S G
Table 1. Rules table for AFLS
Innovation process mean value
α 5.15m
4.8m

Zero Small Large

Innovation Zero Small Zero Large
process Small Zero Large Medium
covariance Large Large Medium Zero B
A
6m

Experiments and Results

Simulation experiments have been conducted to Figure 10. Map of in-door environment
show the implementation of AFLS when fusing the
signals that come from odometry and sonar sensor.
 Position of the vehicle
Conclusions
3
In this paper, Extended Kalman Filter (EKF) has
2
been used to estimate the position of the mobile vehicle.
To prevent the filter from divergence, the innovation and
1 covariance of innovation process are monitored by using
Adaptive Fuzzy Logic System (AFLS). The result is an
y Position (meter)

0 adaptation in the gain of EKF.

Odometry and sonar sensors have been used to
-1
simulate the method. From the simulation experiment, it
-2 shows that beside the improvement in the estimation of
position, the method can also be used to correct the
-3 systematic error. Using this method, real-time operation of
the vehicle can be reduced.
-4

0 1 2 3 4 5 6
x Position (meter)
7 8 9 10
References
[1] Gai, Eli (1996). Guidance, navigation, and control
Figure 11. Simulation experiment result using EKF with
from instrumentation to information management. Journal
only odometry measurement
of Guidance, Control, and Dynamics 19(1), 10-14.
Position of the vehicle [2] Kaminer, I., Pascoal, A., Hallberg, & E., Silvestre, C.
(1998). Trajectory tracking for autonomous vehicles: An
3
integrated approach to guidance and control. Journal of
2 Guidance, Control, and Dynamics 21(1), 29-38.
[3] Borenstein, J., & Feng, L. (1996). Measurement and
1 correction of systematic odometry errors in mobile robots.
y Position (meter)

IEEE Transactions on Robotics and Automation 12(6),

0
869-879.
-1 [4] Shoval, S., Mishan, A., & Dayan, J. (1998). Odometry
and triangulation data fusion for mobile-robots
-2 environment recognition. Control Engineering Practice 6,
1383-1388.
-3
[5] Jetto, L., Longhi, S., & Vitali, D. (1999). Localization
-4 of a wheeled mobile robot by sensor data fusion based on
a fuzzy logic adapted Kalman filter. Control Engineering
0 1 2 3 4 5 6 7 8 9 10
x Position (meter)
Practice 7, 763-771.
[6] Jetto, L., Longhi, S., & Venturini, G. (1999).
Figure 12. Simulation experiment result using EKF with Development and experimental validation of an adaptive
odometry and sonar measurement extended Kalman filter for the localization of mobile
Position of the vehicle robots. IEEE Transactions on Robotics and Automation
15(2), 219-229.
3 [7] Santini, A., Nicosia, S., & Nanni, V. (1997).
Trajectory estimation and correction for a wheeled mobile
2
robot using heterogeneous sensors and Kalman filter.
Preprints of the Fifth IFAC Symposium on Robot Control
1
(11-16). September 3-5, 1997, Nantes, France.
y Position (meter)

0 [8] Jetto, L., Longhi, S., & Venturini, G. (1997).

Development and experimental validation of an adaptive
-1 estimation algorithm for the on-line localization of mobile
robots by multisensor fusion. Preprints of the Fifth IFAC
-2
Symposium on Robot Control (219-229). September 3-5,
-3
1997, Nantes, France.
[9] Tham, Y. K., Wang, H., & Teoh, E. K. (1999). Multi-
-4 sensor fusion for steerable four-wheeled industrial
vehicles. Control Engineering Practice 7, 1233-1248.
0 1 2 3 4 5 6 7 8 9 10
x Position (meter) [10] Sasiadek, J. Z., & Wang, Q. (1999). Sensor fusion
based on fuzzy Kalman filtering for autonomous robot
Figure 13. Simulation experiment result using EKF with vehicle. Proceeding of the 1999 IEEE International
odometry and sonar measurement, adapted by AFLS
 Conference on Robotics & Automation (pp. 2970-2975). [13] Green D. N. & Sasiadek, J. Z. (1998). Path tracking,
May 1999, Detroit, Michigan. obstacle avoidance and dead reckoning by an autonomous
[11] Borenstein, J., Everett, H. R., & Feng, L. (1996). planetary rover. International Journal of Vehicle Design
Navigating Mobile Robots. Systems and Techniques. A. V(1).
K. Peters, Wellesley, Massachusetts. [14] Maybeck, P. S. (1979). Stochastic models,
[12] Wang, C. M. (1988). Location estimation and estimation, and control . Vol. 1.Academic Press Inc., San
uncertainty analysis for mobile robots. Proceeding of the Diego, California.
IEEE International Conference on Robotics and [15] Lewis, F. L. (1986). Optimal Estimation. With an
Automation (pp. 1230-1235). introduction to stochastic control theory. John-Wiley &
Sons, New York.

View publication stats