SENSOR FUSION. J.Z. Sasiadek Department of Mechanical & Aerospace Engineering, Carleton University

Transcription

1 ' Annual Revews n Control PERGAMON Annual Revews n Control 26 (2002) SENSOR FUSON J.Z. Sasadek Department of Mechancal & Aerospace Engneerng, Carleton Unversty e-mal: jsas@mae.carleton.ca Ths paper was orgrtally prepared as an FAC Professonal Bref Abstract. Sensor fuson s a method of ntegratng sgnals from multple sources. t allows extractng nformaton from several dfferent sources to ntegrate them nto sngle sgnal or nformaton. n many cases sources of nformaton are sensors or other devces that allow for percepton or measurement of changng envronment. nformaton receved from multple-sensors s processed usng "sensor fuson" or "data fuson" algorthms. These algorthms can be classfed nto three dfferent groups. Frst, fuson based on probablstc models, second, fuson based on least-squares technques and thrd, ntellgent fuson. The probablstc model methods are Bayesan reasonng, evdence theory, robust statstcs, recursve operators. The least-squares technques are Kalman flterng, optmal theory, regularzaton and uncertanty ellpsods. The ntellgent fuson methods are fuzzy logc, neural networks and genetc algorthms. Ths paper wll present three dfferent methods of ntellgent nformaton fuson for dfferent engneerng applcatons. Chapter 2 s based on Sasadek and Wang (2001) paper and presents an applcaton of adaptve Kalman flterng to the problem of nformaton fuson for gudance, navgaton, and control. Chapter 3 s based on Sasadek and Hartana (2000) and Chapter 4 on Sasadek and Khe (2001) paper. Keywords. Sensor fuson, probablstc models, least squares technques, fuzzy logc, neural networks, genetc algorthms. 1. NTRODUCTON Sensor fuson s a method of ntegratng sgnals from multple sources. t allows extractng nformaton from several dfferent sources to ntegrate them nto sngle sgnal or nformaton. n many cases sources of nformaton are sensors or other devces that allow for percepton or measurement of changng envronment. nformaton receved from multple-sensors s processed usng "sensor fuson" or "data fuson" algorthms. These algorthms can be classfed nto three dfferent groups. Frst, fuson based on probablstc models, second, fuson based on least-squares technques and thrd, ntellgent fuson. The probablstc model methods are Bayesan reasonng, evdence theory, robust statstcs, recursve operators. The least-squares technques are Kalman flterng, optmal theory, regularzaton and uncertanty ellpsods. The ntellgent fuson methods are fuzzy logc, neural networks and genetc algorthms. Ths paper wll present three dfferent methods of ntellgent nformaton fuson for dfferent engneerng /02/$ Publshed by Elsever Scence Ltd. P: S (02)

2 204 J.Z. Sasadek /Annual Revews n Control 26 (2002) applcatons. Chapter 2 s based on Sasadek and Wang (2001) paper and presents an applcaton of adaptve Kalman flterng to the problem of nformaton fuson for gudance, navgaton, and control. Chapter 3 s based on Sasadek and Hartana (2000) and Chapter 4 on Sasadek and Khe (2001) paper. The data/sensor problems and related methods are used n conjuncton to many engneerng applcatons. For example, gudance, navgaton, and control of vehcles requre large number of nformaton from dfferent sources. Ths nformaton often s smlar and has to be ntegrated nto one meanngful sgnal or nformaton that can be used n control systems. n ths paper the sensor/data fuson wll be shown for three dfferent cases. n all three cases the Kalman Flter method s used to ntegrate sgnals/nformaton receved from multplesensor sources. Also, n all three cases the modfcaton of Kalman Flter method s ntroduced to mprove performance. Ths modfcaton s based on Fuzzy Logc System (FLS). n that sense, the ntegraton method becomes an ntellgent ntegraton, and s applcable to broader number of ndustral cases. The chapter 2 s presentng an ntegraton of data/sensor sgnals receved from the Global Postonng System (GPS) and nertal Navgaton System (NS). The ntegraton allows for better and more accurate postonng. Chapter 3 presents the navgaton of an autonomous robot based on sensor/data fuson method for sgnals receved from sonar and odometry sensors. The fuson process allows for more effcent navgaton and obstacle avodance. n both cases descrbed n chapter 2 and 3 the Kalman Flter method s backed up by the Fuzzy Logc System (FLS). Fnally, chapter 4 s presentng an attempt to desgn ntegraton method based fully on FLS. Results and conclusons are shown separately for those three dfferent cases. 2. FUZZY ADAPTVE KALMAN FLTERNG FOR NS/GPS DATA FUSON AND ACCURATE POSTONNG 2.1 ntroducton Ths chapter presents the method of sensor fuson based on the Adaptve Fuzzy Kalman Flterng. Ths method has been appled to fuse poston sgnals from the Global Postonng System (GPS) and nertal Navgaton System (NS) for the autonomous moble vehcles. The presented method has been valdated n 3-D envronment and s of partcular mportance for gudance, navgaton, and control of flyng vehcles. The Extended Kalman Flter (EKF) and the nose characterstc have been modfed usng the Fuzzy Logc Adaptve System and compared wth the performance of regular EKF. t has been demonstrated that the Fuzzy Adaptve Kalman Flter gves better results (more accurate) than the EKF. 2.2 Sensor Fuson When navgatng and gudng an autonomous vehcle, the poston and velocty of the vehcle must be determned. The Global Postonng System (GPS) s a satellte-based navgaton system that provdes a user wth the proper equpment access to useful and accurate postonng nformaton anywhere on the globe (see Brown and Hwang, 1992). However, several errors are assocated wth the GPS measurement. t has superor long-term error performance, but poor short-term accuracy. For many vehcle navgaton systems, GPS s nsuffcent as a stand-alone poston system. The ntegraton of GPS and nertal Navgaton System (NS) s deal for vehcle navgaton systems. n generally, the shortterm accuracy of NS s good; the long-term accuracy s poor. The dsadvantages of GPS/NS are deally cancelled. f the sgnal of GPS s nterrupted, the NS enables the navgaton system to coast along untl GPS sgnal s reestablshed. The requrements for accuracy, avalablty and robustness are therefore acheved.

