Q-Adaptation of UKF Algorithm for Estimation of the Autonomous Underwater Vehicles Dynamics

Transcription

1 Proceedngs of the 5 th Internatonal Conference of Control, Dynamc Systems, and Robotcs (CDSR'8) Nagara Falls, Canada June 7 9, 208 Paper No. 03 DOI: 0.59/cdsr8.03 Q-Adaptaton of UKF Algorthm for Estmaton of the Autonomous Underwater Vehcles Dynamcs Chngz Hajyev, Hall Ersn Soken 2 Istanbul echncal Unversty, Aeronautcs and Astronautcs Faculty Maslak 34469, Istanbul, urkey cngz@tu.edu.tr 2 Japan Aerospace Exploraton Agency (JAXA), Insttute of Space and Astronautcal Scence (ISAS) Kanagawa, Japan ersn_soken@ac.jaxa.jp Abstract - Accurate estmaton for the pose and orentaton of the autonomous underwater vehcles (AUVs) s necessary n most of the applcatons. hs s same for both the mssle type AUVs or the vehcles used for research purposes deep n the sea or ocean. A nonlnear verson of the Kalman Flter as the unscented Kalman flter (UKF) gves satsfactory estmaton results for ths purpose n the normal operaton condtons. However, n the deep sea, changes n the envronment (process nose) ether nstantaneously or perodcally are very lkely. In such case, the UKF must be adapted to become robust aganst such changes and provde accurate estmaton results even n ths case. In ths study, we propose process nose covarance matrx adaptaton (Q-adaptaton) for the UKF algorthm. he man am s to make the algorthm adaptve aganst the changes n the process nose covarance. he Adaptve UKF (AUKF) estmates the AUVs dynamcs. We nvestgate the performance of the algorthm when the process nose ncreases and decreases, whch are very lkely as a result of changes n the AUV dynamcs n dfferent envronments. he results are compared wth those of a non-q-adaptve verson of the UKF algorthm. Keywords: Autonomous Underwater Vehcle, Estmaton, Q-Adaptaton, Adaptve Kalman Flterng, Scale Factor.. Introducton Researches on underwater systems have ganed an mmense nterest durng the last decade wth applcatons n many dfferent felds. Sgnfcant number of autonomous underwater vehcles (AUVs) has been developed for performng a wde range of scentfc mssons for deep sea research. he AUVs requre a precse navgaton system for localzaton, postonng, path trackng, gudance, and control durng long perod of duty cycles. o develop an accurate and robust navgaton and control system for an AUV, fault tolerant estmaton of the AUV dynamcs s a crucal requrement. Snce t was proposed, Kalman flter has been wdely used as a method to estmate the AUV dynamcs parameters [] and dfferent Kalman flter types have been tested for that purpose. As a known fact, dynamcs parameters of an autonomous underwater vehcle (AUV) cannot be estmated by lnear Kalman flters because of the nherent nonlnear dynamcs and knematcs. In such case, the Extended Kalman Flter (EKF) may be used nstead. he EKF can estmate moton dynamcs parameters of an AUV, whch has typcal navgaton sensor outfts such as compass, pressure depth sensor, and some class of nertal navgaton system (INS) [2]. However, mandatory lnearzaton phase of the EKF procedure may cause the flter to dverge and usually lmts the flter s achevable estmaton accuracy [3, 4]. Unscented Kalman Flter (UKF) s a newer Kalman flterng technque compared to the EKF and n case of nonlnear dynamcs t affords consderably more accurate estmaton results. he essence of the UKF s a determnstc samplng technque that we use for calculatng a mnmum number of sample ponts [4]. hese sample ponts enable an estmaton accuracy of thrd-order for Gaussan data compared to the frst-order accuracy of the EKF. As a result the UKF has become one of the preferred flterng algorthms for the AUV dynamcs estmaton as well [5]. A drawback of the tradtonal Kalman flterng algorthms ncludng the UKF, they are not robust aganst the envronment changes. In case of such changes the estmaton accuracy of the flter deterorates and f the faults are long lastng, the flter may dverge. Specfcally for the AUVs, harsh operaton condtons and anomales n the deep water make

2 such crcumstances very lkely. hs stuaton leads us to a search for an Adaptve UKF (AUKF) that we can use for dynamcs estmaton of an AUV. Varous algorthms for adaptng the Kalman flter have been proposed startng wth the earler publcatons by Mehra [6, 7]. One of these deas s to scale the covarances of the flter. he parameters that are used for scalng can be calculated usng dfferent means [8-0]. hs dea s also applcable to the UKF adaptaton. In [] we proposed an ntegrated algorthm whch adapts both the measurement and process nose covarance matrces of the UKF for satellte atttude estmaton. In ths paper, we adapt the UKF algorthm that we use for estmatng the dynamcs of an AUV by usng a straghtforward approach. Dfferent than our prevous researches on adaptve Kalman flterng, whch examned a lnear problem wth both lnear process and measurement models [9], and a nonlnear problem wth both nonlnear process and measurement models [], ths tme the system equaton s nonlnear but the measurement model s lnear. Hence we use a specal form of the UKF. Besdes we apply the method for the AUV dynamcs estmaton for the frst tme and show that the process nose covarance of the flter can be easly adapted to ensure both the stablty and accuracy n ths specal from wth lnear measurement model. Snce the measurement model of the UKF s lnear the flter adaptaton s smlar to the one for a lnear KF and smpler compared to the extng AUKF methods for underwater vehcles. Dfferent than the exstng works such as [2, 3], whch requre parallel two flters runnng to both estmate the AUV dynamcs and adapt the flter, the proposed algorthm has only one flter runnng. hus t does not requre a cumbersome addtonal computatonal load opposed to the two-flter based adaptve algorthms [2, 3]. here are other sngle-flter based adaptve algorthms proposed for AUV state estmaton n the lterature [4]. However they have other drawbacks such as sufferng from nstablty ssue, whch appears whenever the adapted nose covarance matrx lose postve semdefnteness. In contrast n the proposed algorthm the stablty s secured by ) adaptng the flter only when necessary and usng the optmal process nose covarance throughout the rest of the estmaton process; 2) usng the covarance scalng approach n whch the adaptve factor takes always postve values. 2. Autonomous Underwater Vehcle Mathematcal Model AUV modelng s farly complcated, and an exact analyss s only possble by ncludng the underlyng nfnte dmensonal dynamcs of the surroundng flud (sea water). Whle ths can be done by usng partal dfferental equatons n Computatonal Flud Dynamcs (CFD) computer tools, t stll nvolves a formdable computatonal burden, and s nfeasble for most practcal applcatons. AUVs moton wth 6 degrees of freedom (6DOF) s necessary to determne the poston and orentaton of a rgd body [5, 6]. he frst three coordnates and ther tme dervatves are of translatonal moton along the x, y and z-axes, whle the last three coordnates (,, ) and ther tme dervatves are used to descrbe the orentaton and rotatonal moton (See Fg.). Fg. : 6DOF AUV Angular and ranslatonal Motons. In ths study, 6DOF AUV equatons of moton are derved as below [5]: Surge or translatonal moton along the x-axs: ()

