Annual Reviews

' in Control
PERGAMON Annual Reviews in Control 26 (2002) 203-228
www.elsevier.com/locate/arcontrol

SENSOR FUSION

J.Z. Sasiadek
Department of Mechanical & Aerospace Engineering,
Carleton University
e-mail: [email protected]

This paper was origirtally prepared as an IFAC Professional Brief

Abstract. Sensor fusion is a method of integrating signals from multiple sources.

It allows extracting information from several different sources to integrate them
into single signal or information. In many cases sources of information are sensors
or other devices that allow for perception or measurement of changing
environment. Information received from multiple-sensors is processed using
"sensor fusion" or "data fusion" algorithms. These algorithms can be classified
into three different groups. First, fusion based on probabilistic models, second,
fusion based on least-squares techniques and third, intelligent fusion. The
probabilistic model methods are Bayesian reasoning, evidence theory, robust
statistics, recursive operators. The least-squares techniques are Kalman filtering,
optimal theory, regularization and uncertainty ellipsoids. The intelligent fusion
methods are fuzzy logic, neural networks and genetic algorithms. This paper will
present three different methods of intelligent information fusion for different
engineering applications. Chapter 2 is based on Sasiadek and Wang (2001) paper
and presents an application of adaptive Kalman filtering to the problem o f
information fusion for guidance, navigation, and control. Chapter 3 is based on
Sasiadek and Hartana (2000) and Chapter 4 on Sasiadek and Khe (2001) paper.

Keywords. Sensor fusion, probabilistic models, least squares techniques, fuzzy

logic, neural networks, genetic algorithms.

1. INTRODUCTION different groups. First, fusion based on

probabilistic models, second, fusion based
Sensor fusion is a method of integrating on least-squares techniques and third,
signals from multiple sources. It allows intelligent fusion. The probabilistic model
extracting information from several different methods are Bayesian reasoning, evidence
sources to integrate them into single signal theory, robust statistics, recursive operators.
or information. In many cases sources of The least-squares techniques are Kalman
information are sensors or other devices that filtering, optimal theory, regularization and
allow for perception or measurement of uncertainty ellipsoids. The intelligent fusion
changing environment. Information received methods are fuzzy logic, neural networks
from multiple-sensors is processed using and genetic algorithms. This paper will
"sensor fusion" or "data fusion" algorithms. present three different methods of intelligent
These algorithms can be classified into three information fusion for different engineering

1367-5788/02/$20 © 2002 Published by Elsevier Science Ltd.

PII: S 1367-5788(02)00045-7
204 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228

applications. Chapter 2 is based on Sasiadek 2. FUZZY ADAPTIVE KALMAN

and Wang (2001) paper and presents an FILTERING FOR INS/GPS DATA
application of adaptive Kalman filtering to FUSION AND ACCURATE
the problem of information fusion for POSITIONING
guidance, navigation, and control. Chapter 3
is based on Sasiadek and Hartana (2000) and 2.1 Introduction
Chapter 4 on Sasiadek and Khe (2001)
paper. This chapter presents the method of sensor
fusion based on the Adaptive Fuzzy Kalman
The data/sensor problems and related Filtering. This method has been applied to
methods are used in conjunction to many fuse position signals from the Global
engineering applications. For example, Positioning System (GPS) and Inertial
guidance, navigation, and control of vehicles Navigation System (INS) for the
require large number of information from autonomous mobile vehicles. The presented
different sources. This information often is method has been validated in 3-D
similar and has to be integrated into one environment and is of particular importance
meaningful signal or information that can be for guidance, navigation, and control of
used in control systems. In this paper the flying vehicles. The Extended Kalman Filter
sensor/data fusion will be shown for three (EKF) and the noise characteristic have been
different cases. In all three cases the Kalman modified using the Fuzzy Logic Adaptive
Filter method is used to integrate System and compared with the performance
signals/information received from multiple- of regular EKF. It has been demonstrated
sensor sources. Also, in all three cases the that the Fuzzy Adaptive Kalman Filter gives
modification of Kalman Filter method is better results (more accurate) than the EKF.
introduced to improve performance. This
modification is based on Fuzzy Logic 2.2 Sensor Fusion
System (FLS). In that sense, the integration
method becomes an intelligent integration, When navigating and guiding an
and is applicable to broader number of autonomous vehicle, the position and
industrial cases. The chapter 2 is presenting velocity of the vehicle must be determined.
an integration of data/sensor signals received The Global Positioning System (GPS) is a
from the Global Positioning System (GPS) satellite-based navigation system that
and Inertial Navigation System (INS). The provides a user with the proper equipment
integration allows for better and more access to useful and accurate positioning
accurate positioning. information anywhere on the globe (see
Brown and Hwang, 1992). However, several
Chapter 3 presents the navigation of an errors are associated with the GPS
autonomous robot based on sensor/data measurement. It has superior long-term error
fusion method for signals received from performance, but poor short-term accuracy.
sonar and odometry sensors. The fusion For many vehicle navigation systems, GPS
process allows for more efficient navigation is insufficient as a stand-alone position
and obstacle avoidance. In both cases system. The integration of GPS and Inertial
described in chapter 2 and 3 the Kalman Navigation System (INS) is ideal for vehicle
Filter method is backed up by the Fuzzy navigation systems. In generally, the short-
Logic System (FLS). term accuracy of INS is good; the long-term
accuracy is poor. The disadvantages of
Finally, chapter 4 is presenting an attempt to GPS/INS are ideally cancelled. If the signal
design integration method based fully on of GPS is interrupted, the INS enables the
FLS. Results and conclusions are shown navigation system to coast along until GPS
separately for those three different cases. signal is reestablished. The requirements for
accuracy, availability and robustness are
therefore achieved.
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 205

Kalman filtering is a form of optimal filter from divergence. The fuzzy logic
estimation characterized by recursive adaptive controller (FLAC) will continually
evaluation, and an internal model of the adjust the noise strengths in the filter's
dynamics of the system being estimated. The internal model, and tune the filter as well as
dynamic weighting of incoming evidence possible. The FLAC performance is
with ongoing expectation produces estimates evaluated by simulation of the fuzzy
of the state o f the observed system (see •adaptive extended Kalman filtering scheme
Abidi and Gonzalez, 1992). An extended of Fig.1.
Kalman filter (EKF) can be used to fuse
measurements from GPS and INS. In this INS I Corrected }o6ition,vetocity,etc
EKF, the INS data are used as a reference
trajectory, and GPS data are ,applied to
update and estimate the error states of this T Predictedmeasurements
i J
trajectory. The Kalman filter requires that all

II
Estimated INS errors
the plant dynamics and noise processes are
exactly known and the noise processes are
zero mean white noise. If the theoretical
behavior of a filter and its actual behavior do
not agree, divergence problems will occur.
There are two kinds of divergence: Apparent
Residual~lF / ~
I
divergence and True divergence (Gelb,
1992). In the apparent divergence, the actual
estimate error covariance remains bounded,
I
but it approaches a larger bound than does Fig.1. Fuzzy adaptive extended Kalman
predicted error covariance. In true filter [Sasiadek, J. Z., and Wang, Q. (2001)]
divergence, the actual estimation covariance
eventually becomes infinite. The divergence
due to modeling errors is critical in Kalman 2.2.1 WeightedEKF
filter application. If, the Kalman filter is fed
information that the process behaved one Because the processes of both GPS and INS
way, whereas, in fact, it behaves another are nonlinear, a linearization is necessary.
way, the filter will try to continually fit a An extended Kalman filter is used to fuse the
wrong process. When the measurement measurements from the GPS and INS. To
situation does not provide enough prevent divergence by keeping the filter
information to estimate all the state variables from discounting measurements for large k,
of the system, in other words, the computed the exponential data weighting (Lewis,
estimation error matrix becomes 1986) is used.
unrealistically small, and the filter disregards
the measurement, then the problem is The models and implementation equations
particularly severe. Thus, in order to solve •for the weighted extended Kalman filter are:
the divergence due to modeling errors, we
can estimate unmodeled states, but it adds Nonlinear dynamic model
complexity to the filter and one can never be
sure that all of the suspected unstable states xk. , = f(x k,k) + w k (1)
are indeed model states (Lewis, 1986).
w k ~ N(0, Q)
Another possibility is to add process noise. It
makes sure that the Kalman filter is driven
Nonlinear measurement model
by white noise, and prevents the filter from
disregarding new measurement. In this
z~ = h(xk, k) + vk (2)
paper, a fuzzy logic adaptive system (FLAS)
is used to adjust the exponential weighting v k ~ N(0, R)
of a weighted EKF and prevent the Kalman
206 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

Let us set the model covariance matrices P~+~ -- a2~,~Pff~ T + Q (13)