3 J.Z. Sasadek / Annual Revews n Control 26 (2002) Kalman flterng s a form of optmal estmaton characterzed by recursve evaluaton, and an nternal model of the dynamcs of the system beng estmated. The dynamc weghtng of ncomng evdence wth ongong expectaton produces estmates of the state of the observed system (see Abd and Gonzalez, 1992). An extended Kalman flter (EKF) can be used to fuse measurements from GPS and NS. n ths EKF, the NS data are used as a reference trajectory, and GPS data are,appled to update and estmate the error states of ths trajectory. The Kalman flter requres that all the plant dynamcs and nose processes are exactly known and the nose processes are zero mean whte nose. f the theoretcal behavor of a flter and ts actual behavor do not agree, dvergence problems wll occur. There are two knds of dvergence: Apparent dvergence and True dvergence (Gelb, 1992). n the apparent dvergence, the actual estmate error covarance remans bounded, but t approaches a larger bound than does predcted error covarance. n true dvergence, the actual estmaton covarance eventually becomes nfnte. The dvergence due to modelng errors s crtcal n Kalman flter applcaton. f, the Kalman flter s fed nformaton that the process behaved one way, whereas, n fact, t behaves another way, the flter wll try to contnually ft a wrong process. When the measurement stuaton does not provde enough nformaton to estmate all the state varables of the system, n other words, the computed estmaton error matrx becomes unrealstcally small, and the flter dsregards the measurement, then the problem s partcularly severe. Thus, n order to solve the dvergence due to modelng errors, we can estmate unmodeled states, but t adds complexty to the flter and one can never be sure that all of the suspected unstable states are ndeed model states (Lews, 1986). Another possblty s to add process nose. t makes sure that the Kalman flter s drven by whte nose, and prevents the flter from dsregardng new measurement. n ths paper, a fuzzy logc adaptve system (FLAS) s used to adjust the exponental weghtng of a weghted EKF and prevent the Kalman flter from dvergence. The fuzzy logc adaptve controller (FLAC) wll contnually adjust the nose strengths n the flter's nternal model, and tune the flter as well as possble. The FLAC performance s evaluated by smulaton of the fuzzy adaptve extended Kalman flterng scheme of Fg.1. NS Corrected T Estmated NS errors J }o6ton, vetocty, etc Predcted measurements Resdual ~l F /~ Fg.1. Fuzzy adaptve extended Kalman flter [Sasadek, J. Z., and Wang, Q. (2001)] Weghted EKF Because the processes of both GPS and NS are nonlnear, a lnearzaton s necessary. An extended Kalman flter s used to fuse the measurements from the GPS and NS. To prevent dvergence by keepng the flter from dscountng measurements for large k, the exponental data weghtng (Lews, 1986) s used. The models and mplementaton equatons for the weghted extended Kalman flter are: Nonlnear dynamc model xk., = f(x k,k) + w k (1) w k ~ N(0, Q) Nonlnear measurement model z~ = h(xk, k) + vk (2) v k ~ N(0, R)

