Vol. 34, No. 1 ACTA AUTOMATICA SINICA January, 2008 An Adaptive UKF Algorith for the State and Paraeter Estiations of a Mobile Robot SONG Qi 1, 2 HAN Jian-Da 1 Abstract For iproving the estiation accuracy and the convergence speed of the unscented Kalan filter UKF, a novel adaptive filter ethod is proposed. The error between the covariance atrices of innovation easureents and their corresponding estiations/predictions is utilized as the cost function. On the basis of the MIT rule, an adaptive algorith is designed to update the covariance of the process uncertainties online by iniizing the cost function. The updated covariance is fed bac into the noral UKF. Such an adaptive echanis is intended to copensate the lac of a priori nowledge of the process uncertainty distribution and to iprove the perforance of UKF for the active state and paraeter estiations. The asyptotic properties of this adaptive UKF are discussed. Siulations are conducted using an oni-directional obile robot, and the results are copared th those obtained by noral UKF to deonstrate its effectiveness and advantage over the previous ethods. Key words Adaptive Unscented Kalan filter UKF, innovation, MIT rule, process covariance Autonoous control is a ey technology for autonoous systes dely used in areas such as satellite clusters, deepspace exploration, air-traffic control, and battlefield anageent th unanned systes. Most unanned systes are highly nonlinear, vary th tie, and are coupled; in addition, their operating conditions are dynaic, coplex, and unstructured, which represent the unpredictable uncertainties of the control syste. The issue of overcoing these uncertainties and achieving high perforance control is one of the ain concerns in the field of autonoous control. Robust and adaptive control ethods followed traditionally suffer fro several probles, including conservativeness, online convergence, and the coplications involved in their real-tie ipleentation. These probles necessitate the developent of a new control algorith that addresses the situation ore directly. To this end, autonoous control ethods on the basis of odel-reference have becoe the focus of research, and basic technology and online odeling ethod has attracted ore and ore research attention. Neural networs NN and NN-based self-learning were proposed as the ost effective approaches for the active odeling of an unanned vehicle in the 1990s [1 2]. However, the probles involved in NN, such as training data selection, online convergence, robustness, reliability, and realtie ipleentation, liit its application in real systes. In recent years, sequential estiation has becoe an iportant approach for online odeling and odel-reference control th encouraging achieveents [3]. The ost popular state estiator for nonlinear syste is the extended Kalan filter EKF [4]. Although dely used, EKFs have soe deficiencies, including the requireent of differentiability of the state dynaics as well as susceptibility to bias and divergence in the state estiates. Unscented Kalan filter UKF, on the contrary, uses the nonlinear odel directly instead of linearizing it [5]. The UKF has the sae level of coputational coplexity as that of EKF, both of which are thin the order OL 3. Since the nonlinear odels are used thout linearization, the UKF does not need to calculate Jacobians or Hessians, and can achieve Received Noveber 30, 2006; in revised for April 29, 2007 Supported by National High Technology Research and Developent Progra of China 863 Progra, Hi-Tech Research and Developent Progra of China 2003AA421020 1. Robotics Laboratory, Shenyang Institute of Autoation, Chinese Acadey of Sciences, Shenyang 110016, P. R. China 2. Departent of Auto-control, Shenyang Institute of Aeronautical Engineering, Shenyang 110136, P. R. China DOI: 10.3724/SP.J.1004.2008.00072 second-order accuracy, whereas the accuracy of the EKF is of the first order. However, since UKF is th in the fraewor of the Kalan filter, it can only achieve a good perforance under certain assuptions about the syste odeling. But in practice, the assuptions are usually not totally satisfied, and the perforance of the filter ight be seriously downgraded fro the theoretical perforance or could even diverge. To avoid these probles, an adaptive filter ay be applied, which autoatically tunes the filter paraeter to adapt insufficiently nown a priori filter statistics. There have been any investigations in the area of adaptive filter. Maybec [6] used a axiu-lielihood estiator for designing an adaptive filter that could estiate the syste-error covariance atrix. Lee and Alfriend [7] odified the Maybec s ethods by introducing a ndow-scale factor. The new autoated adaptive algoriths are integrated into the UKF and can be applied to the nonlinear syste. One disadvantage of the algorith is that it is not very robust nuerically. Loebis et al. [8] presented an adaptive EKF ethod, which adjusts the easureentnoise-covariance atrix, eploying the principles of fuzzy logic. However, in practice, it is always difficult to deterine the values of the increent of covariance at each sapling tie. Mohae et al. [9] investigated the perforance of ultiple-odel-based adaptive Kalan filters for vehicle navigation using GPS. The ethod assues a nowledge of all the possible statuses beforehand. In this paper, an on-line innovation-based adaptive schee of UKF is proposed to adjust the noise covariance. The filter paraeter is tuned by using an MIT adaptation rule that iniizes the cost function of the innovation sequence. The asyptotic properties of the proposed adaptive UKF are discussed. Extensive siulations are conducted th respect to the dynaics of an onidirectional obile robot. Estiation accuracy is significantly iproved th the adaptive approache copared to the conventional UKF. 1 Standard UKF In this section, the principle of classical UKF is suarized. Consider the general discrete nonlinear syste: { x+1 f x, u + w y h x + v 1
No. 1 SONG Qi and HAN Jian-Da.: An Adaptive UKF Algorith for the State 73 where x R n is the state vector, u R r is the nown input vector, y R is the output vector at tie. w and v are, respectively, the disturbance and sensor noise vector, which are assued to be Gaussian white noise th zero ean. The UKF estiation can be expressed as explained in the follong section. Initialization x 0 E [x 0] P 0 E [x 0 x 0 x 0 x 0 T] 2 Siga points calculation and tie update χ 1 [ x 1, x 1 ± ] n + λ χ 1 f χ 1 x 1 2n χ i, 1 P 1 2n χ c i, 1 x 1 T χ i, 1 x 1 + Q χ 1 [ x 1, x 1 ± ] n + λ P 1 γ 1 h χ 1 ȳy 1 2n γ i, 1 where w0 λ n + λ w0 c λ n + λ + n α 2 + β c 1 i 1,, 2n 2 n + λ λ n α 2 1 Measureent update 2n Pȳy ȳy P x ȳy c 2n c γ i, 1 ȳy 1 T γ i, 1 ȳy 1 + R χ i, 1 x 1 γ i, 1 ȳy 1 T K P x ȳy ȳy ȳy P P 1 K Pȳy ȳy K T x x 1 + K y ȳy 1 where the variables are defined as follows: {w i} is a set of scalar weights, and n is the state diension; the paraeter α deterines the spread of the siga points around x and is usually set to 1e 4 α 1. The constant β is used to incorporate part of the prior nowledge of the distribution of x, and for Gaussian distributions, β2 is optial. Q and R are the disturbance and sensor-noise covariance, respectively. The linear algebra operation of adding a colun to a atrix, i.e., A ± z, is defined as the addition of the vector to each colun of the atrix. 3 4 5 2 Adaptive UKF 2.1 Adaptive paraeter There are six paraeters in UKF, which are the initial state x 0, initial covariance P 0, process-noise covariance Q, easureent-noise covariance R, and unscented transfor UT paraeters α and β. The influence of the initial state and covariance ll becoe asyptotically negligible as ore and ore data are processed. The values of α and β, which can only affect the higher order of the nonlinear syste, have little ipact on iproving the estiate accuracy of the UKF. As a priori nowledge, the covariance atrices Q and R are ost iportant to the perforance and stability of the UKF. If R and/or Q is too sall at the beginning of the estiation process, the uncertainty tube around the true value ll probably tighten and a biased solution ight result if R and/or Q is too large, filter divergence, in the statistical sense, could occur. Additionally, insufficiently nown a priori statistics ll, in any cases, lead to an inadequate estiation of the wea observable coponents in the filter. Therefore, in any adaptive filtering algoriths, the covariance atrices R and Q are the ain paraeters that need to be tuned online. In principle, an adaptive filter can estiate both R and Q. However, adaptive filtering algoriths that try to update both the observational noise and the syste noise are not robust, since it is not easy to distinguish between errors in R and Q [10]. The easureent-noise statistics are relatively well nown copared to the syste-odel error. In this study, the adaptive estiation of the processnoise covariance Q is considered. Usually, the process-noise covariance Q is a diagonal atrix. Therefore, the estiation of Q can be siplified as the estiation of its diagonal eleents. 2.2 Cost function An adaptive filter forulation ais to tacle the proble of iperfect a priori inforation and to achieve a significant iproveent in perforance over the fixed filter through the filter-learning process based on the innovation sequence. Most innovation-based adaptive filter ethods have been developed to iniize the tie-averaged innovation covariance. Even though a iniu true innovation covariance ay be obtained th this criterion, this covariance could be copletely different fro the one coputed by the filter in [11]. Therefore, a recursive algorith to iniize the difference between the filter-coputed and the actual innovation covariance is forulated. The tie-averaged innovation covariance is used as an approxiation to the actual one: S 1 N i N+1 v v T 6 where N is the size of the estiation ndow. v is the innovation and can be written as v y ȳy 1 7 where y and ȳy 1 are the real easureent received by the filter and its estiated predicted value, respectively. Fro the easureent update 5 of the standard UKF, the filter-coputed innovation covariance can be obtained as follows:
74 ACTA AUTOMATICA SINICA Vol. 34 Ŝ 2n w c i γ i, 1 ȳy 1 γ i, 1 ȳy 1 T + R 8 Then, the criterion function for adaptive UKF is to iniize V tr [ ] 2 S 2 tr S Ŝ 9 S is ore sensitive to the changes in the syste than to the actual innovation covariance alone [11]. 2.3 Adaptive law The traditional MIT rule is used in this section to derive the adaptive law. With the MIT rule, the paraeters can be adjusted in the negative gradient direction of the criterion function, i.e., q η V 10 where q is the -th diagonal eleent of the process-noise atrix at tie. η is the tuning rate that deterines the convergence speed, which is assued to satisfy the classic stochastic approxiation conditions η 0, η, η 2 < 11 2.4 Adaptive UKF Equation 10 leads to the follong recursive schee: q q 1 η V T 12 where T is the sapling tie or a constant tie period. This schee can be incorporated into the UKF equations to update Q. In order to calculate 11, the derivative of V needs to be calculated. Fro 9, we have where V [ tr S 2] S 2 tr 13 S S tr S + S S S Ŝ S Ŝ 14 Fro 6 and 7, the equation needed for the first ter is obtained as follows: S 1 v v T v T N + v i N+1 1 ȳy 1 T y N ȳy 1 i N+1 ȳy T 1 y ȳy 1 15 and the second ter can be obtained fro 8 as Ŝ 2n c [ ȳy 1 γ i, 1 ȳy 1 ȳy T 1 T γ i, 1 ȳy 1 ] 16 To ipleent 14 and 15, ȳy 1 / is required. This needs the derivative of the filter equations. For UKF, a recursive algorith for the gradient of innovation vector can be forulated as follows: Initialization x 0 0 P 0 0 Derivative of siga points 17 χ i, 1 x 1 + P 1 n + λ i 1,, n i χ i, 1 x 1 P 1 n + λ q i n i n + 1,, 2n 18 Derivative propagation χ i, 1 x 1 P 1 χ i, 1 χ i, 1 γ i, 1 f x χ i, 1 xχi, 1 2n χ i, 1 q 2n c q [ χ i, 1 x 1 χ i, 1 x 1 T + χ i, 1 x 1 χ i, 1 x T ] 1 + Q + P 1 n + λ x 1 x 1 n + λ h x χ i, 1 xχi, 1 Gradient of prediction ȳy 1 Derivative updates 2n P 1 γ i, 1 i i 1,, n i n i n + 1,, 2n 19 20
No. 1 SONG Qi and HAN Jian-Da.: An Adaptive UKF Algorith for the State 75 P x ȳy 2n [ χi, 1 c q x 1 T γ i, 1 ȳy 1 + χ i, 1 x 1 γi, 1 ȳy T ] 1 Pȳy ȳy 2n [ γi, 1 c q ȳy 1 T γ i, 1 ȳy 1 + γ i, 1 ȳy 1 γi, 1 ȳy T ] 1 ȳy ȳy K P x Pȳy ȳy ȳy ȳy ȳy ȳy P x ȳy ȳy ȳy P x ȳy P 1 x 1 Pȳy ȳy ȳy ȳy K P T x P x ȳy K ȳy + K ȳy ȳy T y ȳy 1 K ȳy 1 21 q ḡq 26 Because ḡq is given by 22, Uq can be used as a Lyapunov function for the ODE. Therefore, follong the results of Ljung in [13], q ll converge to a local iniu of Uq, th a probability 1 as tends to infinity. 3 Siulation The siulations were carried out th respect to the dynaics of the 3-DOF oni-directional obile robot developed in the Shenyang Institute of Autoation See Fig. 1. 2.5 Asyptotic behavior In this section, the asyptotic properties of the proposed adaptive UKF are discussed as in [12]. First, assue that the UKF is stable and that x is uniforly bounded. Then, fro 2 5 and the adaptive UKF / 17 21, it can be deduced that x 1 and x 1 q are stable too. Since η T is sall, the forcing ter η T V / in the adaptive UKF algorith is sall too. Consequently, / q ll change slowly. If the UKF and x 1 q are stable, the influence of the / old paraeter estiates on the current gradients x 1 q, and consequently, V / is less / relevant. Thus, q in the UKF equation and x 1 q can be approxiated to soe noinal constant/ value q 0. The solutions of the UKF equation and x 1 q ll be asyptotically / close to the steady-state solution x 1 and x 1 q th q q 0. Therefore, the estiate updating equation ll approxiately coincide th q q 1 η V T 22 q q 0 Fig. 1 3-DOF oni-directional obile robot 3.1 Siulation odel The dynaic odel of the obile robot is [14] : 2Mr 2 + 3i ni wẍ w + 3i 2 ni wẏ w ϕ w + 3i 2 ncẋ w i nrβ 1u 1 + 2u 2 cos ϕ w + β 2u 3 2Mr 2 + 3i ni wÿ w 3i 2 ni wẋ w ϕ w + 3i 2 ncẏ w i nrβ 3u 1 + 2u 2 sin ϕ w + β 4u 3 3i ni wl 2 + I vr 2 ϕ w + 3i 2 ncl 2 ϕ w i nrl u 1 u 2 u 3 27 / If assuing that the syste x 1 and x 1 q is stable, its solutions are stationary. Then, in view of the law of large nubers, V / ll approach to where ḡ q 0 E V / U/ q q 0 23 Uq 0 EV E [ tr S 2 q 0 ] 24 Up to soe rando error, the equation becoes q q 1 η ḡq 0 T 25 Thus, q should asyptotically follow the trajectories of the follong ordinary differential equation ODE: β 1 3 sin ϕ w cos ϕ w β 2 3 sin ϕ w cos ϕ w β 3 3 cos ϕ w sin ϕ w β 4 3 cos ϕ w sin ϕ w 28 where x w, y w, and φ w represent the displaceents in the x and y directions and the rotation, respectively, u 1, u 2, and u 3 are the actuated torques on each joint. Other paraeters of 27 and 28, and their initial values in the siulation are listed in Table 1.
76 ACTA AUTOMATICA SINICA Vol. 34 Table 1 Robot Paraeters Sybols Physical eanings Values in siulation c Friction coefficient 0.0009 g 2 /s I w Inertia on otor axis 0.0036 g 2 M Mass 120g I v Inertia 45 g 2 r Wheel radius 0.06 L Centroid wheel distance 0.273 i n Motor gear ratio 15 The state and the easureent vectors are selected as { x [x w, y w, ϕ w, ẋ w, ẏ w, ϕ w] T y [ẋ w, ẏ w, ϕ w] T 29 Assue that the initial state of the syste is x T 0 0 and the sapling interval is T 0.01 s. The easureents are corrupted by zero ean-additive white noise th covariance R T diag { 10 8, 10 8, 10 8}. The UKF paraeters are ˆx 0 x T 0 ˆP 0 diag { 10 8, 10 8, 10 8, 10 8, 10 8, 10 8} R R T α 1 and β 1 3.2 The changes of process noise The estiation accuracy of the adaptive UKF th respect to the changes of the process-noise statistics is tested. The change of the true process-noise intensity is assued as Q T 0 diag { 10 12, 10 12, 10 12, 10 8, 10 8, 10 8} t < 10 s Q T diag { 10 10, 10 10, 10 10, 10 6, 10 6, 10 6} t 10 s 30 In UKF, the prior nowledge of the process noise covariance is selected as Q Q T 0. The velocity-estiation errors of the classical UKF, and the adaptive UKF, under the sae condition of the processnoise intensity change, are illustrated in Fig. 2. As can be seen, under incorrect noise inforation, the classical UKF can not produce optial estiates due to the violation of the optiality conditions. On the contrary, the estiation errors in the adaptive UKF are quicly overcoe and are alost the sae as their previous size. 3.3 The changes of the paraeters In this section, the perforance of the adaptive UKF for paraeter estiations is validated. Here, the UKF is used for the online estiation of both otion states and dynaic paraeters of the obile robot. Such an active estiation is further incorporated into a classical inverse dynaic control IDC. This is intended to ae the robot autonoously adaptive to its internal uncertainties, i.e., to achieve a robust tracing perforance a Standard UKF Fig. 2 b Adaptive UKF State-estiation errors th the tie-varying process noise
No. 1 SONG Qi and HAN Jian-Da.: An Adaptive UKF Algorith for the State 77 for the tie-varying unnown changes in the vehicle dynaics. The structure of the controller is shown in Fig. 3. The UKF estiations are fed to both the inner and outer loops. For the outer loop, clean states are provided to the classic PD control, whereas for the inner loop, the estiated paraeters are provided to the inverse dynaic odel enabling it to adaptively trac the variations of the actual dynaics. Other paraeters of the UKF are the sae as those entioned in Section 2. 1. The paraeter-estiate results are shown in Fig. 4, fro which it can be inferred that the standard UKF th a fixed-value noise covariance cannot trac the paraeter change due to the lower pseudonoise intensity, which ll not provide enough driving power to the paraeter estiate. But for the adaptive UKF, the intensity of the pseudonoise ll increase during the paraeter change by the adaptive paraeter update. This can accelerate the convergence of the paraeter estiates and ae the UKF react quicly and trac the abrupt change successfully, after a short period 3 seconds of adaptation. Fig. 5 illustrates the perforance coparison of the standard UKF and the adaptive UKF th respect to the velocity estiate errors due to the paraeter change. The tracing errors of the standard UKF are uch ore significant than those by the adaptive UKF. Fig. 3 Active odel-enhanced IDC In the follong paragraphs, the perforance of the adaptive UKF-based and the conventional UKF-based controller are copared. The true values of the friction coefficient and the otor axis inertia are designed to change according to { c c0, I w I w0 t < 10 s 31 c c 0 + c s, I w I w0 + I ws t 10 s where the constant change value in 10s is c s 0.0031 g 2 /s and I ws0.0164 g 2. The state vector is subject to zero ean-additive white noise th a covariance: Q T diag { 10 14, 10 14, 10 14, 10 10, 10 10, 10 10} Other conditions of the syste are the sae as those in Section 2. 1. So as to estiate the paraeters, the joint estiation is used, which treats the paraeter vector as a dynaical variable and siply appends it onto the true state vector. Since the dynaics of paraeter are unnown, the paraeter can be assued to be a noncorrelated rando drift vector and odeled by θ θ 1 + w θ 32 where w θ is the Gaussian white noise th zero ean, called the pseudonoise. The pseudonoised is very iportant for the tie-varying paraeter estiation [15]. If the pseudonoise is too sall, it ll not have enough strength to drive the UKF to trac the paraeter change. However, if the pseudonoise is too large, an uncertainty ll be introduced, and a biased estiate ll be produced. In short, properly adjusting the pseudonoise covariance ll iprove the convergence rate of the UKF and lead to better tracing of the tie-varying paraeter. In the siulation, the UKF paraeters are designed as ĉ 0 c 0, Î w0 I w0 ˆP 0 θ diag { 10 8, 10 7} Q θ diag { 10 18, 10 17} Q Q T a Standard UKF
78 ACTA AUTOMATICA SINICA Vol. 34 b Adaptive UKF Fig. 4 Paraeter estiation b Adaptive UKF State-estiation errors th the tie-varying parae- Fig. 5 ters 4 Conclusion a Standard UKF In this paper, the estiation errors of UKF th unnown noise statistics were analyzed. A novel adaptive UKF was proposed on the basis of the innovation covariance atrix and the MIT adaptive law. In addition, a recursive algorith has been forulated. Siulations on the dynaics of the oni-directional obile robot were conducted to verify the proposed schee. Results show that the adaptive UKF outperfors the conventional UKF in ters of fast convergence and estiation accuracy by tuning the process-noise covariance atrix Q.
No. 1 SONG Qi and HAN Jian-Da.: An Adaptive UKF Algorith for the State 79 References 1 Pesonen U, Stec J, Rohsaz K. Adaptive neural networ inverse controller for general aviation safety. Journal of Guidance, Control, and Dynaics, 2004, 273: 434 443 2 Leitner J, Calise A, Prasad J V R. Analysis of adaptive neural networs for helicopter flight control. Journal of Guidance, Control, and Dynaics, 1997, 205: 972 979 3 Hayin S, defreitas N. Special issue on sequential state estiation. Proceedings of the IEEE, 2004, 923: 399 400 4 Lerro D, Bar-Shalo Y. Tracing th debiased consistent converted easureents versus EKF. IEEE Transactions on Aerospace and Electronic Systes, 1993, 293: 1015 1022 5 Julier S J, Uhlann J K. Unscented filtering and nonlinear estiation. Proceedings of the IEEE, 2004, 923: 401 422 6 Maybec P S. Stochastic Models, Estiation and Control. New Yor: Acadeic Press, 1979 7 Lee D J, Alfriend K T. Adaptive siga point filtering for state and paraeter estiation. In: AIAA/AAS Astrodynaics Specialist Conference and Exhibit. Rhode Island: 2004. 1 20 8 Loebis D, Sutton R, Chudley J, Naee W. Adaptive tuning of a Kalan filter via fuzzy logic for an intelligent AUV navigation syste. Control Engineering Practice, 2004, 1212: 1531 1539 9 Mohaed A H, Schwarz K P. Adaptive Kalan filtering for INS/GPS. Journal of Geodesy, 1999, 734: 193 203 10 Blanchet I, Franignoul C, Cane M. A coparison of adaptive Kalan filters for a tropical Pacific ocean odel. Monthly Weather Review, 1997, 1251: 40 58 11 Garcia V J. Deterination of noise covariances for extended Kalan filter paraeter estiators to account for odeling errors [Ph. D. dissertation], University of Cincinnati, 1997 12 El-Fattah Y M. Recursive self-tuning algorith for adaptive Kalan filtering. IEE Proceedings, 1983, 1306: 341 344 13 Ljung L. Analysis of recursive stochastic algoriths. IEEE Transactions on Autoatic Control, 1977, 224: 551 575 14 Song Y X. Study on trajectory tracing control of obile robot th orthogonal wheeled asseblies [Ph. D. dissertation], Chinese Acadey of Sciences, 2002 15 Hill B K, Waler B K. Approxiate effect of paraeter pseudonoise intensity on rate of covariance for EKF paraeter estiators. In: Proceedings of the 30th Conference on Decision and Control. Brighton, England: IEEE, 1991. 1690 1697 SONG Qi Lecturer at the Shenyang Institute of Aeronautical Engineering. She received her Ph. D. degree fro the Shenyang Institute of Autoation, Chinese Acadey of Sciences in 2007. Her research interest covers control techniques for robotic systes, estiation techniques for active odel, and fault detection and control. Corresponding author of this paper. E-ail: songqi@163.co HAN Jian-Da Professor at the Shenyang Institute of Autoation, Chinese Acadey of Sciences. His research interest covers robust control, estiation techniques for active odel, fault detection and control, and on-board path planning for autonoous syste. E-ail: jdhan@sia.cn