3 Sway or translatonal moton along the y-axs: (2) Heave or translatonal moton along the z-axs:. (3) Roll or rotaton about the x-axs: (4) Ptch or rotaton about the y-axs:. (5) Yaw or rotaton about the z-axs:.. where; X, Y, Z, K, M, and N represent the resultant forces and moments wth respect to the body-fxed coordnates. Detals about the physcally based model parameters of AUV above and ther defntons are gven n [5]. 3. Unscented Kalman Flter for AUV Dynamcs Estmaton he UKF uses sgma ponts that are calculated based on a pror mean and covarance of the state to represent the state dstrbuton. It s founded on the ntuton that t s easer to approxmate a probablty dstrbuton than t s to approxmate a nonlnear functon or transformaton [3, 4]. In ths sense, t offers a dervatve-free alternatve to the EKF wth a more accurate approxmaton to nonlnear dynamcs than the lnearzaton of the aylor seres. he ntal step for the UKF s the determnaton of 2 ˆx k k and a covarance of P k k. For an n dmensonal state vector, these sgma ponts are obtaned by, n sgma ponts wth a mean of (6) x k k x k k, 0 ˆ ˆ x k k x k k n P k k, (7) (8)

4 ˆ x n k k x k k n P k k, (9) where, x0 k k, x k k and x nk k are sgma ponts, n s the state number and s the scalng parameter whch s used for fne tunng and the heurstc s to chose that parameter as n 3. Also, s gven as K n corresponds to the th column of the matrx. Next each sgma pont s transferred by usng system dynamcs, and x k k f x k k, k. K n. (0) hese transformed values are used to get the predcted mean and the covarance, 2n xˆ k k x0 k k x k k, () n Pk k x k k xˆ k k x k k xˆ k k n 2n x ˆ ˆ k k x k k x k k xk k Q( k) 2. (2) Here, xˆ k ks the predcted mean, Qk s the process nose covarance and Pk k s the predcted covarance. Further steps of the UKF for the AUV dynamcs estmaton problem are smlar to those of a lnear Kalman flter snce the measurement model s lnear. In these steps we do not need to use the sgma ponts anymore. hs smplfes the procedure and also reduces the computatonal load. Frst, the predcted observaton vector s, yˆ( k / k) H( k ) xˆ( k / k) (3) where Hk ( ) s the measurement matrx. After that, observaton covarance matrx and the cross correlaton matrx are determned as, P ( k / k) H( k ) P( k / k) H ( k ), (4) yy P ( k / k) P( k / k) H ( k ) (5) xy Followng part s the update phase of UKF algorthm. At that phase, frst by usng measurements, yk, resdual term (or nnovaton sequence) νk ( ) s found as the dfference between the actual observaton and the predcted observaton: he nnovaton covarance s, k yk yˆ k k, (6) yy P k k P k k R k H( k ) P( k / k) H ( k ) R( k ) (7) vv

