Trajectory Estiation of a Satellite Launch Vehicle Using Unscented Kalan Filter fro Noisy Radar Measureents R.Varaprasad S.V. Bhaskara Rao D.Narayana Rao V. Seshagiri Rao Range Operations, Satish Dhawan Space Centre SHAR, ISRO, Sriharikota- 524 124 Departent of Physics, Sri Venkateswara University, Tirupati- 517501 Forer Director, National Atospheric Research laboratory, Gadanki 517112 Range Operations, Satish Dhawan Space Centre SHAR, ISRO, Sriharikota- 524 124 Abstract Accurate state estiation of a launch vehicle is iperative at any launch base for range safety and trajectory onitoring for assessing the success of the ission. Linear Kalan filter followed by extended Kalan filter were the ubiquitous digital filters used for optial estiation of state for linear / quasi-linear systes. Of late unscented Kalan filter is proposed for state estiation of any non-linear syste. This ethodology consists in propagating the ean and the variance through non-linear transforation using statistically chosen siga points. This paper presents the application of the unscented Kalan filter for estiating the trajectory of a satellite launch vehicle fro noisy radar easureents, the ease of convergence and the accuracy achieved. 1. Introduction For over four decades linear or extended Kalan filter has been the de facto standard in the field of tracking and control applications [1,6]. The ease these filters offer for their application to real tie scenario, their siplicity and robustness are soe of the ost copelling factors for their ubiquitous applications. Of late a new filter naely Unscented Kalan filter [USKF] has outperfored linear Kalan filter and extended Kalan filter in ters of robustness, nuerical stability and better accuracy [2,7]. State estiation consists of estiating the probability density function for the state of the process. Estiation of state involves predicting the state based on current state and then correcting the prediction using easureents. The ost pertinent proble in these filter probles is representation and aintenance of uncertainty [3]. In the linear Kalan filter, the first two oents of the probability distribution naely, ean and variance are aintained. If these two oents of probability distribution are available, Kalan filter yields the best state estiate in the tracking and control applications [7] based on the surise that the distribution is gaussian and application of a linear operator to gaussian distribution is also gaussian. Extended Kalan Filter [EKF] was proposed for state estiation of non-linear systes. It siply calls for linearization of non-linear systes wherever applicable. Assuing that size of errors are sall in each tie step, the state dynaical odel is expanded as a Taylor series about the estiate. The second and higher order ters are neglected and the state prediction propagates through non-linear equations whereas the state errors propagate through linear systes. So the deficiency in extended Kalan filter is its inability to accurately predict the syste state, observations and associated covariance atrices when the syste and/or observations are non-linear. Approxiating non-linear transforations with linearized ones results in divergent state for realistic observations and process odels[4]. So an alternate ethod without using the linearization techniques is proposed by Sion Julier and Jeffrey Uhlann to calculate these statistics. 655
2. Unscented transfor and siga points In estiation arena, if X is an n diensional rando variable with ean X and covariance P xx and it is related to n diensional rando variable Y by a non-linear transforation Ψ as Y =Ψ(X) 1) then finding its ean Y and the covariance P YY is the centrality of the proble. In this process, one coes across two non-linear transforations i.e predicting the state for the next tie step and predicting the observation. Sion Julier and Jeffrey Uhlann [5,7] intuitively surised that with fixed nuber of paraeters, it should be easier to approxiate a gaussian distribution than it is to approxiate an arbitrary non-linear function or transforation. That is, one has to find a set of paraeters which can capture the ean and variance inforation and at the sae tie allow theselves to be propagated through non-linear transforations. A discrete distribution of points can be generated with sae first and second oents and can be approxiately transfored. With this backdrop Sion Julier and Jeffrey Uhlann proposed the ethod of unscented transfor for state estiation of non-linear systes. Using this transfor a gaussian variable of n diensions is represented by 2n+1 siga points. These siga points are selected based on the concepts of atrix square root and covariance and they have the sae first two oents of the gaussian distribution. Sion Julier and Uhlann ade use of this concept of unscented transfor in their filter forulation for state estiation. 2.1. Accuracy of Siga point approach Let X be an diensional rando variable with ean X and variance P xx. It can be approxiated by a set of 2+1 nuber of siga points χ i. They are χ 0 = X W 0 = K/ + k 2) χ i, = X + ( + k P xx ) i W i = 1/2 + k χ i+ = X - ( + k P xx ) i W i+ = 1/2 + k where + k P xx ) i is the i th row or colun of the square root of atrix + k P xx, k R and W i s are the weights associated with each siga point. Each siga point is propagated through the non-linear function to arrive at the transfored siga points ξ i = f(χ i ) 3) and the ean and covariance of the transfored siga points are coputed as 2 y = 0 W i ξ i 4) 2 P yy = 0 W i [ ξ i -y ] [ ξ i -y ] T 5) The coputed value of ean and covariance of transfored siga points are accurate up to second order because the ean and covariance of X is accurate up to second order. That is how the accuracy is aintained. Its perforance is equal to the truncated second order gaussian filter without the derivation of either jacobians or hessians. Perforance of the unscented Kalan filter is equivalent to that of linear Kalan filter for linear systes but fits elegantly for non-linear systes without the linearization steps. Also it achieves better accuracy copared to the extended Kalan filter even with systes of high nonlinearity. The ease of ipleentation and ore accurate state estiation ake this filter preferred copared to linear Kalan filter and extended Kalan filter. 3. Ipleentation of unscented Kalan filter Let us look at a non-linear syste represented by the following discrete tie equations x k = f(x k-1, w k-1, u k-1 ) (6) y k = h(x k, u k ) + ʋ k (7) Where x R n x is the syste state, w R n w is the process noise, ʋ R n ʋ is the observation noise. Here u is the input, y are the noisy observations. The non-linear functions f and h can be continuous or discrete. k is the discrete tie. 3.1. The augented state Sion Julier introduced the concept of augenting the state with noise sources[7]. This technique enables us to treat noise in a non-linear fashion thereby taking care of non-gaussian and nonadditive noises. Siga points are coputed using the augented state. In this process the non-linear effects of noise are also captured with the sae aount of accuracy as that of the original state. The state vector is augented to L where L is the su total of the diension of the original state, process noise and observation noise. The covariance atrices are also siilarly augented to a L 2 atrix. Now the augented state and covariance atrix are represented as a X K-1 = X K1 O w Ov and P K-1 a = E {(X K-1 a - X K-1 a ) (X K-1 a - X K-1 a ) T } 656
where P K-1 a = P k1 0 0 0 Q k1 0 0 0 R k1 Creation of 2L+1 nuber of siga points is carried out in a way to accurately capture the ean and the covariance of the augented state. The siga points are represented by the χ a atrix whose coluns are populated as χ a a 0,K-1 = X K1 for i=0 (9) χ a a i, K-1 = X K1 + ( L + λ P K1 ) i for i= 1,2 L χ a a i, K-1 = X K1 - ( L + λ P K1 ) i-l for i=l+1..2l Here subscript i stands for the i th colun of the square root of the covariance atrix [8]. Paraeter (L + λ) stands for the spread of the siga points and it should be defined in the interval [ 0-1]. To avoid any non local effects, the authors defined it as a low value [9]. The siga point atrix χ a i,k-1 can be viewed as coposing state χ x i,k-1 sapled process noise χ w i,k-1 and sapled easureent noise χ ʋ i,k-1. Each siga point is also weighted. These weights are obtained fro the coparison of the oents of the siga points with a Taylor series expansion of the odel. The weights for the ean and the covariance estiates are W 0 = λ/(l + λ) for i = 0 10) c W 0 = λ/(l + λ) + 1 - α 2 + β for i = 0 W i = W c = 1/2(L + λ) for i = 1,2,.. 2L Here and c denote the weights associated with the ean and the covariance of the state. α and β are the scaling paraeters. The choice of α and β deterines the accuracy of the higher order oents of distribution. α is defined as very low in the interval 0 < α < 1 signifying siga-point spread. β is used to incorporate prior knowledge of the distribution in the state prediction and update. These Siga points are propagated to arrive at the predicted state through the state and easureent odels and to carry out their weighted averages. χ a i, K/K-1 = f(χ x i, K-1, u K-1, χ w i, K-1 ) 11) X K = 2L i=0 W i χ x i, K/K-1 2L P K = i=0 W c i [ χ x i,k/k-1- X K ] [ χ x i,k/k-1- X K ] T Z i,k/k-1 = h(χ x i, K/K-1,χ v i, K-1 ) 2L z K = i=0 W i Z i,k/k-1 Here X K and z K are the weighted averages representing predicted state and predicted easureents respectively. These predictions are used to copute easureent covariances and stateeasureent cross correlation atrices which in turn are used to deterine Kalan gain K K. 2L P zz = i=0 W c i [Z i,k/k-1 - zk ] [Z i,k/k-1 - zk ] T (12) 2L P xz = i=0 W c i [ χ x i,k/k-1 - X K ] [Z i,k/k-1 - zk ] T K K = P xz P zz -1 Once the Kalan gain K K is coputed, filtered state and its covariance are coputed as X K/K = X K + K K (z K, - z K ) T P K/K = P K/K-1 - K K P zz K K Here z K is the new easureent. 3.2. Unscented Kalan filter with additive noise Authors of this paper have assued that the process noise and easureents noise are zero ean gaussian and additive for the tracking proble[1]. For this case the state of the syste need not be augented with state pertaining to noise processes. A set of +1 nuber of siga points are defined for a state of diension N. The siga points at tie k are given by χ 0, K-1 = X K-1 for i=0 13) χ i, K-1 = X K-1 + ( N + λ P K1 ) i for i= 1,2 N χ i, K-1 = X K-1 - ( N + λ P K1 ) i-n for i=n+1. The weights of the ean and covariance are calculated as before W 0 = λ/(n + λ) for i=0 14) c W 0 = λ/(n + λ) + 1- α 2 + β for i=0 W i = W c = 1/2(N + λ) for i=1,2,.. The siga points are propagated through the nonlinear transforation f(x) to arrive at the new siga points χ i, K/K-1. χ i, K/K-1 = f(χ i, K-1,u K-1 ) 15) Weighted average of the siga points gives the predicted state and its covariance as X K P K Q K+1 = i=0 W i χ i, K/K-1 = i=0 W c i [ χ i,k/k-1 - X K ] [ χ i,k/k-1 - X K ] T + Q K+1 stands for the covariance atrix of process noise. To account for the process noise, the siga points are refreshed as follows ξ 0, K/K-1 = X K for i = 0 16) ξ i, K/K-1 = X K + ( N + λ P K ) i for i = 1,2 N ξ i,k/k-1 = X K - ( N + λ P K ) i-n for i = N+1. The new siga points undergo the non-linear easureent function h(x), yielding another set of 657
siga points γ i, K. Then their ean and covariance are used to copute predicted easureent z K, easureent covariance P zz, and state-easureent cross correlation P xz, γ i, K/K-1 = h(ξ i, K/K-1 ) 17) z K = i=0 W i γ i, K/K-1 P zz, = i=0 W i c [γ i,k/k-1 - z K ] [γ i, K/K-1 - z K ] T + R K P xz, = i=0 W c i [ξ i K/K-1- - X K ] [γ i,k/k-1 - z K ] T Here R K stands for the covariance of easureent noise. Now the arrival of new easureent Z K is considered. Kalan gain and updates of the state are coputed as follows. K K = P xz,k P zz,k -1 18) X K/K = X K + K K (z K - z K ) T P K/K = P K - K K P zz K K These filter equations are ipleented to process the noisy radar data. Results are copared with the noinal data to assess the efficacy of the ethodology. 4.0 Estiation of Trajectory of a typical Launch Vehicle Even though Kalan filter is an attractive tool in the estiation theory, the design of filter for optial estiate needs a difficult tuning of initial state, easureent noise and process noise covariances [10]. A siple and elegant state space odel is used to estiate the state of trajectory of a Polar Satellite Launch Vehicle [PSLV] using unscented Kalan filter. Perforance of the unscented Kalan filter for a typical trajectory is shown in the following Figure.2 to Figure.14. In the estiation odel the state space consists of nine variables naely relative position, velocity and acceleration of the target with respect to the radar. It is expressed in the topocentric rotating coordinate syste as [ X, X, X,Y, Y, Y, Z, Z, Z ] T The Topocentric rotating coordinate syste is defined as the radar as the origin and east, north and vertical unit vectors foring the triad [Figure.1]. The target easureents are slant range, aziuth and elevation of the target fro the radar. Noinal track data is generated at a sapling rate of 10 Hz. Rando noise is assued to be zero ean white Gaussian noise and is added to the noinal tracking data with the following specifications to arrive at siulated track data. The 1 siga noise in slant range, aziuth and elevation are σ r = 10 σ az = 0.2 il rad and σ el = 0.2 il rad. Siulated track data is generated with 1 siga noise for a duration of 500 s catering to the visibility of Sriharikota Range. The siulated data was used to generate the initial conditions of the state. Process noise for the state is obtained using the odel uncertainty estiation technique by processing the ideal state, a ethodology proposed by S.K. Pillai, S.S. Balakrishnan, V.Seshagiri Rao and N.Narasaiah [11]. The ideal state is generated using rigid body dynaics of the rocket as described by three diensional six degree of freedo (6 DOF) equations of otion. This concept is successfully used for the state estiation of the target using linear Kalan filter since 1980s at Sriharikota range. The authors have generated the odel copensation values in the cartesian frae and scaled the to take care of the dynaics of the vehicle around stage separations. These scale factors are arrived at after a nuber of siulation runs for different levels of dynaics of the vehicle. Syste odel used for tie update of the siga points is X K+ 1 = Φk X K 19) Where Φk = Here A is defined as A = 0 = A 0 0 0 A 0 0 0 A 1 T T2 2 0 1 T 0 0 1 0 0 0 0 0 0 0 0 0 20) Where T is the sapling period. The authors of this paper used the Cholesky decoposition to arrive at the square root of the covariance atrix. 5.0 Results and Discussion Figure.1 Topocentric coordinate syste The relative position and velocity of the target with respect to the radar are estiated using the syste odel and easureent odel as provided in Section 3.0. Authors have assued that the process noise and easureents noise are zero ean 658
Gaussian and additive for the tracking proble. Unscented scented Kalan filter with additive noise as described fro Equation 13 to Equation 20 are exercised to obtain the filtered estiate. The estiated state is used to arrive at the filtered easureents. Figures 2 to 4 provide the first differences of the raw easureents and the filtered easureents. The ean of their first differences provide the rate of the easureents and the excursions denote the noise in the easureents. It is candidly clear that the noise existent in the angles of raw easureents is around 0.2 il rad. It is shown in red color. The blue curve is the first difference of the filtered easureents. The excursions are very sall since the noise is alost filtered out. The filtered state is copared with the noinal state and the position and velocity errors are coputed. These errors in the state are copared against their ± 1 σ theoretical bounds to establish the consistency of the perforance of the filter. Consistent perforance of the unscented Kalan filter can be observed in the Figures 5 to 10. The errors in the position and velocity coponents i.e X and X, Y and Y, Z and Z are plotted against their 1 σ siga bounds obtained fro the filter s estiated covariance atrices. Figure 4. First differences in Elevation (Raw vs filtered) Figure 5. Error in X against its 1 siga bounds Figure 2. First differences in slant range (Raw vs filtered) Figure 6. Error in VX against its 1 siga bounds Figure 3. First differences in Aziuth (Raw vs filtered) Figure 7. Error in Y against its 1 siga bounds 659
It is observed that with the extended Kalan filter the position error is less than 10 and the velocity error less than 10 /s except at instants of sudden change in the dynaics such as stage burn out and separation events. Later both filters are exercised with tracking data of 3 σ noise and position and velocity errors are provided in the Figures. 13 &14 for coparison. The position and velocity errors are 10 and around 5 /s for unscented Kalan filter and 25 and 20 /s for extended Kalan filter respectively. Figure 8. Error in VY against its 1 siga bounds Figure 9. Error in Z against its 1 siga bounds Figure 11. Position and velocity errors fro Unscented Kalan Filter Figure 10. Error in VZ against its 1 siga bounds The fact that the errors run with their theoretical bounds confir the consistent behavior of the filter. The seeing divergence observed in Y fro T+325 s to T+365 s and divergence in Z fro T+350 s to T+380 s is well captured by the filter and the errors are within their theoretical liits afterwards. Figure 11. provides the position and velocity errors in the estiated state and they are within 5 and 5 /s respectively throughout the flight tie except at stage burn out or stage separation events. Authors have exercised the extended Kalan filter with the sae siulated track data and the accuracy of estiated state is provided in ters of position and velocity errors [Figure.12]. Figure 12. Position and velocity errors fro Extended Kalan Filter Figure 13. Position and velocity errors for USKF with easureents of 3 σ noise 660
Figure 14. Position and velocity errors for EKF with easureents of 3 σ noise Errors in velocity for extended Kalan filter are significantly large whereas they are benign in the case of unscented Kalan filter for trajectory estiation. The above analysis clearly shows that state obtained fro Unscented Kalan filter is ore accurate than that fro extended Kalan filter and it exhibits a consistent behavior for trajectory estiation using easureents with different levels of gaussian noise. 6.0 Conclusion In this paper an effort is ade to accurately estiate the trajectory of a typical PSLV launch vehicle fro noisy radar easureents using unscented Kalan filter and to copare it with extended Kalan filter. The state and the easureent odel are defined in non-linear state space and the filters are exercised over the noisy radar easureents. The accuracy of the filtered estiate is provided in ters of errors in position and velocity. Coplexity in filter forulation being the sae for unscented Kalan filter and extended Kalan filter, the perforance of the unscented Kalan filter is undoubtedly better than extended Kalan filter with an added advantage of ease of ipleentation without the usage of analytical derivatives such as jacobians or hessians. Perforance of the filter is consistent and the ipleentation is elegant. This ethodology can be extended for higher order non-linear systes also without losing any accuracy or stability. Also an adaptive estiate of the process noise can be studied and ipleented for a coplete on line application [12]. ACKNOWLEDGEMENTS The authors wish to thank Dr.MYS Prasad, Director, SDSC SHAR for granting perission to publish this work. Thanks are also due to Dr V.Adiurthy, Dean, IIST, Trivandru for his keen and constructive suggestions. Authors acknowledge the contributions of Real tie data Processing group and Flight Safety group of Range operations at SDSC SHAR References [1] Wan, E. A. and van der Merwe, R.,The Unscented Kalan Filter, in Kalan Filtering and Neural Networks (ed S. Haykin), John Wiley & Sons, Inc., New York, USA. doi: 10.1002/0471221546.ch7, 2002 [2] S.J. Julier,J.K.Uhlann and H Durant Whyte, A new approach for filtering nonlinear systes, Proceedings of Aerican Control conference, Pages 1628-1632, 1995 [3] Papoulis A, Probability, Rando variables and stochastic processes. McGraw- Hill, Inc. 1965 [4] S.J. Julier and J.K.Uhlann, A new extension of Kalan filter to non-linear systes, The 11 th syposiu of Aero space/ Defense sensing, siulation and controls, 1997 [5] Julier.S, Uhlann.J and H.F.Durrant Whyte, A New Method for the Nonlinear Transforation of Means and Covariances in filters and Estiators, IEEE Transactions on Autoatic control, vol. 45, pp. 477 482, 2004. [6] Paul Zarchan, and Howard Musoff, Fundaentals of Kalan filtering: A Practical Approach, 2 nd Edition, Vol 208, Progress in Aeronautics and Astronautics, 2005 [7] S.J. Julier and J.K.Uhlann, A general ethod for approxiating nonlinear transforations of probability distributions, Technical report, Departent of Engineering science, University of Oxford, 1996 [8] W.H. Press, S.A.Teulosky, W.T. Vetterling and B.P. Flannery, Nuerical recipes in C: The Art of Scientific coputing, Cabridge university press, 1992 [9] Ghanbarpour Asl and Portakdoust, UD covariance factorization for Unscented Kalan filter using sequential easureents update, World acadey of Science, Engineering and technology, 2007 [10] M.R. Ananthasayana, The philosophy, principles and practice of Kalan filter since ancient ties to the present, The 62 nd International Astronautical Congress, Cape Town, South Africa, October 2011, Paper No: IAC-11-E4.3.9 [11] S.K. Pillai, S.S. Balakrishnan, V.Seshagiri rao and N.Narasaiah, Model error estiation by Ideal state processing in Linear Kalan filter, Journal of Guidance, Control and Dynaics, Vol.15, No.3, pp 775-777, 1992 661
[12] Ohleyer.E.J., P.K.Menon and Jinwhan Ki, Tracking of spiral reentry vehicles with varying frequency using the Unscented Kalan filter, AIAA Guidance, navigation and Control conference, Toronto, Canada, 2010 662