Sensor Fusion Based on Fuzzy Kalman Filtering for Autonomous Robot Vehicle

8/14/2019 Sensor Fusion Based on Fuzzy Kalman Filtering for Autonomous Robot Vehicle

1/6

Proceedingsof the 1999 IEEEInternational Con ference on Ro botics & AutomationDetroit, Michigan May 1999Sensor Fusion Based on Fuzzy Kalman Filtering forAutonomous Robot Vehicle

J.Z. Sasiadekand Q.WangDept. of Mechanical& Aerospace EngineeringCarleton University1125Colonel By Drive, Ottawa, Ontario, KlS 5B6, Canada

AbstractThispaper presents the m e w f sensor fusionbased on the Adaptive Fuzzy Kalman Filtering.This method has been applied to fuse positionsignals from the Global Positioning System(GPS) and Inertial Navigation System (INS) forthe autonomous mobile vehicles. The presentedmethod has been validated in 3-0 environmentand is of particular importance for guidance,navigation, and control of flying vehicles. TheExten&d Kalma n Filter (EKF) and the noisecharacteristic hasbeen modifwd using theFuzzyLogic Adaptive System and compared with theperformance of egular E m .It has been demonstrated that the FuzzyAdaptive Kalman Filter gives better results(more accurate) than he EKF.

IntroductionWhen navigating and guiding an autonomousvehicle, the position and velocity of the vehiclemust be determined. The Global PositioningSystem (GPS) is a satellite-based navigationsystem that provides a user with the properequipment access to useful and accuratepositioning information anywhere on the globe[l]. However, several errors are associated withthe GP S measurement. It has superior long-termerror performance, but poor short-term accuracy.For many vehicle navigation systems, GPS isinsufficient as a stand-alone position system. Theintegration of GPS and Inertial NavigationSystem (INS) is ideal for vehicle navigationsystems. In generally, the sho rt-term accuracy ofIN S is good; the long-term accuracy is poor. Thedisadvantages of G PSDNS are ideally cancelled.If the signal of GPS is interrupted, the INS

enables the navigation system to coast alonguntil GPS signal is reestablished [l]. Therequirements for accuracy, availability androbustness are therefore achieved.Kalman filtering is a form of optimalestimation characterized b y recursive evaluation,and an internal model of the dynamics of thesystem being estimated. The dynamic weightingof incoming evidence with ongoing expectationproduces estimates of the state of the observedsystem [2]. An extended Kalman filter (EKF)ca n be used to fuse measurements from GPS andINS. n this EKF, th e INS data are used as areference trajectory, and GPS data a re applied toupdate and estimate the error states of thistrajectory. The Kalman filter requires that all theplant dynamics and noise processes are exactlyknown and the noise processes are zero meanwhite noise. If the theoretical beh avior of a filteran d it s actual behavior do not agree, divergenceproblems will occur. There are two kinds ofdivergence: Apparent divergence and Truedivergence [3][4]. In the apparent divergence,the actual estimate error covariance remainsbounded, but it approaches a larger bound thandoes predicted error covariance. In truedivergence, the actual estimation covarianceeventually becomes infinite. The divergence dueto modeling errors is critical in K a h n filterapplication. If, the Kalman filter is fedinformation that the process behaved one way,whereas, in fact, it behaves another way, thefilter will try to continually fit a wro ng process.When the measurement situation does notprovide enough information to estimate all thestate variables of the system, in other words, thecomputed estimation error matrix becomesunrealistically small, and the filter disregards themeasurement, then the problem is particularlysevere. Thus, in order to solve the divergencedue to modeling errors, we can estimateunmodeled states, but it add complexity to the

0-7803-5180-0-5/99$10.00 0 1999 IEEE 2970


2/6