5 Here Rk or s the measurement nose covarance matrx. he optmal Kalman gan s computed as xy vv At last, the updated state and covarance matrx are determned by, Here, xˆ k k ˆ K k P k k P k k. (8) xˆ k k x k k K k k, (9) xy xy P k k P k k P k / k P k k P k / k, (20) vv P k k P k k K k P k k K k. (2) s the estmated state vector and Pk k s the estmated covarance matrx. 4. Process Nose Covarance Adaptaton for the UKF Algorthm A Q-adaptve algorthm must be ntroduced such that the flter s nsenstve to process nose changes and corrects estmaton process wthout affectng the good estmaton behavor. Adaptve algorthm affects the characterstcs of the flter only when the condton of the process nose does not correspond to the model used n the synthess of the flter. Otherwse the flter works wth regular algorthm (7)-(2) n an optmally. Adaptaton occurs as a change n the predcted covarance. Frst, let us rewrte (2) as; So P k k P k k P k k Q( k). (22) s the predcted covarance wthout the addtve process nose. In order to adapt the covarance an adaptve scalng factor s put nto the covarance matrx of the nnovaton sequence; Pvv k k H( k ) P ( k / k) H ( k ) ( k) H( k ) Q( k) H ( k ) R( k ). (23) Here ( k) s the adaptve process nose scale factor. he left hand sde of (23) represents the real fltraton error whle the rght hand sde s the accuracy of the nnovaton sequence known as a result of pror nformaton []. When the predcted observaton vector yˆ( k / k) s reasonably dfferent from the measurement vector, yk ( ), the real fltraton error exceeds the theoretcal one. Hence, the gan matrx must be fxed hereafter by the use of adaptve algorthm and the scale factor ( k) []. o calculate the system nose scale factor, the equalty of ( ) ( / ) ( ) ( ) ( ) ( ) ( ) ( ) tr k k tr H k P k k H k k tr H k Q k H k tr R k (24) s used. Here tr(.) s the trace of the related matrx. akng nto consderaton that tr k k k k, (25)