equal to
Computing the a posteriori covariance
R k = R a -2(k+0 (3) matrix gives:
Qk = Q a-2(k+l) (4)
e~ = (I- KkH k)Pff- (14)
where, c~1, and constant matrices Q and R.
For a>l, as time k increases, the R and Q The initial condition is:
decrease, so that the most recent
measurement is given higher weighting, ff Pg- = P0
or=l, it is a regular EKF.
In equation (10), the term z k -~, is called
By defining the weighted covariance residuals or innovations. It reflects the
degree to which the model fits the data.
Pff- = Pk-a2k (5)
2.2.2 INS and GPS
The Kalman gain can be computed:
The inertial navigation system (INS) consists
Kk = Pk Hk (HkPk Hk + Ra-Z(k+l)) -1
- T - T
of a sensor package, which includes
accelerometers and gyros to measure
=Pk Hk HkPk Hk + a 2 accelerations and angular rates. By using
these signals as input, the attitude angle and
three-dimensional vectors of velocity and
The predicted state estimate is: position are computed (Jochen et al., 1994).
The errors in the measurements of force
ik+l = f(xk, k) (7) made by the accelerometers and the errors in
the measurement of angular change in
The predicted measurement is: orientation with respect to inertial space
made by gyroscopes are two fundamental
= h( q, k) (8) error sources, which affect the error behavior
of an inertial system. The inertial system
The linear approximation equations can be error response, related to position, velocity,
presented in form: and orientation is divergent with time due to
noise input (Kayton and Fried, 1997). There
Of(x,k). are biases associated with the accelerometers
~*" Ox ~=~; (9) and gyros. In order to correct the errors of
INS, the GPS measurements are used to
estimate the inertial system errors, subtract
The predicted estimate on the measurement them from the INS outputs, and then obtain
can be computed: the corrected INS outputs. There is number
of errors in GPS, such as ephemeris errors,
i , = i f +Kk(Z k - ~ , ) (10) propagation errors, selective availability,
Oh(x,k). multi-path, and receiver noise, etc. Using
Hk = Ox x=x; (11) differential GPS (DGPS), most of the errors
can be corrected, but the multi-path and
Computing the a priori covariance matrix: receiver noise cannot be eliminated.

Pk-+l = ¢Yi~k Pk eliot + Q 0~-2(k+1) (12) 2.3 Fuzzy Logic Adaptive System

It is assumed that both, the process noise wk,

Re-writing (12) gives:
and the measurement noise vk are zero-mean
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 207

white sequences with known covariance Q The fuzzy adaptive Kalman filtering has
and R in the Kalman filter, ff the Kalman been used for guidance and navigation of
filter is based on a complete and perfectly mobile robots, especially for 3-D
tuned model, the residuals or innovations environment. The navigation of flying robots
should be a zero-mean white noise process. requires fast, and accurate on-line control
ff the residuals are not white noise, there is algorithms. The "regular" Extended Kalman
something wrong with the design and the Filter requires high number of states for
filter is not performing optimally (Lewis, accurate navigation and positioning and is
1986). The Kalman filters will diverge or unable to monitor the parameters changing.
coverage to a large bound. In practice, it is The FLAC requires smaller number of states
difficult to know the exact values for Q and for the same accuracy and therefore it would
R. In order to reduce computation, we have need less computational effort. Alternatively,
to ignore some errors, but sometimes those the same number of states (as in "regular"
unmodeled errors will become significant. filter) would allow for more accurate
These are the instrument bias errors of INS. navigation.
Sometimes the wk may be different than zero
mean. In those cases, the residuals can be 2.3.1 Fuzzy adaptive Kalman filtering for
used to adapt the filter. In fact, the residuals parameter uncertainties
are the differences between actual
measurements and best measurement Sometimes, uncertain or time varying
predictions based on the filter's internal parameters are considered to exist in the Q
model. A well-tuned filter is that where the and R matrices. The fuzzy adaptive Kalman
95% of the autocorrelation function of filtering is used to detect and then adapt the
innovation series should fall within the _ 2o filter on-line. There are two groups of fuzzy
boundary (Cooper and Dyrrant-White, controllers. In those two fuzzy controllers,
1994). If the filter diverges, the residuals the covariance of the residuals and the mean
will not be zero mean and become larger. of residuals are used as the inputs to both
There are very few papers on application of controllers for all three fuzzy inference
fuzzy logic to adapt the Kalman filter. Other engines. The exponential weighting ct for
authors (Abdelnour et al., 1993)), use fuzzy first group controller and the scales for
logic for on-line detection, and correction of second group controller of three axes are the
divergence in a single state Kalman filter. outputs.
There were three inputs and two outputs to
fuzzy logic controller (FLC), and 24 rules The first group, which output is ~ was used
were used. In our works (Sasiadek and to detect the filter divergence and adapt the
Wang, 1999), the basic adaptive fuzzy logic EKF. Generally, when the covariance is
controller has been introduced and designed. becoming large, and mean value is moving
In this paper the new FLAC is proposed. The away from zero, the Kalman filter is
purpose of the fuzzy logic adaptive becoming unstable. In this case, a large ct
controller (FLAC) is to detect the bias of will be applied. A large a means that process
measurements and prevent divergence of the noises are added. It can ensure that in the
extended Kalman filter. It has been applied model all states are sufficiently excited by
in three a x e s - East (x), North (y), and the process noise. When the covariance is
Altitude (z). The covariance of the residuals extremely large, there are some problems
and mean values of residuals are used to with the GPS measurements, so the filter
decide the degree of divergence. The value cannot depend on these measurements
of covariance relates to R. ff the residual has anymore, and a smaller a will be used. By
zero mean, the equation for covariance of the selecting an appropriate, a, the fuzzy logic
residual is: controller will adapt the Kalman filter
optimally and try to keep the innovation
Pz = H k P ~- H Tk + R (15) sequence acting as zero-mean white noise.
208 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228

Some membership functions are shown at I f the covariance of residuals is small and
figure 2, 3 and 4. the mean values is small then the scale is
large.
Zero~Large Table 1 and 2 are the rule table for those two
groups of fuzzy controllers.
Table. 1. Rule Table for a
0 (2.5) 2 (5) 2 P~/R [m2]
Mean Value
Fig.2. Covariance Membership Functions
Z S L
[Sasiadek, J. Z., and Wang, Q. (2001),

Zero~Large P
Z
S
Z
S
S
L
S
S

L Z NS NS
I
0 1 10 Mean Ira]

Fig.3. Mean Value Membership Functions S --- Small; L --- Large;