4 206 J.Z. Sasadek / Annual Revews n Control 26 (2002) Let us set the model covarance matrces equal to R k = Ra -2(k+0 (3) Qk = Q a-2(k+l) (4) where, c~1, and constant matrces Q and R. For a>l, as tme k ncreases, the R and Q decrease, so that the most recent measurement s gven hgher weghtng, ff or=l, t s a regular EKF. By defnng the weghted covarance Pff- = Pk-a 2k (5) The Kalman gan can be computed: - T - T Kk = Pk Hk (HkPk Hk + Ra-Z(k+l)) -1 =Pk Hk HkPk Hk +a2 The predcted state estmate s: k+l = f(xk, k) (7) The predcted measurement s: = h( q, k) (8) The lnear approxmaton equatons can be presented n form: Of(x,k). ~*" Ox ~=~; (9) The predcted estmate on the measurement can be computed:, =f +Kk(Z k -~,) (10) Oh(x,k). Hk = Ox x=x; (11) Computng the a pror covarance matrx: P~+~ -- a2~,~pff~ T + Q (13) Computng the a posteror matrx gves: covarance e~ = (- KkH k)pff- (14) The ntal condton s: Pg- = P0 n equaton (10), the term z k -~, s called resduals or nnovatons. t reflects the degree to whch the model fts the data NS and GPS The nertal navgaton system (NS) conssts of a sensor package, whch ncludes accelerometers and gyros to measure acceleratons and angular rates. By usng these sgnals as nput, the atttude angle and three-dmensonal vectors of velocty and poston are computed (Jochen et al., 1994). The errors n the measurements of force made by the accelerometers and the errors n the measurement of angular change n orentaton wth respect to nertal space made by gyroscopes are two fundamental error sources, whch affect the error behavor of an nertal system. The nertal system error response, related to poston, velocty, and orentaton s dvergent wth tme due to nose nput (Kayton and Fred, 1997). There are bases assocated wth the accelerometers and gyros. n order to correct the errors of NS, the GPS measurements are used to estmate the nertal system errors, subtract them from the NS outputs, and then obtan the corrected NS outputs. There s number of errors n GPS, such as ephemers errors, propagaton errors, selectve avalablty, mult-path, and recever nose, etc. Usng dfferental GPS (DGPS), most of the errors can be corrected, but the mult-path and recever nose cannot be elmnated. Pk-+l = Y~ k Pk elot + Q 0~-2(k+1) Re-wrtng (12) gves: (12) 2.3 Fuzzy Logc Adaptve System t s assumed that both, the process nose wk, and the measurement nose vk are zero-mean

5 J.Z. Sasadek / Annual Revews n Control 26 (2002) whte sequences wth known covarance Q and R n the Kalman flter, ff the Kalman flter s based on a complete and perfectly tuned model, the resduals or nnovatons should be a zero-mean whte nose process. ff the resduals are not whte nose, there s somethng wrong wth the desgn and the flter s not performng optmally (Lews, 1986). The Kalman flters wll dverge or coverage to a large bound. n practce, t s dffcult to know the exact values for Q and R. n order to reduce computaton, we have to gnore some errors, but sometmes those unmodeled errors wll become sgnfcant. These are the nstrument bas errors of NS. Sometmes the wk may be dfferent than zero mean. n those cases, the resduals can be used to adapt the flter. n fact, the resduals are the dfferences between actual measurements and best measurement predctons based on the flter's nternal model. A well-tuned flter s that where the 95% of the autocorrelaton functon of nnovaton seres should fall wthn the _ 2o boundary (Cooper and Dyrrant-Whte, 1994). f the flter dverges, the resduals wll not be zero mean and become larger. There are very few papers on applcaton of fuzzy logc to adapt the Kalman flter. Other authors (Abdelnour et al., 1993)), use fuzzy logc for on-lne detecton, and correcton of dvergence n a sngle state Kalman flter. There were three nputs and two outputs to fuzzy logc controller (FLC), and 24 rules were used. n our works (Sasadek and Wang, 1999), the basc adaptve fuzzy logc controller has been ntroduced and desgned. n ths paper the new FLAC s proposed. The purpose of the fuzzy logc adaptve controller (FLAC) s to detect the bas of measurements and prevent dvergence of the extended Kalman flter. t has been appled n three axes- East (x), North (y), and Alttude (z). The covarance of the resduals and mean values of resduals are used to decde the degree of dvergence. The value of covarance relates to R. ff the resdual has zero mean, the equaton for covarance of the resdual s: Pz - T = HkP ~ H k + R (15) The fuzzy adaptve Kalman flterng has been used for gudance and navgaton of moble robots, especally for 3-D envronment. The navgaton of flyng robots requres fast, and accurate on-lne control algorthms. The "regular" Extended Kalman Flter requres hgh number of states for accurate navgaton and postonng and s unable to montor the parameters changng. The FLAC requres smaller number of states for the same accuracy and therefore t would need less computatonal effort. Alternatvely, the same number of states (as n "regular" flter) would allow for more accurate navgaton Fuzzy adaptve Kalman flterng for parameter uncertantes Sometmes, uncertan or tme varyng parameters are consdered to exst n the Q and R matrces. The fuzzy adaptve Kalman flterng s used to detect and then adapt the flter on-lne. There are two groups of fuzzy controllers. n those two fuzzy controllers, the covarance of the resduals and the mean of resduals are used as the nputs to both controllers for all three fuzzy nference engnes. The exponental weghtng ct for frst group controller and the scales for second group controller of three axes are the outputs. The frst group, whch output s ~ was used to detect the flter dvergence and adapt the EKF. Generally, when the covarance s becomng large, and mean value s movng away from zero, the Kalman flter s becomng unstable. n ths case, a large ct wll be appled. A large a means that process noses are added. t can ensure that n the model all states are suffcently excted by the process nose. When the covarance s extremely large, there are some problems wth the GPS measurements, so the flter cannot depend on these measurements anymore, and a smaller a wll be used. By selectng an approprate, a, the fuzzy logc controller wll adapt the Kalman flter optmally and try to keep the nnovaton sequence actng as zero-mean whte nose.

6 208 J.Z. Sasadek /Annual Revews n Control 26 (2002) Some membershp functons are shown at fgure 2, 3 and 4. Zero ~Large 0 (2.5) 2 (5) 2 P~/R [m 2] Fg.2. Covarance Membershp Functons [Sasadek, J. Z., and Wang, Q. (2001), Zero ~Large f the covarance of resduals s small and the mean values s small then the scale s large. Table 1 and 2 are the rule table for those two groups of fuzzy controllers. Table. 1. Rule Table for a P Z S Mean Value Z S L Z S S S L S Mean ra] L Z NS NS Fg.3. Mean Value Membershp Functons [Sasadek, J. Z., and Wang, Q. (2001), S --- Small; Z --- Zero; L --- Large; NS --- Negatve Small Table. 2 Rule Table for Scale Scale Mean Value Z S L Fg.4. a Membershp Functons [Sasadek, J. Z., and Wang, Q. (2001), P Z S Z z Z S S S The fuzzy logc controller uses 9 rules, such as" L L S Z f the covarance of resduals s large and the mean value s zero Then a s zero. f the covarance of resduals s zero and the mean value s large Then ct s small. The second group, whch output s scale, was used to detect the change of measurement nose covarance R. From equaton (15), the R s related to the covarance of resdual, the larger the covarance of resdual, the more the measurement nose. When the fuzzy logc controller fnds that the covarance of resdual s larger than that expected, t apples a large scale to adjust the a. A sample rule s: Fuzzy adaptve Kalman flterng for non-whte process nose t s assumed that the process nose wk s whte nose for Kalman flterng. But sometme the process nose could be correlated wth tself, non-whte. n ths case, we can add a shapng flterng that manufactures colored nose wk wth a gven spectral densty from whte nose, but t wll ncrease the state varables. n some realtme stuaton, the computng tme have a restrcton for ncreasng the state varables. We can use a fuzzy adaptve Kalman flterng to adaptve the process nose rather than add more state varables. There are 9

7 J.Z. Sasadek / Annual Revews n Control 26 (2002) rules and therefore, lttle computatonal tme s needed. The membershp functons for ths fuzzy control are showed as fgure 5.8, 5.9, and The FLAC uses 9 rules, such as: f the covarance of resduals s large and the mean values are zero Then ct s large. f the covarance of resduals s zero and the mean values are large Then ot s zero. P Table. 3. Rule Table for FLAS Mean Value Ot Z S L Z S Z Z S Z L M L L M Z zero small large S --- Small; L --- Large; M --- Medum; Z --- Zero; 2.4 Smulaton 0 (4) 2 1.1(4) 2 (m 2) Fg.5. Covarance Membershp Functons [Sasadek, J. Z., and Wang, Q. (2001)] MATLAB codes developed by authors has been used to smulate and test the proposed method. The state varables used n smulaton are: zer( small large xk --- [Xk, 2k, Yk, J~k, Zk, 2k, cat, cat] (16) (m) Fg.6. Mean Value Membershp Functons [Sasadek, J. Z., and Wang, Q. (2001)] zer~ small medum large (a) Fg.7. t~ Membershp Functons [Sasadek, J. Z., and Wang, Q. (2001) ] The states are poston, and velocty errors of the NS East, North, Alttude, GPS range bas and range drft Smulaton experment 1 The frst part of smulaton uses the fuzzy adaptve Kalman flterng for parameter uncertantes. The desgned standard devaton of GPS measurement R s 5 [m]. The desgned standard devatons of Q for NS are meter, meter, and meter for the East (x), North (y), and Alttude (z) respectvely. The smulatons (Table 4, 5 and 6 and Fgure 8 and 9) show that after the flter s stablzed, the actual error covarance of fuzzy logc adaptve EKF almost agrees wth the theory error covarance. n the Table 4, 5 and 6, the desgned parameters are Q and R.

8 210 J.Z. Sasadek /Annual Revews n Control 26 (2002) The 5Q, 2R etc. mean that the real tme parameters are 5 and 2 tme as large as the desgned Q and R. n fgure 8, and 9, dashed lnes are the theoretcal covarance of EKF, and the sold lnes are the covarance of fuzzy adaptve EKF. Table 4 Comparson of theoretcal and actual error varance (X-axs) Q 5Q 50 3Q 5Q R R 2R 4R Theory Actual Table 5 Comparson of theoretcal and actual error varance (Y-axs) Q 5Q 5Q 3Q 5Q R R 2R 2R 4R Theory Actual Table 6 Comparson of theoretcal actual error varance (Z-axs) [Sasadek, J. Z., and Wang, Q. (2001) ] Q 5Q 5Q 3Q 5Q R R 2R 2R 4R Theory Actual Smulaton experment 2 and n the second set of smulatons, we smulate the nputs of non-whte process nose. The covarance of GPS measurement R s 25 [m2]. t s assumed that the measurements of NS have some bases. n the frst part of ths smulaton (Fg. 5), the mean values of NS are meter, meter, and meter for the East (x), North (y), and Alttude (z) respectvely. A whte nose wth a standard devaton of 3 meter s added to GPS measurements. The sample perod s 1 second. The frst row n Fg. 10 s the nnovatons of fuzzy adaptve EKF and EKF n East (x). The nnovaton of EKF had a large drft, and was stable at a hgh mean value. The fuzzy adaptve EKF clearly mproved the performance of EKF, and the mean value was much smaller than that of EKF. Other fgures present the corrected poston (frst column) and velocty (second column) errors. The corrected error s the current NS error mnus estmated NS error. The dashed lnes are the corrected errors of EKF, and the sold lnes are the corrected errors of fuzzy adaptve EKF. The fuzzy adaptve EKF sgnfcantly reduced the corrected poston and velocty errors. n the second part of ths smulaton (Fg. 11), the same measurements as n the frst part of ths smulaton for NS were used. A whte nose wth a standard devaton of 2 meter from 0 s to 1000 s and 1500 s to 2000s was appled to GPS measurements. From 1000 s to 1500 s, the standard devaton of 6 meter wth mean value of 6 meter was added to GPS measurements. Although, the GPS measurement noses features were changed, the fuzzy adaptve EKF stll worked well. Those smulatons also showed that the corrected errors of EKF were proportonal to the mean values of NS measurements. n other word, the more errors are not modeled, the worse the EKF performs. 2.5 Summary n ths chapter, a fuzzy adaptve extended Kalman flter has been developed to detect and prevent the EKF from dvergence. By montorng the nnovatons sequences, the FLAS can evaluate the performance of an EKF. ff the flter does not perform well, t

9 J.Z. Sasadek / Annual Revews n Control 26 (2002) would apply an approprate weghtng factor ot to mprove the accuracy of an EKF. The FLAS can use lower order state-model wthout compromsng accuracy sgnfcantly. Other words, for any gven accuracy, the fuzzy adaptve Kalman flter may be able to use a lower order state model. The FLAS makes the necessary trade-off between accuracy and computatonal burden due to the ncreased dmenson of the error state vector and assocated matrces. When a desgner lacks suffcent nformaton to develop complete models or the parameters wll slowly change wth tme, the fuzzy controller can be used to adjust the performance of EKF on-lne, and t wll reman senstve to parameter varatons by "rememberng" most recent N data samples. t can be used to navgate and gude autonomous vehcles or robots and acheved a relatvely accurate performance. 150 X a. 100 Q) -...." ; ~....f " "lme(s) ~,60 40,._ > ~me(s) n N > "tme(s) Fg.8. Actual and Theoretcal Covarance for 5Q and R. [Sasadek, J. Z., and Wang, Q. (2001),]

10 212 J.Z. Sasadek / Annual Revews n Control 26 (2002) nx 200 ~ 100 " j r - - f, ; t... ~...,...,L... K... l t4... '! ~,, ~, 150 ~100 o-- > N 150 ~ 100 o~ > l J -rme(s) ~ 4~ 5~ ~ "rme(s) ~... ~... ~... '---' ',-"--'--"'--- _. : " t J.. l '! t t nrne(s) Fg. 9. Actual and Theory Covarance for 5Q and 4R [Sasadek, J. Z., and Wang, Q. (2001)]

11 J.Z. Sasadek / Annual Revews n Control 26 (2002) Fuzzy Adaptve EKF EKF g g... ~ 2 o ~ "~" ~lll~'!" ' "lr... '"'... ' X ~P " -~ "~-2% lobo ' J, 0.5 ~,o go "X -5C ~_,ol > ~ ( O" ~. ~ -0.5 >, E 0 ~-~ ~ ~ -2C Tme (s) "N Tme (s) Fg. 10. Smulaton A [Sasadek, J. Z., and Wang, Q. (2001)] Fuzzy Adaptve EKF E" 20~1. ' o ~ o x o 5oo looo 15oo 2000 q, EKF Jo0 -o _ o ;oo ooo o 2 -" ' E E "~'2C,,,o: ~0"5 ~o... ~o._?, o. 51 >, q) >. ' 0 1C..,..,- %,.~.,.~"......~_,,,,j - ~ " _,=o N 'N -1C Tme (s) Tme (S) Fgure 11. Smulaton B [Sasadek, J. Z., and Wang, Q. (2001)]

12 214 J.Z. Sasadek /Annual Revews n Control 26 (2002) SENSOR FUSON FOR DEAD- RECKONNG MOBLE ROBOT NAVGATON 3.1 ntroducton n postonng and localzaton problems, two or more dfferent sensors are often used to obtan the best estmaton data for control system. Extended Kalman Flter (EKF) s wdely used to fuse those data to obtan one ntegrated, optmal result. One consderaton s usng EKF when the sgnal used durng navgaton s a whte nose sgnals. However, many sgnals n real world nclude non-whte nose. n ths case the regular Kalman flter derved for whte nose would dverge. Ths paper presents the sensor fuson for dead-reckonng moble robot navgaton. Odometry and sonar sgnals are fused usng Extended Kalman Flter (EKF) and Adaptve Fuzzy Logc System (AFLS). The AFLS was used to adapt the gan and therefore prevent the Kalman flter dvergence. The fused sgnal s more accurate than any of the orgnal sgnals consdered separately. The enhanced, more accurate sgnal s used to gude and navgate the robot. 3.2 Sensor Fuson For the navgaton system, there are two basc poston-estmaton methods commonly appled,.e. relatve and absolute postonng, see Borensten (1996), Shoval, et al. (1998), Jetto, et al. (1999), Jetto, et al. (1999), and Roumelots, et al. (1999). Relatve postonng, whch s sometmes called dead reckonng, s usually based on nertal sensors or odometry sensors. n ths method, the calculated dstance from ntal poston determnes current poston estmaton. n an absolute postonng system, the postonng sensors nteract wth a dynamc envronment, whch can be navgaton beacons, actve or passve landmark, map matchng, or satellte-based navgaton sgnal, to fnd the poston estmaton. To solve the postonng problems, there are two types of sensors avalable: nternal and external sensors, as explaned by McKerrow (1991). nternal sensors measure physcal varables on the vehcle tself. Ths selfcontanng characterstc means the measurement results of these sensors are almost always avalable to solve postonng problems. The examples of these sensors are accelerometer, odometry, gyroscopes, and compasses. External sensors measure relatonshps between the vehcle and ts envronment, whch can be natural or artfcal objects. The examples of external sensors are satellte sgnal recever, sonar sensor, radars, and laser range fnders. When the above sensors are mplemented to solve postonng problems, both have advantages and dsadvantages. For short perods, measurements usng nternal sensors are qute accurate. However, for long-term estmaton, the measurements usually produce a drft. On the contrary, because t measures absolute quantty, external sensors do not produce the drft, however, the measurements from these sensors are usually not always avalable, Santn, et al. (1997). The common method used n navgaton problem s to combne those sensors so that t wll produce the best desrable output. The common combnaton method s by applyng the Extended Kalman Flter (EKF), such as shown n the work by Jetto, et al., (1997, 1999), Tham, et al., (1999), Sasadek and Wang (1999), Sasadek and Hartana (2000). The most common combnaton of sensors used n postonng and localzaton problems s combnaton of odometry and sonar sensor. Odometry sensor s mounted on the robot's drvng wheels and regster angular movements of the wheels, whch are then translated nto lnear movements. Besde the drawback that the translaton ntroduces the error, see Sasadek and Hartana (2000), ths mplementaton makes the odometry sgnal always avalable. The sonar sensor, whch measures absolute poston of the robot, s used to update the poston measured by odometer.

13 J.Z. Sasadek / Annual Revews n Control 26 (2002) Other errors can also occur n odometry sensors. One s systematc error. Ths error causes the bas n one drecton of the movement of the vehcle. Borensten and Feng (1996) presented ther method to correct ths error. The method s based on a benchmark experment performed pror to regular operaton of the vehcle. The experment can fnd the systematc error and, subsequently, the error s appled to correct the control system parameters. f the systematc errors occur frequently, ths method may not be suffcent. For example, f the vehcle uses elastc tres, the benchmarkng process has to be performed each tme the unequal dameter occurs. t s benefcal that the error correcton s done n real tme operaton. t s wdely known that poorly desgned mathematcal model for the EKF wll lead to the dvergence. Clearly, f the plant parameters are subject to perturbatons and dynamcs of the system are too complex to be characterzed by an explct mathematcal model, an adaptve scheme s needed. Jetto, et al., (1999) used Fuzzy Logc Adapted Kalman Flter (FLAKF) to prevent the flter from dvergence when fusng measurement from odometry and sonar sensors. n ths method, the rato of nnovaton and covarance of nnovaton s used as nput to the fuzzy logc, and the output s used to weght the process nose covarance n EKF. Sasadek and Wang (1999) used exponental data weghtng to prevent the dvergence. Mean value and covarance of nnovaton are used as the nput of the Fuzzy Logc Adaptve Controller (FLAC). The output s then used to weght process nose and measurement nose covarance n EKF. Ths FLAC s mplemented on the flyng vehcle navgatng n three-dmensonal space. Both those methods have shown mprovement n the estmaton of the vehcle poston n comparson wth the EKF only. n ths paper, the systematc error n odometry sensor s corrected durng realtme operaton of the vehcle by usng measurements result from the sonar sensor. EKF s appled to fuse those two sgnals to fnd the best estmaton of poston. Adaptve Fuzzy Logc System (AFLS) s used to prevent the flter from dvergence. The objectve of ths paper s to develop an effcent method for sgnal fusng to get accurate postonng. 3.3 Mathematcal Model The model of the vehcle used n the smulaton s based on a dfferental-drve. n ths model, the vehcle can be steered by dfferentatng the wheels angular velocty. The knematc model of ths vehcle s descrbed by the followng equatons, see Wang (1988): Jc(t) = v(t) sn O(t) y(t) -- v(t) sn O(t) (17a) (17b) o(t) = to(t) (18) where, v(t) and to(t) are, respectvely, the lnear and angular veloctes of the robot, and are expressed by: v(t) to(t) (.O r (t) + tot (t) D (19) 4 (,t) r (t) - w l (t) D (20) 2d where D and d are the wheel dameter and the dstance between the odometry encoder respectvely. f, we denote the state varable of the vehcle as x(t)--[x(t) y(t) 0(t)] r, and the vehcle control nput as u(t) = [v(t) to(t)] r, the knematc model n equatons (17a) -18) can be wrtten n the form of stochastc dfferental equaton as: X(t) = f(x(t), U(t)) + W(t) (21) where w(t) s a zero-mean Gaussan whte nose wth covarance matrx Q(t), whch represents the model naccuraces. Ths tmeequaton s lnearzed and sampled n a small perod T =tk+ 1 -t k. Assumng that durng ths tme nterval, the lnear and angular veloctes are constant, and that the vehcle s

14 Kk+lCk+ 216 J.Z. Sasadek /Annual Revews n Control 26 (2002) followng an arc path (see Wang (1988)), then, the equatons for Extended Kalman Flter can be expressed by: Xk X k + BkU k (22) Pk-+, -- AkP#<Af + Qk (23) - T - T K~+ = P lck l[ck.lp~+lck l + Rk.l] -1 (24) where: xk+ 1 = x;+ 1 + Kk+[yk+ 1 - Ck+lXk+l] (25) Pk+l = [ - 1 ]PtT+l (26) n k -- xk =[xk Yk 0k]r T cos/0 k\ )A0t~ ~ 0 T sn/0 k A0k ) +-5-J o o 1 (27) (28) 0 -vktsno k A k = 1 vkt cos0k (29) 0 1 Qk=[Q1 Q2 Q3] (30) Q1-~l-o33(T3/3)v~snO, cosokl (31) [ )v, sn, ] [ -Q33 ( T3/3) v2 sn 0t, cos0 k Q2 =Qz2T +e33(t313)v~, cos2 0, (32) L ( T2/2)vk cos0k F-Q3 (r~/2)v k sn o k - o3 coso, (33) L o3 r and Q33 = or: are and, Qll = Crx, Qz2 = Cry, dagonal elements of covarance matrx Q(t) of w(t) n Eq. (21). The measurement, n ths case, wll consst of the measurement from odometry sensor and sonar sensor. The sze of the measurement vector depends on the number of actve sonar sensor. n general, ths vector can be expressed as (See Jetto et. al. (1999)): Y(Xk,)=[Xk Yk Ok d~ dzk... d~] r (34) where dn, s the measurement of sonar nth at tme k. 3.4 Adaptve Fuzzy Logc System n Kalman flter model, both process nose wk and measurement nose v k are assumed zero-mean whte nose sequence wth covarance Qk and Rk. f the model of EKF s tuned perfectly, the resdual between actual and predcted measurement should be a zero-mean whte nose process. Often, we do not know all parameters of the model or we want to reduce the complexty of modelng. Therefore, n real applcaton, the exact values of Qk and R k are not known, ff the actual process and measurement noses are not a zero-mean whte nose, the resdual n Kalman flter wll also not be a whte nose, ff ths s happened, the Kalman flter would dverge or at best converge to a large bound. Jetto, et al. (1999) used fuzzy logc adapted Kalman flter to prevent the flter from dvergence. The fuzzy logc controller uses one nput and one output. The rato between nnovaton and covarance of nnovaton process s used as an nput. The output s a constant, whch s used to weght the process nose covarance. The controller uses fve fuzzy rules, and t s mplemented n a wheeled moble robot equpped wth odometry and sonar sensors. Sasadek and Wang (1999) used fuzzy logc adapted controller (FLAC) to prevent the flter from dvergence when fusng sgnals comng from NS and GPS on flyng vehcle. Nne rules were used. There were two nputs, whch are the mean value and covarance of nnovaton, and the output s a constant that s used to weght exponentally the model and measurement nose covarance. n the case of fusng sgnals that come from odometry and sonar sensors, sometme only odometry measurements are avalable. The nnovaton wll be a whte nose as long as the process and measurement noses are assumed as a whte nose. However, when the sonar measurements become avalable,

15 J.Z. Sasadek / Annual Revews n Control 26 (2002) and combned wth the odometry measurement, the nnovaton mght be not a whte nose anymore. Ths wll cause the flter to dverge. When systematc error occurs n the vehcle, the process and measurement nose actually are not a gaussan whte nose, whch causes dvergence n EKF. AFLS can be used to adapt the flter gan so that the dvergence can be avoded. The adaptaton process used n ths paper s based on exponental data weghtng (Lews, 1986). The scheme of the adaptaton process s shown n Fg. 12. Odometnj ~ ~'~ ~' Estmated t,m ~ L_ &Sonar ~ O~ ~ t'oston [ ] r--~ C [ ~ - measu- > [ L J ~k romen+ &4 ze~large. 0 (4) 2 1.1(4) 2 (m 2) Fg.13. MF of nnovaton process covarance [Sasadek, J. Z. and Hartana, P. (2000)] z e ~ large (m) Fg.14. MF of nnovaton process mean value [Sasadek, J. Z. and Hartana, P. (2000)] Fg. 12. Adaptve Fuzzy Logc System (AFLS) scheme The membershp functon used n ths AFLS s dsplayed n Fg Fg.15. The AFLS uses nne rules, whch are summarzed n Table 1. Xk ze~arge ~ (a) Fg.15. MF of a [Sasadek, J. Z. Hartana, P. (2000)] Table 7. Rules table for AFLS and Weghted EKF Usng exponental data weghtng as an adaptaton process, the equaton for the EKF wll be dfferent. For exponental data weghtng, the weghted process and measurement nose covarance can be wrtten as: Rk = Ra-2(k+l) (35) Qk = Qa-2(*+) (36) where a > 1. Q and R are constant matrces of process and measurement nose covarance. For a > 1, as tme k ncreases, Qk and R k wll decrease, whch means that the most recent measurement s gven hgher weghtng. nnovaton process mean a value Zero Small Large nnovaton Zero Small Zero Large process Small Zero Large Medum covarancelarge Large Medum Zero f the weghted estmaton covarance s defned as: Pff- = Pka 2k (37) then the EKF equatons become: x~, 1 = x k + Bkuk (38) p~a+~. ct 2 AkP k a A k r +Qk (39) a- ~ a_ ~ Rk l Kk+l = P k+l C k+l [C+P+C k k 1 k+l + 2]-1 (40) o~ Xk Xk+ 1 4" Kk+l[yk+ 1 - Ck+lXk+l] (41) P~+a = [ - K ~+lck l lp~+q (42)

16 218 J.Z. Sasadek /Annual Revews n Control 26 (2002) Experments and Results Smulaton experments have been conducted to show the mplementaton of AFLS when fusng the sgnals that come from odometry and sonar sensor. Systematc error n odometry measurement, whch comes from unequal n wheel's dameter, s also consdered. The vehcle s planned to follow snus path n n-door envronment. The map of the n-door envronment along wth the movement of the moble vehcle that has systematc error s shown n Fg. 16. Three smulaton experments have been performed. The frst experment s to show the mplementaton of EKF n the moble robot usng odometry sensor, where the sensor has systematc error. The result of ths experment s shown n Fg.17. n ths experment, t shows that the mplementaton of EKF wth only one measurement sgnal s avalable, cannot be used to correct the systematc error. The EKF n ths case only flters the Gaussan whte nose of the odometry measurement error. However, the systematc error s stll present n the movement of the moble vehcle. A l ".L_J The second experment s to use the EKF to fuse measurement sgnals that come from odometry and sonar sensor wthout usng AFLS. Ths experment result s shown n Fg. 18. The presence of sonar sensor, whch measures the relaton of the moble vehcle and ts envronment, reduces the systematc error, and the moble vehcle can follow the desgned path. However, the movement of the moble vehcle n ths case s not smooth. The result of sonar measurement n ths experment s not used effcently to mprove the poston estmaton. The thrd experment s to use AFLS to adapt the gan of EKF to prevent the flter from dvergence. n ths experment, when the sonar measurement becomes avalable, the EKF uses ths sgnal to mprove ts estmaton. AFLS makes the poston estmaton smoother than wthout AFLS. The result of ths experment s shown n Fg Summary n ths chapter, Extended Kalman Flter (EKF) has been used to estmate the poston of the moble vehcle. To prevent the flter from dvergence, the nnovaton and covarance of nnovaton process are montored by usng Adaptve Fuzzy Logc System (AFLS). The result s an adaptaton n the gan of EKF. Odometry and sonar sensors have been used to llustrate the method. From the smulaton experment, t shows that besde the mprovement n the estmaton of poston, the method can also be used to correct the systematc error. Usng ths method, realtme operaton of the vehcle can be reduced. Fg.16. Map of n-door envronment [Sasadek, J. Z. and Hartana, P. (2000)]

17 J.Z. Sasadek /Annual Revews n Control 26 (2002) Poston of the vehcle 4. SENSOR FUSON BASED ON FUZZY KALMAN FLTER 4.1 ntroducton -2 3 ~4 L L x Poston (meter) Fg.17. Results of smulaton experment usng EKF wth only odometry measurement [Sasadek, J. Z. and Hartana, P. (2000)] ~o _o = Poston of the vehcle x Poston (meter) Fg. 18. Smulaton experment result usng EKF wth odometry and sonar measurement [Sasadek, J. Z. and Hartana, P. (2000)] >, Poston of the vehcle x Poston (meter) Fg.19. Smulaton experment result usng EKF wth odometry and sonar measurement, adapted by AFLS [Sasadek, J. Z. and Hartana, P. (2000)] n ths chapter, a fuzzy Kalman flter was presented, whch s based on fuzzy logc theory and Kalman flterng. t s smlar to Kalman flter when a lnear system wth Gaussan nose s consdered. However, when non-gaussan nose s ntroduced, t s shown that fuzzy Kalman flter s outperformng Kalman flter, whle Kalman flter does not work well. t was demonstrated the performance of Kalman flter and fuzzy Kalman flter for poston estmaton applcaton under dfferent knds of crcumstances. The comparsons are made to draw conclusons Kalman Flter Expermental measurements are never perfect, even wth sophstcated modern nstruments. The problem of estmatng the state of a stochastc dynamcal system from nosy observatons taken on the state s of central mportance n engneerng. Nose flterng s an mportant part of processng a real sgnal sequence. There are many knds of flters could be used for estmaton purpose, such as mean flter, medan flter, Gaussan flter, and so on. n ths artcle, we dscuss the performances of Kalman flter and fuzzy Kalman flter. There are two basc processes that are modeled by the Kalman flter. The frst process s a model descrbng how the error state vector changes n tme. Ths model s the system dynamcs model. The second model defnes the relatonshp between the error state vector and any measurements processed by the flter, and t s the measurement model. ntutvely, the Kalman flter sorts out nformaton and weghts the relatve contrbutons of the measurements and of the dynamc behavor of the state vector. The measurements and state vector are weghted by ther respectve covarance matrces.

18 220 J.Z. Sasadek / Annual Revews n Control 26 (2002) The Kalman flter estmates a process by usng a form of feedback control: the flter estmates the process state at some tme and then obtans feedback n the form of (nosy) measurements. As such, the equatons for the Kalman flter fall nto two groups: tme update equatons and measurement update equatons. The tme update equatons are responsble for projectng forward (n tme) the current state and error covarance estmates to obtan the a pror estmates for the next tme step. The measurement update equatons are responsble for the feedback.e. for ncorporatng a new measurement nto the a pror estmate to obtan an mproved a posteror estmate. t was assumed the random process to be estmated can be modeled n the form x~+ 1 = CkXk + w k (43) The observaton (measurement) of the process s assumed to occur as dscrete ponts n tme n accordance wth the lnear relatonshp P[=Ee~e: ]=E x~-x: x k-x~ (49) The equatons and the sequence of computaton step are shown n Fg.20 Project ahead: xl =., x, PL. =.,,p,~ r +O, Enter pror estmate ^- xk and ts error l covarance p[ Compute Kalman gan: K k = pk-h~ (HkPk-H r + R k )-' 1 P t es mate 1 t 7"-- measurement zg / / Compute error covarance for updated estmate: P, = (l-kkhk)p[ zt, = Hkx k + v, (44) The covarance matrces for the wk and vk vectors are gven by E[WkWr] = ~Q~, =k (45) [0, ~k E[vkv ] = [0, =k (46),~ k E[wkv r ] = 0, for all k and (47) We also assume that we know the error ^_ covarance matrx assocated wth x~. That s, we defne the estmaton error to be ^_ e~ = x k - xt (48) and, the assocated error covarance matrx s Fg. 20. Kalman Flter Recursve Computaton Loop [Sasadek, J. Z. and Hartana, P. (2000)] t s clear that once the loop s entered, t can be contnued ad nfntum Fuzzy Logc Control Fuzzy logc control s a control method based on fuzzy logc. Just as fuzzy logc can be descrbed smply as "computng wth words rather than numbers"; fuzzy logc control can be descrbed smply by "control wth sentences rather than equatons". The basc confguraton of the fuzzy logc controller s shown n Fg. 21.

19 J.Z. Sasadek / Annual Revews n Control 26 (2002) Refer. r(0 Z 11 nferen( r o n mts u(o ~z~,~--area under the membershp functon Center Average method... RULE- BASE =y (51) Outputs y(t) 4.4 Dynamc System Model Fg: 21. Fuzzy logc Controller Archtecture 1) Rule Base Specfcally, the fuzzy rule-base comprses the followng fuzzy f-then rules: F x 1 s A~ and...and x, s Al~, THEN y s B l where ~1 and B t are fuzzy sets n U C R and V C R, respectvely, and X=(X~,X2,...,Xn)'~U and y~v are the nput and output (lngustc) varables of the fuzzy system, respectvely. 2) nference Mechansm The premses of all the rules are compared to the controller nputs to determne whch rules apply to the current stuaton. The "matchng" process nvolves determnng the certanty that each rule apples. 3) Fuzzfcaton The fuzzfcaton process s the act of obtanng a value of an nput varable and fndng the numerc values of the membershp functon(s) that are defned for that varable. 4) Defuzzfcaton Defuzzfcaton operates on the mpled fuzzy sets produced by the nference mechansm and combnes ther effects to provde the "most certan" controller output. Center of Gravty (COG) method uc, Z,b, fl,,,, where b (5o) --center of the membershp functon of the consequent of rule (/) n ths chapter a dynamc system model s used whch conssts of a spacecraft acceleratng wth random bursts of gas from ts reacton control system thrusters, the vector x mght consst of poston P and velocty V. The dynamc equatons are 1 Pk+l =Pk +VkAt +~ak At2 (52) The system equaton s Vk+ 1 = V k + akat (53) rpk+'l=[loat'l[-pk- r~2]+ :2 a, (54) where ak s the random, tme-varyng acceleraton and At s the tme between step k and step k+l. Now suppose we can measure the poston P. Then our measurement at tme k can be denoted zk = Pk + vk, where vk s random measurement nose. We assume that the process nose wk s Gaussan nose wth a covarance matrx Q. Further assume that the measurement nose vk s Gaussan nose wth a covarance matrxr, and that t s not correlated wth the process nose. The state transton matrx s: o,=l10:,l The measurement matrx s: (55) Hk=[1 0] (56) Process nose matrx s:

20 222 J.Z. Sasadek / Annual Revews n Control 26 (2002) rl ] nose (57) L at J Measurement nose s : Vk = v~ ~N(0,R) (58) 4O[ ~ ~ 30 L --]---~- ~ ~ - F - ~ - ~ F- - - T ~ ~ ~ -- //2 *process nose* =e]l At J [[1At2 At ]* process nose 2 / * pro~ nose 2 (59) ~ ~me(sec) Fg.23 Measured Poston Trajectory [Sasadek, J.Z., and Khe, J., (2001)] R, =E[v,v;] = measurement nose 2 (60) The ntal parameters for chosen smulaton: 1. True poston trajectory: xk = 10*sn(tk); 2. Standard devaton of poston measurement nose: 10 m; 3. Standard devaton of acceleraton process nose: 0.5 m/s2; 4. Total smulaton tme perod: 100 sec.; 5. Tme step At: 0.2 sec.; 15 t 10 o ~ ! / L o lo 20 3O O 9O Tme (see) Fg. 24 Estmated Poston Trajectores [Sasadek, J.Z., and Khe, J., (2001)] 4.5 Smulaton Results 40 F J r 30 --T T--q- ----F--S-- ~ ) r~ 10 l -10 ' Fg. 22 True Poston Trajectory [Sasadek, J.Z., and Khe, J., (2001)] ~rne (sec) r ] ] f [ / q t me (s~) m Fg. 25 nnovaton [Sasadek, J.Z., and Khe, J., (2001)]

21 J.Z. Sasadek / Annual Revews n Control 26 (2002) r ~ b ~ k ~ - - q q , ~ ~ "nne (sec) Fg. 26 Kalman Flter Gan [Sasadek, J.Z., and Khe, J., (2001)] x L _ / /. Q t 0.6. _ l _ J _ ' ' L - _ ] [ ' L L J _ - L - - J _ L l tll- -t- - L t... L... ~ "tme Fg. 28 Process nose covarance Q [Sasadek, J.Z., and Khe, J., (2001)] (s~) From Fg. 23 to 26, we can see that Kalman Flter works very well n spte of large measurement nose. Kalman gan K converges to a certan value and becomes stable at n ths case. A. Process Nose Covarance Q and Measurement Nose Covarance R [13][14] --Weghted Q and R Qk = (~(Z-2(k+l) (61) R k = Ra-2(k*l) where ct > 1, Q and R are constant matrces. lo0 T " t t ~ 90! , - - -, _ , _ _. - _. _ ] r r f f f f ~ f 80 -T T--q------F--T R ~ t l F t t t SC - T T - - q T ~ $ _ T - - J T % T ~ / / / _ L l _1 _ M F T / ~ Tme (sec) Fg. 29. Measurement Nose Covarance R [Sasadek, J.Z., and Khe, J., (2001)] ) 20 r~ 0"91 ~ q t t l l J- 1 _ J.. _..... L L r 0, L----~..... L----J-----d L K h O0.tme (sec) L 0.4 ~ R 0.3 t T......, - - -, - -, - - -, - - -, - -, - - -, t T T--~------F--T ) 0.1 } ~---, F--T q r o Tme (sec) Fg. 27 Estmated Poston Trajectory [Sasadek, J.Z., and Khe, J., (2001)] Fg. 30 Kalman Gan K [Sasadek, J.Z., and Khe, J., (2001)]

22 224 J.Z. Sasadek / Annual Revews n Control 26 (2002) Wth the decreasng of Q and R, Kalman gan K becomes dvergng and cannot reach a stable value. The estmated poston trajectory (Fg. 27) becomes more and more naccurate too. B. Correlated Process Nose and Measurement Nose Let ths correlaton be descrbed by E[w'vr] ={ C~ #k =k (62).. _. -, ;- f -,o 4 _2O t l _. t ~o-- ~---C--T--]---7 t t f V -.50 t t t 10 2O 3O 40 5O O "lme (tle ) Fg. 31 Estmated poston [Sasadek, J.Z., and Khe, J., (200)] 100 A generalzed dervaton yelds the optmal estmaton algorthm wth the same ntal condtons and measurement update relatons: ^- ^ -,-1( ^-) z -rcx (63) +Q,-q (n P;n: + L At J (64) (65) Ths can be compared to the case of no correlaton between wk and vk. The decrease n steady state values from =[ ] to [ ] P[ [_ J t_ J =[ ] tof ] and p~ [ _1 [_ J s due to the explotaton of the correlaton between the dynamc nose and the nose that corrupts the observable outputs: the zk realzatons reveal more about the nose process wk. 4.6 Fuzzy Kalman Flter Now we ntroduce an "optmal" state estmator, based on fuzzy set theory, whch [t 1. 1, X 10 ~ T --,... ~ --,"..., -~- ~... ~ -,~- --,--- ~ ~-~.... ~..... f.... ] F- - - F - q.... t 1 t t " -- F - - q F F - ] - -,,,,_,_,,,_,_,,,,... V t.....,,,,, ~ go 1 O0 Tme ($oc) Fg. 32 Kalman Flter Gan [Sasadek, J.Z., and Khe, J., (2001)] s capable of dealng wth systems wth random dsturbances and "uncertanty". We wll refer to ths as a fuzzy Kalman flter, and t s a fuzzy system model of the process n the estmator. Ths flter s smlar to that of the Kalman flter when a lnear system wth Gaussan nose s consdered. Let the nputs be ek,ek,%,,ep,, and the output s the estmated poston at tme step k+l, where e~ s the error between measured poston value and true poston value, ek s the change n ek, ep~ s the dfference between Pk and P[, and ep, s the change n %. Here we use a MSO (Multple nputs Sngle Output) system to accomplsh the task. The rules beng used are such as Rule 1: F ek s negatve large, ek s zero, % s negatve large, and ep, s negatve large, THEN estmated poston at step k+l s postve large.

23 J.Z. Sasadek / Annual Revews n Control 26 (2002) Rule 2: F e, s zero, e, s zero, % s negatve large, and e~, s negatve large, THEN estmated poston at step k+l s postve large. Rule 3: F e~ s postve large, e, s zero, % s zero, and e~, s zero, THEN estmated poston at step k+l s negatve large. Rule 4: F e~ s zero, e~ s postve small, er~ s negatve small, and e~ s negatve small, THEN estmated poston at step k+l s negatve small. The results are plotted n Fg. 33 to Fg. 36. Compared wth Fg. 24 whch s obtaned by usng Kalman flter, t s more accurate. X 10 ~ Error between p and Estm~ed P r~. t [ ~ ~ , - -, - -, t - -, - -, - t -, - -, - -, - -, O lp L ~ 1 1 t r ~ ~ t -5 -, - -, - -, - -, - -, - -, - -, - -, - -, ~ ~ S ~ "~lme (sac) Fg. 35 ep, and epk [ Sasadek, J.Z., and Khe, J., (2001)1 A. Process Nose Covarance Q and Measurement Nose Covarance R-- Weghted Q and R (as n Kalman flter) J _ t -- T~ - ~ - 5r - F, f - - T~ -~---~ 1 5 -, ' o 510 6o 70 8o 9o los ~3 rne (s~) Fg. 33 Estmated Poston [Sasadek, J.Z., and Khe, J., (2001)] t.../ll= -- l --U--1 r t r q ~ me (see) -~ 'o ' ~'o --~--~ ' " ~ ~ - Fg. 36 Estmated Trajectory [Sasadek, J.Z., and Khe, J., (2001)] Error between Measured Poston and True Poston Error between Measured Poston and True Poston 40(--- f P 1 t / t 20 - J- - J - - _t.... _ L _.3 _ o 5o 6o too "lroe (pec) Change n aoo~ error t l f t to "lme (~ec) Change n aoo'~ error 41 l T d 0 t --] ] F o 30 4o 5o 6o 70 8o 9o 100 "tme (see) Fg. 34 e, and ek [Sasadek, J.Z., and Khe, J., (2001)1-4 [ t h L lme Fg. 37 ee and e~ [Sasadek, J.Z., and Khe, J., (2001), (see)

24 / 226 J.Z. Sasadek / Annual Revews n Control 26 (2002) x 10 ~ Error between P and Estmated P x t0 s Error between P and Estmated P k ~ J ~ ] tlr-- - f-tl ~ lme (@ec) x t0 -~ Change m ado~e error,,~ ~ t_ 1 l "tme(~ec) X 10 "7 Change n BDO~ error 1 J, ~, ~ 05 ~ ' -- :.... ~ t ] me (see) Fg. 38 % and epk [Sasadek, J.Z., and Khe, J., (2OOl)] -- - ~ --,~ ~ -1 L~ F t t "tme (sec) Fg. 41 ep~ and ep k [ Sasadek, J.Z., and Khe, J., (2001)] B. Correlated Process Nose and Measurement Nose (as n Kalman flter) 15 J r r x,,,!t, tf = ~ Tme (sec) Fg. 39 Estmated Poston [Sasadek,J.Z., and Khe, J., (2001)] Error between Measured Poston and True Poston 401- r 20 - J ~ ~ ~ me (~ec) Change n El~o'~e error l "tme (sec) 4. 7 Summary The method that s the most wdely used for sensor fuson n engneerng applcatons s the Kalman flter. Ths flter s often used to combne all measurement data (e.g., for fusng data from dfferent sensors) to get an optmal estmate n a statstcal sense, ff the system can be descrbed wth a lnear model and both the system error and the sensor error can be modeled as Gaussan nose, then the Kalman flter wll provde a unque statstcally optmal estmate for the fused data. Ths means that under certan condtons the Kalman flter s able to fnd the best estmates based on the "correctness" of each ndvdual measurement. On the bass of smulaton performed n ths chapter for fuzzy Kalman flter and regular Kalman flter under dfferent condtons and parameters, t was shown that for lnear systems and trangular shaped membershp functons, the fuzzy Kalman flter works smlarly as Kalman flter but produces better results than the Kalman flter. The general comparson of fuzzy Kalman Flter and regular Kalman Flter was presented. t can be proved that the convergence of Kalman Flter's estmate to the true value s guaranteed only when the system s stochastcally controlled and observed. Fg. 40 e k and ek [Sasadek, J.Z.,and Khe, J, (2001)]

25 J.Z. Sasadek / Annual Revews n Control 26 (2002) REFERENCES Abdelnour, G., Chand, S., Chu, S., and Kdo T., (1993)," On-lne Detecton & Correcton of Kalman Flter Dvergence by Fuzzy Logc ", 1993 Amercan Control Conf., pp Abd, Mong, A. and Gonzalez, Rafael C., (1992), "Data Fuson n Robotcs and Machne ntellgence ", Academc Press, nc., Babuska, Robert, (1998), "Fuzzy Modelng for Control", Kluwer Academc Publshers, 1998 Borensten, J., & L. Feng.(1996). Measurement and correcton of systematc odometry errors n moble robots. EEE Transactons on Robotcs and Automaton 12(6), Borensten, Johann; Everett, H. R.; Feng, Lqang, (1996), "Navgatng Moble Robots", A. K. Peters, Ltd., 1996 Brown, Robert Grover, and Hwang, Patrck Y. C.,(1992), "ntroducton to Random S~gdnals and Appled Kalman Flterng", 2 edton, John Wley & Sons, nc., 1992 Cooper, S. and Durrant-Whyte H., (1994), "A Kalman Flter Model for GPS Navgaton of Land Vehcles ", 1994 EEE nt. Conf. on ntellgent Robot and Systems, pp Chand, S., and Hansen, A. (1989), "Energy Based Stablty Analyss Of A Fuzzy Roll Controller Desgn For A Flexble Arcraft Wng", Proceedngs of 1989 EEE Conference on Decson and Control, pp , Tampa, Florda, December 1989 Cuman, A., (1982) "On a Possblstc Approach To The Analyss Of Fuzzy Feedback Systems", EEE Transactons on Systems, Man, and Cybernetcs vol.12, pp ,may/june 1982 Gelb, A., (1974), " Appled Optmal Estmaton ", The MT Press, 1974 Jetto, L., S. Longh, & G. Venturn (1997). Development and expermental valdaton of an adaptve estmaton algorthm for the on-lne localzaton of moble robots by multsensor fuson. Preprnts of the Ffth FAC Symposum on Robot Control, SYROCO 1997, , Nantes, France, 1997 Jetto, L., S. Longh, & D. Vtal (1999). Localzaton of a wheeled moble robot by sensor data fuson based on a fuzzy logc adapted Kalman flter. Control Engneerng Practce 7, Jetto, L., S. Longh, & G. Venturn (1999). Development and expermental valdaton of an adaptve extended Kalman flter for the localzaton of moble robots. EEE Transactons on Robotcs and Automaton 15(2), Jochen, P., Meyer-Hlberg, W., and Thomas, Jacob (1994) "Hgh Accuracy Navgaton and Landng System usng GPS/MU System ntegraton", 1994EEE Poston Locaton and Navgaton Symposum, pp Kalman, R. E., "New Methods n Wener Flterng Theory", Proc. Symp. Eng. Applcatons of Random Functon Theory and Probablty, John Wley, New York, 1963 Kayton, M. and Fred, W. R., "Avoncs Navgaton Systems ", John Wley & Sons, nc Kosko, Bart, "Neural Networks And Fuzzy Systems", A Dynamcal Systems Approach to Machne ntellgence, Prentce- Hall, nc., 1992 Layne J.R., Passno K.M., (2001) "A Fuzzy Dynamc Mode Based State Estmator", Fuzzy Sets and Systems, Lews, F. L., (1986), " Optmal Estmaton wth an ntroducton to Stochastc Control Theory ", John Wley & Sons, nc., Mamdan, E. "Twenty years of fuzzy control: experences ganed and lessons learnt." Proceedngs of the Second EEE nternatonal Conference on Fuzzy Systems, 1993, pp Maybeck, Peter S., (1979), "Stochastc Model, Estmaton and Control", Vol.1-3, Academc Press, nc McKerrow, Phllp John (1991) "ntroducton to Robotcs." Addson- Wesley Publshng Company, Sydney