6 the sngle scale factor ( k) s determned from the equalty (24) as k k tr H ( k ) P ( k / k) H ( k ) trr( k ) ( k) tr H ( k ) Q( k) H ( k ) (26) We can conclude that f the process nose covarance change, ( k) wll change and so affect the Kalman gan matrx as K k Pxy k k H( k ) P ( k / k) H ( k ) ( k) H( k ) Q( k) H ( k ) R( k ). (27) Accordng to the proposed approach the gan matrx of UKF s changed when the followng condton s vald tr k k tr H( k ) P ( k / k) H ( k ) tr H( k ) Q( k) H ( k ) tr R( k ) (28) If the left sde of the expresson (28) s greater than the rght sde, the scale factor value ( k) wll ncrease. hs corresponds to the begnnng of adaptaton of flter. Consequently, the nnovaton covarance (equaton (23)) ncreases, and K k (equaton (27)) decreases, whch wll cause strengthenng of the correctve nfluence of the flter gan matrx nnovaton sequence n the estmaton algorthm and approach the estmaton value xˆ k k to the actual value xk. hs wll lead to the decrease of nnovaton sequence k (equaton (6)) and adaptve factor ( k), weakenng of the correctve nfluence of nnovaton sequence, etc. Consequently, an adaptve scheme s performed to adjust the process nose covarance matrx n response to the changng envronment. As a result, the changes n the process nose covarance are corrected by the system, wthout affectng the good estmaton behavour. Remark that, due to the scale factor, the covarance of the estmaton error of the AUKF ncreases n comparson wth the regular UKF. herefore, the adaptve algorthms are used only n case of change n the process nose covarance and n all other cases the optmal UKF gan s used. 5. Smulaton Results of the Q-Adaptve UKF Durng smulatons for testng the Q-adaptve UKF algorthm, the process nose covarance matrx s changed temporarly for a perod n between 40 th and 45 th s. At frst ths change s smply formed by multplyng the covarance wth a constant and ncreasng the covarance for ths tme nterval. As s apparent from Fgs. 2 and 3, the proposed Q-adaptve UKF s not affected sgnfcantly by the change n the process nose covarance, and ensures accurate estmates for the whole process by decreasng the adaptve factor.

7 Adaptve Factor Fg. 2: Estmaton of u velocty term va Q-adaptve UKF n the case of temporary change n the process nose covarance. Subfgure (a) zooms to the ndcated part on the man fgure. Varaton of Adaptve Factor tme(sec) Fg.3: Behavor of adaptve factor when Q matrx ncreases. In Fg. 3, varaton of the adaptve factor for Q scalng s presented for the perod wthn the 40 th and 45 th seconds. As expected, t takes smaller values than so as to reduce the process nose covarance and then changes n a relevant way wth the changng characterstcs of the estmaton process. Another fact s the Q-adaptaton scheme works n a smlar manner when there s a decrease n the process nose covarance, whch may rsk the stablty of the algorthm, and ncreases the covarance to match wth the real fltraton error. In a case we decrease the Q matrx sgnfcantly (0-0 tmes smaller Q values than the optmal one) the adaptve algorthm reacts by ncreasng the adaptve factor and ensures both the estmaton accuracy and flter stablty (Fgs.4-5).

8 Adaptve Factor Fg. 4: Estmaton of x-poston va Q-adaptve UKF n the case of temporary change of the process nose covarance. Subfgure (a) zooms to the ndcated part on the man fgure. 4 x 03 Varaton of Adaptve Factor tme(sec) Fg. 5: Behavor of adaptve factor n case Q matrx decreases. 6. Conclusons here are many dffcultes for estmatng the dynamcs of an autonomous underwater vehcle n operaton. One of these dffcultes s the changes n the envronment (process nose) that may deterorate the estmator s accuracy. In such case the estmator, whch s mplemented onboard the AUV, must be adapted to make t robust aganst envronment changes and provde accurate estmaton results even n ths case. In ths study, we adapted the UKF algorthm that we use for estmatng the dynamcs of an AUV and propose process nose covarance matrx adaptaton (Q-adaptaton) for the UKF algorthm. he man am s to make the algorthm adaptve aganst the changes n the process nose covarance. he Adaptve UKF estmates the AUVs dynamcs. he numercal results confrm that the adaptve UKF gves better results than the regular UKF n the case of envronment changes.