[Sasiadek, J. Z., and Wang, Q. (2001), Z --- Zero; NS --- Negative Small

Table. 2 Rule Table for Scale

Mean Value
Scale
Z S L

Z Z z Z
Fig.4. a Membership Functions [Sasiadek, J.
Z., and Wang, Q. (2001), P S S S S
L L S Z
The fuzzy logic controller uses 9 rules, such
as"

I f the covariance of residuals is large and

the mean value is zero Then a is zero. 2.3.2 Fuzzy adaptive Kalman filtering for
non-white process noise
I f the covariance of residuals is zero and the
mean value is large Then ct is small. It is assumed that the process noise wk is
white noise for Kalman filtering. But
The second group, which output is scale, sometime the process noise could be
was used to detect the change of correlated with itself, non-white. In this case,
measurement noise covariance R. From we can add a shaping filtering that
equation (15), the R is related to the manufactures colored noise wk with a given
covariance of residual, the larger the spectral density from white noise, but it will
covariance of residual, the more the increase the state variables. In some real-
measurement noise. When the fuzzy logic time situation, the computing time have a
controller finds that the covariance of restriction for increasing the state variables.
residual is larger than that expected, it We can use a fuzzy adaptive Kalman
applies a large scale to adjust the a. A filtering to adaptive the process noise rather
sample rule is: than add more state variables. There are 9
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 209

rules and therefore, little computational time Table. 3. Rule Table for FLAS
is needed. The membership functions for this
fuzzy control are showed as figure 5.8, 5.9, Mean Value
and 5.10. Ot
Z S L
The FLAC uses 9 rules, such as:
Z S Z Z
I f the covariance o f residuals is large and
the mean values are zero Then ct is large. S Z L M
P
I f the covariance o f residuals is zero and the L L M Z
mean values are large Then ot is zero.

S --- Small; M --- Medium;

L --- Large; Z --- Zero;
zero small large

2.4 Simulation
0 (4) 2 1.1(4) 2 (m 2)
MATLAB codes developed by authors has
been used to simulate and test the proposed
Fig.5. Covariance Membership Functions method.
[Sasiadek, J. Z., and Wang, Q. (2001)]
The state variables used in simulation are:

zer( small large xk ---[Xk, 2k, Yk, J~k, Zk, 2k, cat, cat] (16)

The states are position, and velocity errors of

the INS East, North, Altitude, GPS range
bias and range drift.
0 2 20 (m)
2.4.1 Simulation experiment 1
Fig.6. Mean Value Membership Functions
[Sasiadek, J. Z., and Wang, Q. (2001)] The first part of simulation uses the fuzzy
adaptive Kalman filtering for parameter
uncertainties.

zer~ small medium large The designed standard deviation of GPS

measurement R is 5 [m]. The designed
standard deviations of Q for INS are 0.0012
meter, 0.0012 meter, and 0.0012 meter for
the East (x), North (y), and Altitude (z)
1 1.02 1.1 1.2 (a) respectively.

Fig.7. t~ Membership Functions [Sasiadek, The simulations (Table 4, 5 and 6 and Figure
J. Z., and Wang, Q. (2001) ] 8 and 9) show that after the filter is
stabilized, the actual error covariance of
fuzzy logic adaptive EKF almost agrees with
the theory error covariance. In the Table 4, 5
and 6, the designed parameters are Q and R.
 210 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228

The 5Q, 2R etc. mean that the real time covariance of GPS measurement R is 25
parameters are 5 and 2 time as large as the [m2]. It is assumed that the measurements of
designed Q and R. In figure 8, and 9, dashed INS have some biases. In the first part of this
lines are the theoretical covariance of EKF, simulation (Fig. 5), the mean values of INS
and the solid lines are the covariance of are 0.0014 meter, 0.00035 meter, and 0.0007
fuzzy adaptive EKF. meter for the East (x), North (y), and
Altitude (z) respectively. A white noise with
a standard deviation of 3 meter is added to
Table 4 Comparison of theoretical and GPS measurements. The sample period is 1
actual error variance (X-axis) second. The first row in Fig. 10 is the
innovations of fuzzy adaptive EKF and EKF
in East (x). The innovation of EKF had a
Q R Theory Actual large drift, and was stable at a high mean
5Q R 3.1711 3.3912 value. The fuzzy adaptive EKF clearly
improved the performance of EKF, and the
50 5.3293 5.3121
mean value was much smaller than that of
3Q 2R 4.6896 4.8469 EKF. Other figures present the corrected
5Q 4R 8.9612 8.3122 position (first column) and velocity (second
column) errors. The corrected error is the
current INS error minus estimated INS error.
The dashed lines are the corrected errors of
Table 5 Comparison of theoretical and EKF, and the solid lines are the corrected
actual error variance (Y-axis) errors of fuzzy adaptive EKF. The fuzzy
adaptive EKF significantly reduced the
corrected position and velocity errors. In the
second part of this simulation (Fig. 11), the
Q R Theory Actual
same measurements as in the first part of this
5Q R 2.5540 2.7227
simulation for INS were used. A white noise
5Q 2R 4.2877 4.1030 with a standard deviation of 2 meter from 0 s
3Q 2R 3.7694 4.0864 to 1000 s and 1500 s to 2000s was applied to
5Q 4R 7.2002 7.7340 GPS measurements. From 1000 s to 1500 s,
the standard deviation of 6 meter with mean
value of 6 meter was added to GPS
measurements. Although, the GPS
Table 6 Comparison of theoretical and measurement noises features were changed,
actual error variance (Z-axis) the fuzzy adaptive EKF still worked well.
[Sasiadek, J. Z., and Wang, Q. (2001) ] Those simulations also showed that the
corrected errors of EKF were proportional to
the mean values of INS measurements. In
other word, the more errors are not modeled,
Q R Theory Actual the worse the EKF performs.
5Q R 0.8344 0.8072
5Q 2R 1.3979 1.1796
2.5 Summary
3Q 2R 1.2268 1.2989
5Q 4R 2.3417 2.5005 In this chapter, a fuzzy adaptive extended
Kalman filter has been developed to detect
and prevent the EKF from divergence. By
2.4.2 Simulation experiment 2 monitoring the innovations sequences, the
FLAS can evaluate the performance of an
I n the second set of simulations, we simulate EKF. ff the filter does not perform well, it
the inputs of non-white process noise. The
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 211

would apply an appropriate weighting factor state vector and associated matrices. When a
ot to improve the accuracy of an EKF. designer lacks sufficient information to
develop complete models or the parameters
The FLAS can use lower order state-model will slowly change with time, the fuzzy
without compromising accuracy controller can be used to adjust the
significantly. Other words, for any given performance of EKF on-line, and it will
accuracy, the fuzzy adaptive Kalman filter remain sensitive to parameter variations by
may be able to use a lower order state model. "remembering" most recent N data samples.
The FLAS makes the necessary trade-off It can be used to navigate and guide
between accuracy and computational burden autonomous vehicles or robots and achieved
due to the increased dimension of the error a relatively accurate performance.

150

X
a.
Q)
100 . . . . . . . . . . . . . . . . . . . . ." . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - . . . . . . . . . . . . . . . . . . . . . .i. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ; ...................... .- ..................... ~ ....................

¢-

.f-

50

0
0 100 200 300 400 500 " 600 700 800 900 1000
"lime(s)
80

~,60

40
I,._

> 20
................ i ................................................................................................................................................ i...............................................
0
0 100 200 300 400 500 600 700 800 900 1000
~me(s)
80

nN 60

40

> 20

0
0 100 200 300 400 500 600 700 800 900 1000
"time(s)

Fig.8. Actual and Theoretical Covariance for 5Q and R. [Sasiadek, J. Z., and Wang, Q. (2001),]
212 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

300

nX 200
i " j r I --
fi i , ; I I t
~ 100 t4 ...... I
..........~......................., ........................,L..................................K ................ l ....................i . . . . . . . . . . . . . . . . . . . . . . . . . .
i ' ! ~, , --I---
0
0 100 200 300 400 500 600 700 800 900 1000
-rime(s)
200 i !'
i I
J
i i I i
~ , 150 I

i i i i
i
~100 .................. -l .i
i i
J.. tI
o--

l i
> 50

0
0 100 200 3~ 4~
i . .4. . 5~
. . . . . . . .

600
i

700 8~ 900
t

i
1000
"rime(s)
200

N 150 .................. 4- .................... ~ ............................................................................... ~ ........................... ~ ................. '---'-------',-"--'--"'---

~
o~
100

> 50 _. :. . . . . . . . . . . . . . . . . . . . . . 1-.- . . . . . . . . . . . . . . . . . . . . . . . . . .

• "--4. .... i t i

0
0 100 200 300 400 500 600 700 800 900 1000
-nrne(s)

Fig. 9. Actual and Theory Covariance for 5Q and 4R [Sasiadek, J. Z., and Wang, Q. (2001)]
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 213

Fuzzy Adaptive EKF EKF

g
~ 2 o ~
g
"~"
...........................
~ l l i l ~ ' ! " ' "lr ....... '"i' ..... '

~P
"~-2% 500
"
i000 1500 2000
I -~X

0 500 lObO 1500 2000

50 ' i J , 0.5

go
"X

-5C -0.5
500 1000 1500 2000
~,o 0 500 1000 1500 2000 ~ (I

~_,ol
>.
20
0 500 1000 1500 2000
i
~
>,
O"I ~ .

-0.5 I
0 500 1000 1500 2000

E 0 ~-~ ~ ~

"N0.5
-2C
0 500 1000 1500 2000 500 1000 1500 2000
Time (s) Time (s)

Fig. 10. Simulation A [Sasiadek, J. Z., and Wang, Q. (2001)]

Fuzzy Adaptive EKF EKF

E" 2 0 ~ 1 . '
o
• q, i

~ o ..... I ..... I
x 201 I I I
- o 5oo lOOO 15oo 2000 0 500 1000 1500 2000

_
E
o ;oo ooo o 2 Jo0 -o
-"
E
' _,=o 2000

"~'2C,,,o: _ _ ~0"5 I

~o._
?,
~oI....
o. -2c @-0 51
>, q) 500 1000 1500 2000 >. ' 0 500 1000 1500 2000
1C ..,..,- %,.~.,.~" . . . . ..~_
,,,,j - ~ "

N
'N

-1C i 500 1000 1500 2000 00 500 1000 1500 2000

Time (s) Time (S)

Figure 11. Simulation B [Sasiadek, J. Z., and Wang, Q. (2001)]

214 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228

To solve the positioning problems, there are

3. SENSOR FUSION FOR DEAD- two types of sensors available: internal and
RECKONING MOBILE ROBOT external sensors, as explained by McKerrow
NAVIGATION (1991). Internal sensors measure physical
variables on the vehicle itself. This self-
3.1 Introduction containing characteristic means the
measurement results of these sensors are
In positioning and localization problems, almost always available to solve positioning
two or more different sensors are often used problems. The examples of these sensors are
to obtain the best estimation data for control accelerometer, odometry, gyroscopes, and
system. Extended Kalman Filter (EKF) is compasses. External sensors measure
widely used to fuse those data to obtain one relationships between the vehicle and its
integrated, optimal result. One consideration environment, which can be natural or
is using EKF when the signal used during artificial objects. The examples of external
navigation is a white noise signals. sensors are satellite signal receiver, sonar
However, many signals in real world sensor, radars, and laser range finders.
include non-white noise. In this case the
regular Kalman filter derived for white When the above sensors are implemented to
noise would diverge. This paper presents the solve positioning problems, both have
sensor fusion for dead-reckoning mobile advantages and disadvantages. For short
robot navigation. Odometry and sonar periods, measurements using internal sensors
signals are fused using Extended Kalman are quite accurate. However, for long-term
Filter (EKF) and Adaptive Fuzzy Logic estimation, the measurements usually
System (AFLS). The AFLS was used to produce a drift. On the contrary, because it
adapt the gain and therefore prevent the measures absolute quantity, external sensors
Kalman filter divergence. The fused signal do not produce the drift, however, the
is more accurate than any of the original measurements from these sensors are usually
signals considered separately. The not always available, Santini, et al. (1997).
enhanced, more accurate signal is used to
guide and navigate the robot. The common method used in navigation
problem is to combine those sensors so that
3.2 Sensor Fusion it will produce the best desirable output. The
common combination method is by applying
For the navigation system, there are two the Extended Kalman Filter (EKF), such as
basic position-estimation methods shown in the work by Jetto, et al., (1997,
commonly applied, i.e. relative and absolute 1999), Tham, et al., (1999), Sasiadek and
positioning, see Borenstein (1996), Shoval, Wang (1999), Sasiadek and Hartana (2000).
et al. (1998), Jetto, et al. (1999), Jetto, et al.
(1999), and Roumeliotis, et al. (1999). The most common combination of sensors
Relative positioning, which is sometimes used in positioning and localization
called dead reckoning, is usually based on problems si combination of odometry and
inertial sensors or odometry sensors. In this sonar sensor. Odometry sensor is mounted
method, the calculated distance from initial on the robot's driving wheels and register
position determines current position angular movements of the wheels, which are
estimation. In an absolute positioning then translated into linear movements.
system, the positioning sensors interact with Beside the drawback that the translation
a dynamic environment, which can be introduces the error, see Sasiadek and
navigation beacons, active or passive Hartana (2000), this implementation makes
landmark, map matching, or satellite-based the odometry signal always available. The
navigation signal, to find the position sonar sensor, which measures absolute
estimation. position of the robot, is used to update the
position measured by odometer.
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 215

Other errors can also occur in odometry find the best estimation of position. Adaptive
sensors. One is systematic error. This error Fuzzy Logic System (AFLS) is used to
causes the bias in one direction of the prevent the filter from divergence. The
movement of the vehicle. Borenstein and objective of this paper is to develop an
Feng (1996) presented their method to efficient method for signal fusing to get
correct this error. The method is based on a accurate positioning.
benchmark experiment performed prior to
regular operation of the vehicle. The 3.3 Mathematical Model
experiment can find the systematic error and,
subsequently, the error is applied to correct The model of the vehicle used in the
the control system parameters. If the simulation is based on a differential-drive. In
systematic errors occur frequently, this this model, the vehicle can be steered by
method may not be sufficient. For example, differentiating the wheels angular velocity.
if the vehicle uses elastic tires, the The kinematic model of this vehicle is
benchmarking process has to be performed described by the following equations, see
each time the unequal diameter occurs. It is Wang (1988):
beneficial that the error correction is done in
real time operation. Jc(t) = v(t) sin O(t) (17a)
y(t) -- v(t) sin O(t) (17b)
It is widely known that poorly designed o(t) = tO(t) (18)
mathematical model for the EKF will lead to
the divergence. Clearly, if the plant
where, v(t) and tO(t) are, respectively, the
parameters are subject to perturbations and
dynamics of the system are too complex to linear and angular velocities of the robot,
be characterized by an explicit mathematical and are expressed by:
model, an adaptive scheme is needed. Jetto,
e t al., (1999) used Fuzzy Logic Adapted v(t) (.O r (t) + tot (t) D (19)
4
Kalman Filter (FLAKF) to prevent the filter
from divergence when fusing measurement tO(t) (,t) r (t) - w l (t) D (20)
2d
from odometry and sonar sensors. In this
method, the ratio of innovation and
covariance of innovation is used as input to where D and d are the wheel diameter and
the fuzzy logic, and the output is used to the distance between the odometry encoder
weight the process noise covariance in EKF. respectively.
Sasiadek and Wang (1999) used exponential
data weighting to prevent the divergence. If, we denote the state variable of the vehicle
Mean value and covariance of innovation are a s x(t)--[x(t) y(t) 0(t)] r , and the vehicle
used as the input of the Fuzzy Logic control input as u(t) = [v(t) to(t)]r, the
Adaptive Controller (FLAC). The output is kinematic model in equations (17a) -18) can
then used to weight process noise and be written in the form of stochastic
measurement noise covariance in EKF. This differential equation as:
FLAC is implemented on the flying vehicle
navigating in three-dimensional space. Both X(t) = f ( x ( t ) , U(t)) + W(t) (21)
those methods have shown improvement in
the estimation of the vehicle position in where w(t) is a zero-mean Gaussian white
comparison with the EKF only. noise with covariance matrix Q(t), which
represents the model inaccuracies. This time-
In this paper, the systematic error in
equation is linearized and sampled in a small
odometry sensor is corrected during real-
period T =tk+ 1 - t k . Assuming that during
time operation of the vehicle by using
measurements result from the sonar sensor. this time interval, the linear and angular
EKF is applied to fuse those two signals to velocities are constant, and that the vehicle is
216 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228

following an arc path (see Wang (1988)), 3.4 Adaptive Fuzzy Logic System
then, the equations for Extended Kalman
Filter can be expressed by: In Kalman filter model, both process noise
wk and measurement noise v k are assumed
X k + 1 ---- X k + BkUk (22) zero-mean white noise sequence with
Pk-+, -- AkP#<Af + Qk (23) covariance Qk and Rk. If the model of EKF
- T
K~+I = Pi÷lCk÷l[Ck.lP~+lCk÷l+ Rk.l]-1 - T
(24) is tuned perfectly, the residual between
actual and predicted measurement should be
xk+1 = x;+1 + Kk+i[yk+1 - Ck+lXk+l] (25)
a zero-mean white noise process.
Pk+l = [I Kk+lCk+1]PtT+l
- (26)
Often, we do not k n o w all parameters of the
where: model or we want to reduce the complexity
xk =[xk Yk 0k]r (27) of modeling. Therefore, in real application,
the exact values of Qk and R k are not
T cos/0
k \ +---2---)A0t~
~ 0
known, ff the actual process and
measurement noises are not a zero-mean
n k -- T sin/0 k +-5-J
A0k ) o (28)
w h i t e noise, the residual in Kalman filter
o 1 will also not be a white noise, ff this is
happened, the Kalman filter would diverge
or at best converge to a large bound.
i 0 -vkTsinO k
Ak = 1 vkT cos0k (29)
Jetto, et al. (1999) used fuzzy logic adapted
0 1
Kalman filter to prevent the filter from
Qk=[Q1 Q2 Q3] (30) divergence. The fuzzy logic controller uses
one input and one output. The ratio between
Q1-~l-o33(T3/3)v~sinO, cosOkl (31) innovation and covariance of innovation
[-033¢212)v, sin°, ] process is used as an input. The output is a
constant, which is used to weight the process
[ -Q33 (T3/3) v2 sin 0t, cos0 k noise covariance. The controller uses five
Q2 =IQz2T+e33(T313)v~,cos2 0, (32) fuzzy rules, and it is implemented in a
L (T2/2)vk cos0k wheeled mobile robot equipped with
F-Q3 (r~/2)v k sin o k - odometry and sonar sensors.
o3 coso, (33)
Sasiadek and Wang (1999) used fuzzy logic
L o3 r adapted controller (FLAC) to prevent the
2 Qz2 = Cry,
and, Qll = Crx, 2 and Q33 = or:2 are filter from divergence when fusing signals
diagonal elements of covariance matrix Q(t) coming from INS and GPS on flying
vehicle. Nine rules were used. There were
of w(t) in Eq. (21).
two inputs, which are the mean value and
covariance of innovation, and the output is a
The measurement, in this case, will consist constant that is used to weight exponentially
of the measurement from odometry sensor
the model and measurement noise
and sonar sensor. The size of the covariance.
measurement vector depends on the number
of active sonar sensor. In general, this vector In the case of fusing signals that come from
can be expressed as (See Jetto et. al. (1999)):
odometry and sonar sensors, sometime only
odometry measurements are available. The
Y(Xk,II)=[Xk Yk Ok d~ dzk ... d~] r (34) innovation will be a white noise as long as
the process and measurement noises are
where dn, is the measurement of sonar nth at assumed as a white noise. However, when
time k. the sonar measurements become available,
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 217

and combined with the odometry

measurement, the innovation might be not a
ze~large
white noise anymore. This will cause the
filter to diverge.

0 (4) 2 1.1(4) 2 (m 2)
ii.
When systematic error occurs in the vehicle,
the process and measurement noise actually Fig.13. M F of innovation process covariance
are not a gaussian white noise, which causes [Sasiadek, J. Z. and Hartana, P. (2000)]
divergence in EKF. AFLS can be used to
adapt the filter gain so that the divergence
can be avoided. The adaptation process used z e ~ large
in this paper is based on exponential data
weighting (Lewis, 1986). The scheme of the
adaptation process is shown in Fig. 12.
0 2 20 (m)

Fig.14. MF of innovation process mean

Odometn~j~'~~'t,m~~ L_~ O~ ~ Est
t'osiimt~iatoned value [Sasiadek, J. Z. and Hartana, P.
&Sonar
measu-
I
I
I [ ]
>
r--~ C [
[ L____J ~k
-

(2000)]
romen+ &4
ze~arge I~
Xk
Fig. 12. Adaptive Fuzzy Logic System
(AFLS) scheme 1 1.02 1.1 1.2 (a)

Fig.15. MF of a [Sasiadek, J. Z. and

The membership function used in this AFLS Hartana, P. (2000)]
is displayed in Fig. 13 - Fig.15. The AFLS
uses nine rules, which are summarized in
Table 1. Table 7. Rules table for AFLS

Innovation process mean

3.4.1 Weighted EKF a value
Zero Small Large
Using exponential data weighting as an Innovation Zero Small Zero Large
adaptation process, the equation for the EKF process Small Zero Large Medium
will be different. For exponential data covarianceLarge Large Medium Zero
weighting, the weighted process and
measurement noise covariance can be If the weighted estimation covariance is
written as: defined as:

Rk = Ra-2(k+l) (35) Pff- = Pka 2k (37)

Qk = Qa-2(*+I) (36)
then the EKF equations become:
where a > 1. Q and R are constant matrices x~,1 = xk + Bkuk (38)
of process and measurement noise p~a+~. ct 2AkPka A kr +Qk (39)
covariance. For a > 1, as time k increases,
a- ~ a_ ~ Rk÷l
Qk and R k will decrease, which means that Kk+l = Pk+l C k+l [ Ck+IP +k C
1 k+l + 2 ] - 1 (40)
o~
the most recent measurement is given higher
Xk+ 1 -----Xk+ 1 4" K k + l [ y k + 1 - Ck+lXk+l] (41)
weighting.
P~+a = [I - K ~+lCk÷llP~+q (42)
218 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228

3.5 Experiments and Results The second experiment is to use the EKF to
fuse measurement signals that come from
Simulation experiments have been odometry and sonar sensor without using
conducted to show the implementation of AFLS. This experiment result is shown in
AFLS when fusing the signals that come Fig. 18. The presence of sonar sensor, which
from odometry and sonar sensor. Systematic measures the relation of the mobile vehicle
error in odometry measurement, which and its environment, reduces the systematic
comes from unequal in wheel's diameter, is error, and the mobile vehicle can follow the
also considered. The vehicle is planned to designed path. However, the movement of
follow sinus path in in-door environment. the mobile vehicle in this case is not smooth.
The map of the in-door environment along The result of sonar measurement in this
with the movement of the mobile vehicle experiment is not used efficiently to improve
that has systematic error is shown in Fig. 16. the position estimation.
Three simulation experiments have been
performed. The first experiment is to show The third experiment is to use AFLS to adapt
the implementation of EKF in the mobile the gain of EKF to prevent the filter from
robot using odometry sensor, where the divergence. In this experiment, when the
sensor has systematic error. The result of this sonar measurement becomes available, the
experiment is shown in Fig.17. In this EKF uses this signal to improve its
experiment, it shows that the implementation estimation. AFLS makes the position
of EKF with only one measurement signal is estimation smoother than without AFLS.
available, cannot be used to correct the The result of this experiment is shown in
systematic error. The EKF in this case only Fig.9.
filters the Gaussian white noise of the
odometry measurement error. However, the 3.6 Summary
systematic error is still present in the
movement of the mobile vehicle. In this chapter, Extended Kalman Filter
(EKF) has been used to estimate the position
of the mobile vehicle. To prevent the filter

.L_J from divergence, the innovation and

covariance of innovation process are
monitored by using Adaptive Fuzzy Logic
System (AFLS). The result is an adaptation
in the gain of EKF.

Odometry and sonar sensors have been used

to illustrate the method. From the simulation
experiment, it shows that beside the
improvement in the estimation of position,
the method can also be used to correct the
systematic error. Using this method, real-
A l__ I " time operation of the vehicle can be reduced.

Fig.16. Map of in-door environment

[Sasiadek, J. Z. and Hartana, P. (2000)]
 J.Z. Sasiadek /Annual Reviews in Control 26 (2002) 203-228 219

Position of the vehicle

4. SENSOR FUSION BASED ON FUZZY
KALMAN FILTER

4.1 Introduction

In this chapter, a fuzzy Kalman filter was

presented, which is based on fuzzy logic
theory and Kalman filtering. It is similar to
-2
Kalman filter when a linear system with
3
Gaussian noise is considered. However,
~4 when non-Gaussian noise is introduced, it is
I I i I L I i I I L
0 1 2 3 4 5 6 7 8 9 10 shown that fuzzy Kalman filter is
x Position (meter)
outperforming Kalman filter, while Kalman
Fig.17. Results of simulation experiment filter does not work well. It was
using EKF with only odometry measurement demonstrated the performance of Kalman
[Sasiadek, J. Z. and Hartana, P. (2000)] filter and fuzzy Kalman filter for position
P o s i t i o n of t h e v e h i c l e estimation application under different kinds
of circumstances. The comparisons are made
3
to draw conclusions.
2

1 4.2. Kalman Filter

~o
_o
= Experimental measurements are never
perfect, even with sophisticated modern
-2 instruments. The problem of estimating the
-3
state of a stochastic dynamical system from
noisy observations taken on the state is of
-4
central importance in engineering. Noise
x Position (meter) filtering is an important part of processing a
Fig. 18. Simulation experiment result using real signal sequence. There are many kinds
EKF with odometry and sonar measurement of filters could be used for estimation
[Sasiadek, J. Z. and Hartana, P. (2000)] purpose, such as mean filter, median filter,
Gaussian filter, and so on. In this article, we
discuss the performances of Kalman filter
Position of t h e vehicle
and fuzzy Kalman filter.
3

2 There are two basic processes that are

modeled by the Kalman filter. The first
1
process is a model describing how the error
state vector changes in time. This model is
the system dynamics model. The second
>,

-2
model defines the relationship between the
error state vector and any measurements
processed by the filter, and it is the
-4 measurement model. Intuitively, the Kalman
filter sorts out information and weights the
x Position (meter)
relative contributions of the measurements
Fig.19. Simulation experiment result using
and of the dynamic behavior of the state
EKF with odometry and sonar measurement,
vector. The measurements and state vector
adapted by AFLS [Sasiadek, J. Z. and
are weighted by their respective covariance
Hartana, P. (2000)]
matrices.
220 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

The Kalman filter estimates a process by

P[=EIe~e: ]=E x~-x: xk-x~ (49)
using a form of feedback control: the filter
estimates the process state at some time and
then obtains feedback in the form of (noisy)
The equations and the sequence of
measurements. As such, the equations for the
computation step are shown in Fig.20
Kalman filter fall into two groups: time
update equations and measurement update
Enter prior estimate
equations. The time update equations are ^-
responsible for projecting forward (in time) xk and its error
the current state and error covariance covariance p[
estimates to obtain the a priori estimates for
the next time step. The measurement update
equations are responsible for the feedback-
i.e. for incorporating a new measurement
l
Compute Kalman gain:
K k = pk-H~ (HkPk-H r + R k )-'
into the a priori estimate to obtain an
improved a posteriori estimate.

It was assumed the random process to be

estimated can be modeled in the form Project ahead:
1
Ptesimatei1t
measurement zg
7"--

xL =., x,
x~+1= CkXk+ wk (43)
PL. = . , , p , ~ ¢ r + O , /
The observation (measurement) of the
process is assumed to occur as discrete
points in time in accordance with the linear
relationship
Compute error covariance
for updated estimate:
P, = ( l - K k H k ) P [
/
zt, = Hkx k + v, (44)
Fig. 20. Kalman Filter Recursive
Computation Loop
The covariance matrices for the wk and vk [Sasiadek, J. Z. and Hartana, P. (2000)]
vectors are given by

E[WkWr] = ~Q~, i=k (45) It is clear that once the loop is entered, it can
[0, i~k be continued ad infinitum..
i=k (46)
E[vkvi ] = [0, i ,~ k

E[wkv r ] = 0, for all k and i (47) 4.3 Fuzzy Logic Control

Fuzzy logic control is a control method

We also assume that we know the error
^_ based on fuzzy logic. Just as fuzzy logic can
covariance matrix associated with x~. That be described simply as "computing with
is, we define the estimation error to be words rather than numbers"; fuzzy logic
control can be described simply by "control
^_
e~ = x k - xt (48) with sentences rather than equations".

The basic configuration of the fuzzy logic

and, the associated error covariance matrix is
controller is shown in Fig. 21.
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 221

In mts u(O ~z~,~--area under the membership function

Refer.
o
r(0
Z 11 Inferen(
r Center Average method

. . . . RULE- =y (51)
BASE

Outputs y(t) 4.4 Dynamic System Model

Fig: 21. Fuzzy logic Controller Architecture In this chapter a dynamic system model is
used which consists of a spacecraft
1) Rule Base accelerating with random bursts of gas from
Specifically, the fuzzy rule-base its reaction control system thrusters, the
comprises the following fuzzy if-Then vector x might consist of position P and
rules: velocity V. The dynamic equations are
IF x1is A~and ...and x, is Al~,THEN y is Bl
where ~1 and Bt are fuzzy sets in Ui C R and
1 (52)
Pk+l=Pk +VkAt+~ak At2
V C R, respectively, and X=(X~,X2,...,Xn)'~U Vk+1= Vk + akAt (53)
and y~V are the input and output
(linguistic) variables of the fuzzy system, The system equation is
respectively.
2) Inference Mechanism
The premises of all the rules are rPk+'l=[loAt'l[-Pk-Ir~2]+
:2 a, (54)
compared to the controller inputs to
determine which rules apply to the
current situation. The "matching" where ak is the random, time-varying
process involves determining the acceleration and At is the time between step
certainty that each rule applies. k and step k+l. Now suppose we can
3) Fuzzification measure the position P. Then our
The fuzzification process is the act of measurement at time k can be denoted zk =
obtaining a value of an input variable Pk + vk, where vk is random measurement
and finding the numeric values of the noise.
membership function(s) that are defined
for that variable. We assume that the process noise wk is
4) Defuzzification Gaussian noise with a covariance matrix Q.
Defuzzification operates on the implied Further assume that the measurement noise
fuzzy sets produced by the inference vk is Gaussian noise with a covariance
mechanism and combines their effects to matrixR, and that it is not correlated with
provide the "most certain" controller the process noise.
output.
The state transition matrix is:
Center of Gravity (COG) method
uC, Z,b, fl,,,, (5o) o,=L10:,l (55)
The measurement matrix is:
where
bi --center of the membership function Hk=[1 0] (56)
of the consequent of rule (/)
Process noise matrix is:
222 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

4O[ I I ~ I I I I I I
rl ] noise (57) 30 L --]---~-
I

I
I

I
~
~ . . . .
I
I I
I - - ~ I - F - ~ - ~ F- - -- T - -
I I
I

I
I

Lat J 20 I i I I ~ I I

I I I I
Measurement noise is : -- ~I
I 11 ~I Ii -- I

Vk = v~ ~ N ( 0 , R ) (58)

(59)
//2 |*processnoise* - 4 0 30 ~
=e]LAt J 0 10 20 40
~me(sec)
50 60 70 80 90 100

[[1At2 At ]* process noise Fig.23 Measured Position Trajectory

[Sasiadek, J.Z., and Khe, J., (2001)]
2 / * p r o ~ noise2

R, =E[v,v;] 15 i i i
(60) I
I
I
I
t
I
10
= measurement noise 2
.... i-i --i
The initial parameters for chosen simulation:
1. True position trajectory: xk = 10*sin(tk);
2. Standard deviation of position
i o ~5
! i ....
measurement noise: 10 m;
-10
3. Standard deviation of acceleration I I I
process noise: 0.5 m/s2; -15 - -
o
I
lo
I
20
I
3O
I
40
I
50
/
60
I
70
L
8O
I
9O
Time (see)
4. Total simulation time period: 100 sec.;
5. Time step At: 0.2 sec.; Fig. 24 Estimated Position Trajectories
[Sasiadek, J.Z., and Khe, J., (2001)]
4.5 Simulation Results

40 I I F I I I
I I J I I I

ii 30 - - T
I

I
. . . . . . . . .
I
T--q-
I
I

I
r
-i---F--S--
i I
I I

I
I I I I ~ I

I I I I

) 10 I I I I I

r~

l -10°
-30
i__m
I I r I i I
I ] ] f [ I
I I / q I I t I
'-0 10 20 30 40 50 60 70 80 90 100 -40
10 20 30 40 50 60 70 80 90 100
~rne (sec)
•me (s~)
Fig. 22 True Position Trajectory Fig. 25 Innovation [Sasiadek, J.Z., and
[Sasiadek, J.Z., and Khe, J., (2001)] Khe, J., (2001)]
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 223

x 10 4.
1 i i i i [ i i '
i I I r I I I
I I I I I I I i I I I
0.06 - - -+ . . . . . . . . . . . . . . . . . . I I I I I I I
0.8 - - L _ / _ _ / .I L _ _ L _ _ J . . . . . . .
I I I I I I I I I I
~ b i I I I I
Q i t i I I I
I I
i I I I I I I I I I 0.6 . _ l _ _ _ J _ _ _ _ - L - - J - . . . . . .
0.04 4 - - i- - 4- - - ~ - - - k- - - 4 - -I - - ~ - - q - - I I I I
I I I I
I I
I I
0.03 q . . . . . . . . . . . . . . . . . . . . . .
9 , I 0.4- - L - _ ]
I ' ' I
. . . . . . . . . . . . _ _ _ L _ _ l
I I
. . . . . .

I i
0.02 • . . . . . . . . . . . . . . . . . . . .
I i I
I I I I
I
0.2 - tlL-__-t-__- L _ _ I . . . . . .
I
0.01 - ~ . . . . . . . . . . . . . . . . .
I I I
i t I
i I I
0 ~ i i i i .... L ............................... ~ I
0 10 20 30 40 50 60 70 80 90 100
"nine (sec) "time (s~)

Fig. 26 Kalman Filter Gain [Sasiadek, J.Z., Fig. 28 Process noise covariance Q
and Khe, J., (2001)] [Sasiadek, J.Z., and Khe, J., (2001)]

From Fig. 23 to 26, we can see that Kalman

lO0
Filter works very well in spite of large I
I
I
~
T
I
I"
I
t
I
t
I
I
I
I
I
I
I

measurement noise. Kalman gain K 90! - . - - - , - - - , - - - . - _ . - - - , _ - - . - _ . - _ . _ _ _

I I ] I I I I I I
r r f f f f I ~ f
converges to a certain value and becomes R
80 -T---i--
I ~
I---T--q---I---F--T---i----
t I I I I I

stable at 0.0613 in this case. l 70 - - +

I
- -I -
I
- - I-
I
- -- 4- -- -- -I -
t I
- - I-
I
-- -- F- -- -- + -
I I
-- --I -- -- --
I
I t t I I I I I I
SC - T - - -i - - i- - - T - - q -- -- --I- -- -- I-- -- -- T - - -i - - -
i ~ $ i i
A. Process Noise Covariance Q and
Measurement Noise Covariance R _
T
i i
- - J - - - I - - - T - - I -
i i i i i i
% - - - i - - - T - - - ~ - - -
i

[13][14] --Weighted Q and R

20 - / . . . . . . i_ _ / _ _ / _ _ i_ _ _ L _ _ l _ _ _1 _ _ _
I I I I I i I I

Qk (~(Z-2(k+l)
= (61) 1 . . . . . . . . i - - - I - - - M - - - I . . . . F -- -- T -- -- --I-- -- --
I I I I I I I

Rk Ra-2(k*l) =
0
0 10 20 30
/
40
I
50
I
60
I ~
70 80
I
90
I
100
Time (sec)

where ct > 1, Q and R are constant matrices. Fig. 29. Measurement Noise Covariance R
[Sasiadek, J.Z., and Khe, J., (2001)]

0"91 ~ q I I t I I t I
I I I l I I l I I
0.8 - - J- _ _1 I_ _ _ £ _ _ J .I. I_ . . . . . I_ _ __
I I L I L I I I
I I r I I I I I

) 20 0,7 ----4-------I------I------L----~ . . . . . L----J-----d------

r~ i I I I I L I I I
I I I I I i I I

K I I I I I I I I h

I I L I I I I I I
0.4 I I ~ I I I I I

I ° -10 R
0.3
I
I I
I
I
I
I
I
I I
I
I
i I I
t
I
I
- - - T . . . . . . , - - - , - - , - - - , - - - , - - , - - - , - - - -
1 I I I t I I I I
.201 0.2 I I I i I I I I I
---T- -I---I---T--~---I---F--T---i----
I I I ) I I I I
-30 0.1 I I I I I I I i }
-- 7---~---, . . . . . . . . . . . . F--T---i---
q r I I I I
-40 o
10 20 30 40 50 60 70 80 90 1O0 10 20 30 40 50 60 70 80 90 100
.time (sec) Time (sec)

Fig. 27 Estimated Position Trajectory Fig. 30 Kalman Gain K [Sasiadek, J.Z., and
[Sasiadek, J.Z., and Khe, J., (2001)] Khe, J., (2001)]
224 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

With the decreasing of Q and R, Kalman

gain K becomes diverging and cannot reach . . _ _ _ . _ _ - , _ _ - - . - - - - . - - - - - - . - - - - . - -1- - - ;-

a stable value. The estimated position

trajectory (Fig. 27) becomes more and more I I I f I I I I I

inaccurate too.
-,o I 4
B. Correlated Process Noise and _2O I t l _. t I

Measurement Noise ~o-- ~---C--T--]---7 I I I I I t I