filter and one can never be sure that al l of thesuspected unstable states are indeed modelstates[3]. Another possibility is to add processnoise. It makes sure that the Kalman filter isdriven by white noise, and prevents the filterfrom disregarding new measurement. In thispaper, a fuzzy logic adaptive system (FLAS) isused to prevent th e Kalman filter fromdivergence. The fuzzy logic adaptive controller(FLAC) will continually adjust the noisestrengths in the filter's internal model, and tunethe filter a s well as possible. The FLACperformance is evaluated by simulation of thefuzzy adaptive extended Kalman filteringscheme of Fig.1.

Estimated INS errors

EKFseudo-range -Predicted measurements

Fig.1. Fuzzy adaptive extended K alman filter

Weighted EKFBecause the processes of both GPS and INSare nonlinear, a linearization is necessary. Anextended Kalman filter is used to fuse th emeasurem ents from the GPS and INS. Toprevent divergence by keeping the filter fromdiscounting measurements for large k, theexponential data w eighting [4 ] is used.The models and implementation equations forthe weighted ex tended Kalma n filter are:

Nonlinear dynamic m odel' k + l a ( ' k 9 k, + w k (1)

Nonlinear m easurement m odel

Let us set the model covariance matrices equal to(3)(4)

R , I a - 2 ( k " )Q k Q a - 2 ( k + l )

where, ail, nd constant matrices Q an d R. Fo ra>l, s time k increases, the R nd Q decrease,so that the most recent measurement is givenhigher weighting. If a=l,t is a regular EKF.By defining the weighted covarianceP = PiaZk (5 )

The Kalman gain ca n be computed:K, = PlH:(H,PiHT + Ra-*("'))-'

The predicted state estimate is:ki+l f ( ' k 9k ) (7)The p redicted measurement is:f k = h ( % ; , k ) (8)The linear approximation equations can bepresented in form:

(9)The predicted estimate on the measurement canbe computed:

Computing the a priori covariance matrix:Pi+'=


3/6

Computing the U posteriori covariance matrixgives:

P = ( I - K,H,)P,"- (14)The initial condition is:p,"- = P o

In equation (lo), th e term a-& s calledresiduals or innovations. It reflects the degree towhich the m odel fits the data.

INS and GPSThe inertial navigation system (INS) consists

of a sensor package, which includesaccelerometers and gyros to measureaccelerations and angular rates. By using thesesignals as input, the attitude angle and three-dimensional vectors of velocity and position arecomputed [SI. he errors in the m easurements offorce made by the accelerometers and the errorsin the measurement of angular change inorientation with respect to inertial space made bygyroscopes are two fundamental error sources,which affect the error behavior of an inertialsystem. The inertial system error response,related to position, velocity, and orientation isdivergent with time due to noise input [6]. Thereare biases associated with the accelerom eters andgyros. In order to correct the errors of INS, theGPS measurements are used to estimate theinertial system errors, subtract them from theINS outputs, and then obtain the corrected INSoutputs. There is number of errors in GPS, suchas ephemeris errors, p ropagation errors, selectiveavailability, multi-path, and receiver noise, etc.By using differential GPS (DGPS), most of theerrors can be corrected, but the multi-path andreceiver noise cannot be eliminated.

Fuzzy Logic Adaptive SystemIt is assumed that both, the process noise wkand the measurement noise vk are zero-meanwhite sequen ces with known covariance Q an d Rin the Kalman filter. If the Kalman filter is basedon a complete and perfectly tuned model, theresiduals should be a zero-mean white noiseprocess. If the residuals are not white noise, there

is something wrong with the design and the filteris not performing optimally [4]. The Kalmanfilters will diverge o r coverage to a large bound.In practice, it is dific ult to know the exact valuefo r Q an d R. In order to reduce compu tation, wehave to ignore some errors, but sometimes thoseunmodeled errors will become significant. Theseare the instrument bias errors of INS. Generallyth e Wk does not always have zero mean. In thosecases, the residuals can be used to adapt thefilter. In fact, the residuals are the differencesbetween actual measurements and bestmeasurement predictions based on the filter'sinternal model. A well-tuned filter is that whereth e 95% of the autocorrelation function ofinnovation series should fall within the 2 2aboundary [7]. f the filter diverges, the residualswill not be zero mean and becom e larger.

There ar e very few papers on application offuzzy logic to adapt the Kalman filter. In (81,fuzzy logic is used to the on-line detection andcorrection of divergence in a single state Kalmanfilter. There were three inpu ts and two outputs tofuzzy logic controller (FLC), and 24 NI = wereused. The purpose of our fuzzy logic adaptivesystem (FLAS) is to detect the bias ofmeasurements and prevent divergence of th eextended Kalman filter. It has been applied inthree axes- ast (x), North (y), and Altitude(z). Th e covariance of the residuals and th e meanof residuals are used as the inputs to FLAS fo rall three fuzzy inference engines. Theexponential weighting a or three axes are theoutputs. As an input to FLAS, he covariance ofthe residuals and mean values of residuals areused to decide the degree of divergence. Thevalue of covariances relates to R.The equationfor covariance of the residual is:

P, = H,P;Hi + RWhen the Kaliilan filter is performingoptimally, the mean values of residuals are nearzero. Generally, when the covariance isbecoming large, and mean value is moving awayfrom zero, th e K a h n filter is becoming

unstable. In this case, a large a will be applied.A large a means that process noises are added. Itcan ensure that in the model a l l states aresufficiently excited by the process noise. Whenthe covariance is extremely large, there are someproblems with the GPS measurements, so th efilter cannot depend on these measurementsanymore, and a smalla will be used. The perfectmeasurements are given more weighting. By

2972


4/6

selecting appropriate a,he FLASwill adapt theKalman filter optimally and try to keep theinnovation sequence acting a s zero-mean whitenoise.Table. 1. Rule Table for FLAS

I I Mean Value Ismall large

Fig.2. Covariance Mem bership Functions

large

S --- Small;L -- Large; M -- Medium;Z --- Zero;

SimulationM A W odes developed by authors ha sbeen used to simulate and test the proposedmethod.Fig.3. Mean Value Membe rship Functions

Th e state variables used in simulation are:$m small meciium large

1 1.02 1.1 1. 2 aFig.4. a Membership Functions

The FLAS uses 9 rules, such as:If the covariance of residuals is large and themean values are zero Then a is large.If the covariance of residuals is zero and themean values are large Thena s zero.The fuzzy adaptive Kalman filtering has beenused for guidance and navigation of mobilerobots, especially for 3-D environment. Thenavigation of flying robots requires fast, on-linecontrol algorithms. The regular ExtendedKalman Filter requires high number of states foraccurate navigation and positioning. The FLACrequires smaller number of states for the sameaccuracy and therefore it would need lesscomputational effort. Alternatively, the samenumber of states (as in regular filter) wouldallow for more accurate navigation.

The states are position, and velocity errors of th eIN S East, North, Altitude, GPS range bias andrange drift. Th e covariance of GPS measurementR is 5 [m]. It is assumed that the measurementsof INS have some biases. In the first simulation(Fig. 5) , the mean values of INS are 0.0014meter, 0.00035 meter, and 0.0007 meter for theEast (x), North (y), and Altitude (z) respectively.A white noise with a standard deviation of 3meter is added to GP S measurements. Ibesamp le period is 1 second. The first row in Fig. 5is the innovations of fuzzy adaptive EKF andEKF in East (x). The innovation of EKF had alarge drift, and w as stable a t a high mean value.The fuzzy adaptive EKF clearly improved th eperformance of EKF, and the mean value wasmuch smaller than that of EKF. Other figurespresent the corrected position (first column) an dvelocity (second column) errors. The correctederror is the current INS error minus estimatedIN S error. The dashed l ines are the correctederrors of EKF, and the solid lines are thecorrected errors of fuzzy adaptive EKF. Thefuzzy adaptive EKF significantly reduced thecorrected position and velocity errors. In th e

2973


5/6

second simulation (Fig. 6), the samemeasurements as in the first simulation for INSwere used. A white noise with a standarddeviation of 2 meter from 0 s to loo0 s an d 1500s to 2oooS was applied to GPS measurements.From loo0 s to 1500 s, the standard deviation of6 meter with mean value of 6 meter was added toGPS measurements. Although, the GPSmeasurement noises features were changed, thefuzzy adaptive E KF still worked well. Thosesimulations also showed that the corrected errorsof EKFwere proportional to the mean values ofINS measurements. In other word, the moreerrors are not modeled, the worse the EK Fperforms.

ConclusionsIn this paper, a fuzzy adaptive extendedKalman filter has been developed to detect andprevent the EKF from divergence. Bymonitoring the innovations sequence, the FLAScan evaluate the performance of an EKF. If thefilter does not perform well, it would apply an

appropriate weighting factor a o improve theaccuracy of an EKF.The simulation results show that the FLASsignificantly reduces the corrected position andvelocity errors when the EKF results diverge. InFLM, there are 9 rules and therefore, littlecomputational time is needed. It can be used tonavigate and guide autonomous vehicles orrobots [9] and achieved a relatively accurateperformance. Also, the FLAS ca n use lowerord er state-model without comprom isingaccuracy significantly. Another words, for anygiven accuracy, the fuzzy adap tive Kalman filtermay be able to use a lower order state model.T h e FLAS makes the necessary trade-offbetween accuracy and computational burden d ueto the increased dimension of the error statevector and associated matrices.

References

[2] Abidi, Mongi A. and Gonzalez, Rafael C. Data Fusion in Robotics and MachineIntelligence ,Academic Press, nc., 1992.[3] Gelb, A., Applied Optimal Estimation ,The MIT ress, 1974.[4] Lewis, F. L., Optimal Estimation with anIntroduction to Stochastic Control Theory ,John W iley & Sons, Inc., 1986.[5] Jochen, P., Meyer-Hilberg, W., andIbomas, Jacob. High Accuracy Navigationand Landing System using GPSAMUSystem Integration, 1994 IEEE PositionLocation and Navigation Symposium,pp.298 - 305.[6] Kayton ,M. and Fried, W. R., AvionicsNavigation Systems , John Wiley & Sons,Inc. 1997.[7] Cooper, S . and Durrant-Whyte H., AKalman Filter Model for GPS Navigation ofLand Vehicles , 1994 IEEE Int. Conf. onIntelligent Robot and Systems, pp. 157 -163.[8] AWelnour, G., Chand, S., Chiu, S., and KidoT., On-line Detection & Correction ofKalman Filter Divergence by Fuzzy Logic

,1993 American Control Conf., pp 1835 -1839.

[9] Sasiadek, J. and Wang, Q., 3-D Guidanceand N avigation of Mobile and Flying Robotusing Fuzzy Logic, 1998 AIAA Guidance,Navigation and Control Conference, Boston,MA, August 1998, Paper No. AIAA-98-4126.

[ l ] Brown, R. G. and Hw ang, P.Y.C. Introduction to Random Signals and AppliedKalman Filtering , John Wiley & Sons,Inc., 1992.

2974


6/6

E K F50v I I I

0 I I Ic I I I- -50

0 .5 0 5 0 0 1000 1500 2 0 0 0I I

I I I

Fuzzy adaptive E K F2 0

G Ov I I IC0._

I I I

I I I1 J

0C

Fig.5. Simulation 1

I I II I II

Fuzzy adaptive E K F

0C

hc 2 0 ,

I I II I I

L -vC0._U2 0

-...,>y---"*-----E 0 ,-zi 1 I II I I

-10 I I I I, t I J0 50 0 1000 1500 2000Tim e s )

E KFhs 5 0 ,

"0 500 1000 1500 2000Tim e s )

Fig.6. Simulation 2

2975

Documents

Sensor Fusion Based on Fuzzy Kalman Filtering for Autonomous Robot Vehicle