9 he results of ths study show that t s not possble to get precse estmaton results by the regular UKF f the model for the process nose covarance dsagrees wth the real value as a result of some change. An adaptve scheme to adjust the process nose covarance matrx n response to the changng envronment s proposed. he presented adaptve UKF algorthm s not affected by those changes and secures ts good estmaton characterstc all the tme. he addtonal computatonal load of the adaptaton s neglgble. References [] A. Lammas, K. Sammut and F. He, 6-DoF Navgaton Systems for Autonomous Underwater Vehcles, Moble Robots Navgaton, Alejandra Barrera, Ed. Inech Publshng, 200, pp [2] L. Stutters, H. Lu, C. ltman and D. J. Brown, Navgaton echnologes for Autonomous Underwater Vehcles, IEEE rans. Syst., Man, Cyber.: Appl. Rev., vol. 38, No. 4, pp , [3] S. J. Juler, J. K. Uhlmann and H. F. Durrant-Whyte, A new approach for flterng nonlnear systems, Proc. of Amercan Control Conference, Seattle, USA, 995, vol. 3, pp [4] S. J. Juler, J. K. Uhlmann and H. F. Durrant-Whyte, A new method for the nonlnear transformaton of means and covarances n flters and estmators, IEEE rans. Autom. Control, vol. 45, pp , [5] J. Km, K. Km, H. S.Cho, W. Seong and K-Y Lee, Estmaton of Hydrodynamc Coeffcents for an AUV Usng Nonlnear Observers, IEEE J. Ocean. Eng., vol. 27, No. 4, pp , [6] R. K., Mehra, On the Identfcaton of Varances and Adaptve Flterng, IEEE rans. Automatc Cont., vol. 5, No.2, pp.75-84, 970. [7] R. K., Mehra, Approaches to Adaptve Flterng, IEEE rans. on Automatc Cont., vol. 7, No.0, pp , 972. [8] C. Hajyev, Adaptve fltraton algorthm wth the flter gan correcton appled to ntegrated INS/Radar altmeter, Proc. IMechE, Part G: J. Aerosp. Eng., vol. 22, pp , [9] C. Hajyev and H. E. Soken, Robust Estmaton of UAV Dynamcs n the Presence of Measurement Faults, J. Aerosp. Eng., vol. 25, No., pp , 202. [0] C. H. Kang, S. Y. Km and C. G. Park, A GNSS nterference dentfcaton and trackng based on adaptve fadng Kalman flter, Proc. 9th IFAC World Congress, 204, vol.9, pp [] C. Hajyev and H. E. Soken, Robust adaptve unscented Kalman flter for atttude estmaton of pco satelltes, Internatonal Journal of Adaptve Control and Sgnal Processng, vol. 28, No. 2, pp , 204. [2] K. Lu, J. L, W. Guo, P. Zhu and X. Wang Navgaton of a class of underwater vehcle based on adaptve unscented Kalman flter algorthm, J. Cent. South Unv, vol. 2, pp , 204. [3] W. L, Y. L, S. Ren and X. Feng rackng and underwater manoeuvrng target usng an adaptve Kalman flter, Proc. IEEE encon, pp.-4, 203. [4] Y. Ln, K. Lu and X. Wang, An adaptve square-root unscented Kalman flter for underwater vehcle navgaton, Proc. of IEEE Int. Conf. on Mechatroncs and Automaton, pp [5]. Prestero, Verfcaton of a Sx-Degree of Freedom Smulaton Model for the REMUS Autonomous Underwater Vehcle, M.Sc. hess n Ocean and Mechancal Engneerng, MI, USA, 200. [6] M. Dnc and C. Hajyev, Integraton of navgaton systems for autonomous underwater vehcles, J.Marne Engneerng & echnology, vol. 4, no., pp.32-43, 205.