t I f I I I V I I
-.50 I I I I t I t t I
10 2O 3O 40 5O 60 70 8O 100
Let this correlation be described by "lime (tle¢)

Fig. 31 Estimated position [Sasiadek, J.Z.,

E[w'vr] ={ C~ i#k
i=k (62) and Khe, J., (200i)]

X 10 ~

A generalized derivation yields the optimal I I T I I I I

1.
estimation algorithm with the same initial 1,6 -- , . . . . ~ --," . . . . , -~- ~ . . . . .
conditions and measurement update 1.4 ~ - ,~- --,--I- ~. . . . .
relations: 1.2 . . . . ~-~ . . . . ~. . . . .
I I f I
1
. . . . ] F- - - F - q . . . .

^- ^ - ,-1( ^-) [t t 1 t t I

z -rcx (63)
0.8 " -- F - - q - - - F - - - F - ] - -

0.6

0.4i
0.2i ,__,,,_,_,___,__,_,_,__,___,__, ....
I V I I I I I t I I
. . ..... , , , , ,
+ Q , - q (n P;n: + (64)
0 10 20 30 40
Time
~
($oc)
60 70 80 gO 1 O0

Fig. 32 Kalman Filter Gain [Sasiadek, J.Z.,

and Khe, J., (2001)]

(65) is capable of dealing with systems with

random disturbances and "uncertainty". We
LAt J will refer to this as a fuzzy Kalman filter,
and it is a fuzzy system model of the process
This can be compared to the case of no in the estimator. This filter is similar to that
correlation between wk and vk. The decrease of the Kalman filter when a linear system
in steady state values from with Gaussian noise is considered. Let the
-0.0353 0.0172] inputs be ek,ek,%,,ep,, and the output is the
=[6.5286 1.0321] to [0.0172
P[ [_1.0321 0.3213J t_ 0.0084J estimated position at time step k+l, where e~
=[6.1285 0.9689] tof0.0287 0.0158] is the error between measured position value
and p~ [0.9689 0.3113_1 [_0.0158 0.0087J and true position value, ek is the change in
ek, ep~is the difference between Pk and P[, and
is due to the exploitation of the correlation
between the dynamic noise and the noise ep, is the change in %. Here we use a MISO
that corrupts the observable outputs: the zk (Multiple Inputs Single Output) system to
realizations reveal more about the noise accomplish the task. The rules being used
process wk. are such as
Rule 1: IF ek is negative large, ek is zero,
4.6 Fuzzy Kalman Filter % is negative large, and eP, is negative
large, THEN estimated position at step
Now we introduce an "optimal" state
k+l is positive large.
estimator, based on fuzzy set theory, which
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 225

Rule 2: IF e, is zero, e, is zero, % is

X 10 ~ Error b e t w e e n p and Estim~ed P

negative large, and e~, is negative °r~.

1
i t [ ~ I I I ~ I -|
- - ,i - - , i - - , t- - , -i - , - t- , - i- , - -I , - -I, - - I
large, THEN estimated position at
step k+l is positive large. ----.----.----.----.----.----.----.----.----.----

Rule 3: IF e~ is positive large, e, is zero, O lp 20 30 40 50 60 70 80 90 100

% is zero, and e~, is zero, THEN

0 . . . . L I I ~ 1 i I 1
estimated position at step k+l is t
1
i
i 1
i I
1 I I
i
i ~
I r
t
~ I

negative large. -5 - , - - , - - , - - , - - , - - , - - , - - , - - , - -

Rule 4: IF e~ is zero, e~ is positive small, -15 I I ~ ~ I I I

0 10 2S 30 40 50 ~0 70 80 90 100

er~ is negative small, and e~ is

"~lme (sac)

negative small, THEN estimated Fig. 35 ep,and epk[ Sasiadek, J.Z., and Khe,
position at step k+l is negative
small. J., (2001)1
The results are plotted in Fig. 33 to Fig. 36.

Compared with Fig. 24 which is obtained by A. Process Noise Covariance Q and

using Kalman filter, it is more accurate. Measurement Noise Covariance R - -
Weighted Q and R (as in Kalman filter)

15
J I I I t
I I
I r t
_ I __ i I I _ t I I I I
10

........./lili= II
-- T~ - ~ - 5r - F,f - - T~ -~---~
I I 1 I I i
5 - , . . . . . . . . . . .
I 5

i 0

-5 ' I °
-I- I

-10
-10 l --U--1 r
q ~ i i i
0 10 20 30 4o 510 6o 70 8o 9o lOS
~i3 rne ( s ~ ) -~ 0 1 'o '
20 ~'o --~--~
40 50 60 ' " ~ 9 0~ - 100
33me (see)

Fig. 33 Estimated Position [Sasiadek, J.Z.,

and Khe, J., (2001)] Fig. 36 Estimated Trajectory [Sasiadek, J.Z.,
and Khe, J., (2001)]

Error between Measured Position and True Position

Error between Measured Position and True Position
40(--- I I I I I I f i I P
i 1 t i i i / i t
20 - J- - J - - _t . . . . I _ L _ .3 _

-20
-40
0 10 20 30 4o 5o 6o 70 80 90 too -40
"liroe (pec) 0 tO 20 30 40 50 60 70 80 90 100
Change in aoo~ error
" l i m e (~ec)
Change in aoo'~ error
I I t i i l f I I
41 I l I I T d I I 0 I t --]
I t I i I I I I
] I I I I I I I i
2 . . . . I - F -

-2

-4
-5 [ t i I h I L I
0 10 2o 30 4o 5o 6o 70 8o 9o 100
-6
" t i m e (see) 0 10 20 30 40 50 60 70 80 90 100
lime (see)

Fig. 34 e, and ek [Sasiadek, J.Z., and Khe, J.,

Fig. 37 ee and e~ [Sasiadek, J.Z., and Khe, J.,
(2001)1
(2001),
226 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

x 10 ~ Error between P and Estimated P x t0 s Error between P and Estimated P

i
k
-0.5 . . . . . . I- - - i . . . . . . . . I - - ~- - - 5 - - -J - - --

I ~ i I I I

• -1.. . . . . ]11-1 - - IIII. . . . . . . IIII- - - Itlr-- - fi-itl- - -11~1- - -

-1 t_ 1 i i i i l I 1 I
0 10 20 30 40 50 60 70 80 90 100 0 10 20 30 40 50 60 70 80 90 100
l i m e (@ec) "time(~ec)
x t 0 -~ Change m aDo~e error X 10 "7 Change In BDO~ error
1 i i J , i i ~ , i
i I ~ i i i i
, ,~ ~ . . . . 05 I I I I I I

I I I i I I ~
° -- -i ~ -- ,~ --

' i -i- I : . . . . - 0 . 5 ~

| I I ~ t I I I I ]
-1 L~ i i I I F I I t t
0 10 20 30 40 50 60 70 80 90 100 0 10 20 30 40 50 60 70 80 90 100
11me (see) "time (sec)

Fig. 38 % and ePk [Sasiadek, J.Z., and Khe, Fig. 41 ep~and ep k [ Sasiadek, J.Z., and Khe,
J., (2OOl)] J., (2001)]

B. Correlated Process Noise and 4. 7 Summary

Measurement Noise (as in Kalman filter)
The method that is the most widely used
15 I I J I I
for sensor fusion in engineering applications
I r I I I
r I ¢ x is the Kalman filter. This filter is often used
to combine all measurement data (e.g., for
fusing data from different sensors) to get an

I 0 10
,i,, !t,- itfi=
20
....

30 40 50 60 70
I
80
I
90 1~
optimal estimate in a statistical sense, ff the
system can be described with a linear model
and both the system error and the sensor
error can be modeled as Gaussian noise, then
the Kalman filter will provide a unique
statistically optimal estimate for the fused
Time (sec) data. This means that under certain
conditions the Kalman filter is able to find
Fig. 39 Estimated Position [Sasiadek,J.Z., the best estimates based on the "correctness"
and Khe, J., (2001)] of each individual measurement. On the
basis of simulation performed in this chapter
for fuzzy Kalman filter and regular Kalman
Error between Measured Position and True Position
401- I I r I I I I I I I filter under different conditions and
20 - J. - -I- - I - - ~ - I- - - ~- - - - parameters, it was shown that for linear
0 ~ I I I I
systems and triangular shaped membership
-20
functions, the fuzzy Kalman filter works
similarly as Kalman filter but produces
-40
0 10 20 30 40 50
•me (~ec)
60 70 80 90 100 better results than the Kalman filter. The
Change in El~o'~e error
general comparison of fuzzy Kalman Filter
/
I i i I I i I I l and regular Kalman Filter was presented. It
can be proved that the convergence of
Kalman Filter's estimate to the true value is
guaranteed only when the system is
0 10 20 30 40 50 60 70 80 90 100 stochastically controlled and observed.
"time (sec)

Fig. 40 e k and ek [Sasiadek, J.Z.,and Khe, J,

(2001)]
 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228 227

5. REFERENCES SYROCO 1997, 219-229, Nantes, France,

1997
Abdelnour, G., Chand, S., Chiu, S., and Jetto, L., S. Longhi, & D. Vitali (1999).
Kido T., (1993)," On-line Detection & Localization of a wheeled mobile robot
Correction of Kalman Filter Divergence by by sensor data fusion based on a fuzzy
Fuzzy Logic ", 1993 American Control logic adapted Kalman filter. Control
Conf., pp 1835 - 1839. Engineering Practice 7, 763-771.
Abidi, Mongi, A. and Gonzalez, Rafael C., Jetto, L., S. Longhi, & G. Venturini (1999).
(1992), "Data Fusion in Robotics and Development and experimental validation
Machine Intelligence ", Academic Press, of an adaptive extended Kalman filter for
Inc., 1992. the localization of mobile robots. IEEE
Babuska, Robert, (1998), "Fuzzy Modeling Transactions on Robotics and Automation
for Control", Kluwer Academic 15(2), 219-229.
Publishers, 1998 Jochen, P., Meyer-Hilberg, W., and Thomas,
Borenstein, J., & L. Feng .(1996). Jacob (1994) "High Accuracy
Measurement and correction of Navigation and Landing System using
systematic odometry errors in mobile GPS/IMU System Integration",
robots. IEEE Transactions on Robotics 1994IEEE Position Location and
and Automation 12(6), 869-879. Navigation Symposium, pp.298 - 305.
Borenstein, Johann; Everett, H. R.; Feng, Kalman, R. E., "New Methods in Wiener
Liqiang, (1996), "Navigating Mobile Filtering Theory", Proc. Symp. Eng.
Robots", A. K. Peters, Ltd., 1996 Applications of Random Function
Brown, Robert Grover, and Hwang, Patrick Theory and Probability, John Wiley,
Y. C.,(1992), "Introduction to Random New York, 1963
S~gdnals and Applied Kalman Filtering", Kayton, M. and Fried, W. R., "Avionics
2 edition, John Wiley & Sons, Inc., Navigation Systems ", John Wiley &
1992 Sons, Inc. 1997.
Cooper, S. and Durrant-Whyte H., (1994), Kosko, Bart, "Neural Networks And Fuzzy
"A Kalman Filter Model for GPS Systems", A Dynamical Systems
Navigation of Land Vehicles ", 1994 Approach to Machine Intelligence, Prentice-
IEEE Int. Conf. on Intelligent Robot Hall, Inc., 1992
and Systems, pp. 157- 163. Layne J.R., Passino K.M., (2001) "A Fuzzy
Chand, S., and Hansen, A. (1989), "Energy Dynamic Mode Based State Estimator",
Based Stability Analysis Of A Fuzzy Fuzzy Sets and Systems,
Roll Controller Design For A Flexible Lewis, F. L., (1986), " Optimal Estimation
Aircraft Wing", Proceedings of 1989 with an Introduction to Stochastic
IEEE Conference on Decision and Control Theory ", John Wiley & Sons,
Control, pp.705 -709, Tampa, Florida, Inc., 1986.
December 1989 Mamdani, E. "Twenty years of fuzzy
Cumani, A., (1982) "On a Possibilistic control: experiences gained and lessons
Approach To The Analysis Of Fuzzy learnt." Proceedings of the Second IEEE
Feedback Systems", IEEE Transactions on International Conference on Fuzzy
Systems, Man, and Cybernetics vol.12, Systems, 1993, pp.339-344
pp.417 -422,May/June 1982 Maybeck, Peter S., (1979), "Stochastic
Gelb, A., (1974), " Applied Optimal Model, Estimation and Control", Vol.1-
Estimation ", The MIT Press, 1974 3, Academic Press, Inc. 1979.
Jetto, L., S. Longhi, & G. Venturini (1997). McKerrow, Phillip John (1991)
Development and experimental validation "Introduction to Robotics." Addison-
of an adaptive estimation algorithm for Wesley Publishing Company, Sydney
the on-line localization of mobile robots
by multisensor fusion. Preprints of the
Fifth IFAC Symposium on Robot Control,
228 J.Z. Sasiadek / Annual Reviews in Control 26 (2002) 203-228

Passino, Kevin M., and Yurkovich, Stephen, Sasiadek, J.Z., Wang, Q., and Zaremba,
(1998), "Fuzzy Control", Addison M.B.,(2000) "INS/GPS Navigation Data
Wesley Longrnan, Inc., 1998 Fusion Using Fuzzy Adaptive Kalman
Roumeliotis, S. I., G. S. Sukhatme, & G. A. Filtering", pp. 543-547, Proc. of IFAC,
Bekey (1999) "Circumventing Dynamic Symposium on Robot Control (SYROCO
Modeling: Evaluation of the Error-State '00), Sept. 21-23, 2000 Vienna, Austria
Kalman Filter applied to Mobile Robot Sasiadek, J, and Wang, Q., (1999), "Sensor
Localization." Proceeding of the 1999 Fusion Based on Fuzzy Kalman Filtering
IEEE International Conference on for Autonomous Robot Vehicle", Proc.
Robotic & Automation, 1656-1663. IEEE Int. Conf. On Robotics and
Santini, A., Nicosia, S., Nanni, V., (1997). Automation, Detroit, Michigan, 1999
Trajectory estimation and correction for a Sasiadek, J. and Wang, Q., "3-D Guidance
wheeled mobile robot using and Navigation of Mobile and Flying
heterogeneous sensors and Kalman filter. Robot using Fuzzy Logic", 1998 AIAA
Preprints of the Fifth IFAC Symposium Guidance, Navigation and Control
on Robot Contro-SYROCO 1997, 11-16, Conference, Boston, MA, August 1998,
Nantes, France, 1997. Paper No. AIAA-98-4126.Shoval, S., A.
Sasiadek, J. Z., and Wang, Q. (2001), Fuzzy Mishan, & J. Dayan (1998), " Odometry
Adaptive Kalman Filtering for INS/GPS and triangulation data fusion for
Data Fusion and Accurate Positioning, mobile-robots environment
Proceeding of the 2001 IFAC recognition", Control Engineering
International Symposium on Practice 6, 1383-1388.
Aerospace Control, 451- 459, Bologna, Tham, Y. K., H. Wang, & E. K. Teoh
Italy, 2001 (1999), "Multi-sensor fusion for steerable
Sasiadek, J.Z., and Khe, J., (2001), Sensor four-wheeled industrial vehicles",
Fusion based on Fuzzy Logic Kalman Control Engineering Practice 7, 1233-
Filter, Proceedings of the IEEE 1248.
International Conference on Robotics, Wang, C. M. (1988), "Location estimation
Modeling, and Control (RoMoCo'2001), and uncertainty analysis for mobile
230-239 robots", Proceeding of the IEEE
Sasiadek, J. Z. and Hartana, P. (2000), International Conference on Robotics and
Odometry and Sonar Data Fusion for Automation, 1230-1235.
Mobile Robot Navigation. Proceeding of
the 6 th IFAC Symposium on Robot
Control - SYROCO 2000, 531-536,
Vienna, Austria, 2000