Trajectory Estimation of a Satellite Launch Vehicle Using Unscented Kalman Filter from Noisy Radar Measurements

Trajectory Estimation of a Satellite Launch Vehicle Using Unscented Kalman Filter from Noisy Radar Measurements

R.Varaprasad S.V. Bhaskara Rao D.Narayana Rao V. Seshagiri Rao

Range Operations, Satish Dhawan Space Centre SHAR, ISRO, Sriharikota- 524 124

Department of Physics, Sri Venkateswara University, Tirupati- 517501

Former Director, National Atmospheric Research laboratory, Gadanki 517112

Range Operations, Satish Dhawan Space Centre SHAR, ISRO, Sriharikota- 524 124

Abstract

Accurate state estimation of a launch vehicle is imperative at any launch base for range safety and trajectory monitoring for assessing the success of the mission. Linear Kalman filter followed by extended Kalman filter were the ubiquitous digital filters used for optimal estimation of state for linear / quasi-linear systems. Of late unscented Kalman filter is proposed for state estimation of any non-linear system. This methodology consists in propagating the mean and the variance through non-linear transformation using statistically chosen sigma points. This paper presents the application of the unscented Kalman filter for estimating the trajectory of a satellite launch vehicle from noisy radar measurements, the ease of convergence and the accuracy achieved.

1. Introduction

   For over four decades linear or extended Kalman 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 time scenario, their simplicity and robustness are some of the most compelling factors for their ubiquitous applications. Of late a new filter namely Unscented Kalman filter [USKF] has outperformed linear Kalman filter and extended Kalman filter in terms of robustness, numerical stability and better accuracy [2,7] .

   State estimation consists of estimating the probability density function for the state of the process. Estimation of state involves predicting the state based on current state and then correcting the prediction using measurements. The most pertinent problem in these filter problems is representation and maintenance of uncertainty [3]. In the linear Kalman filter, the first two moments of the probability distribution namely, mean and variance are maintained. If these two moments of probability distribution are available, Kalman filter yields the best state estimate in the tracking and control applications

   [7] based on the surmise that the distribution is gaussian and application of a linear operator to gaussian distribution is also gaussian.

   Extended Kalman Filter [EKF] was proposed for state estimation of non-linear systems. It simply calls for linearization of non-linear systems wherever applicable. Assuming that size of errors are small in each time step, the state dynamical model is expanded as a Taylor series about the estimate. The second and higher order terms are neglected and the state prediction propagates through non-linear equations whereas the state errors propagate through linear systems. So the deficiency in extended Kalman filter is its inability to accurately predict the system state, observations and associated covariance matrices when the system and/or observations are non-linear. Approximating non-linear transformations with linearized ones results in divergent state for realistic observations and process models[4]. So an alternate method without using the linearization techniques is proposed by Simon Julier and Jeffrey Uhlmann to calculate these statistics.

2. Unscented transform and sigma points

   In estimation arena, if X is an n dimensional random variable with mean and covariance and it is related to n dimensional random variable Y by a non-linear transformation as

   Y =(X) 1)

   then finding its mean and the covariance PYY is the centrality of the problem. In this process, one comes across two non-linear transformations i.e predicting the state for the next time step and predicting the observation. Simon Julier and Jeffrey Uhlmann [5,7] intuitively surmised that with fixed number of parameters , it should be easier to approximate a gaussian distribution than it is to approximate an arbitrary non-linear function or transformation. That is, one has to find a set of parameters which can capture the mean and variance information and at the same time allow themselves to be propagated through non-linear transformations. A discrete distribution of points can be generated with same first and second moments and can be approximately transformed. With this backdrop Simon Julier and Jeffrey Uhlmann proposed the method of unscented transform for state estimation of non-linear systems. Using this transform a gaussian variable of n dimensions is represented by 2n+1 sigma points. These sigma points are selected based on the concepts of matrix square root and covariance and they have the same first two moments of the gaussian distribution. Simon Julier and Uhlmann made use of this concept of unscented transform in their filter formulation for state estimation.

   1. Accuracy of Sigma point approach

      Let X be an m dimensional random variable with mean and variance . It can be approximated by a set of 2m+1 number of sigma points i . They are

      0 = W0 = K/ + 2) i, = + ( + )i Wi = 1/2 + i+m = – ( + )i W i+m = 1/2 +

      where + )i is the ith row or column of the square root of matrix + , R and Wis are the weights associated with each sigma point.

      The computed value of mean and covariance of transformed sigma points are accurate up to second order because the mean and covariance of is accurate up to second order. That is how the accuracy is maintained. Its performance is equal to the truncated second order gaussian filter without the derivation of either jacobians or hessians. Performance of the unscented Kalman filter is equivalent to that of linear Kalman filter for linear systems but fits elegantly for non-linear systems without the linearization steps. Also it achieves better accuracy compared to the extended Kalman filter even with systems of high non- linearity. The ease of implementation and more accurate state estimation make this filter preferred compared to linear Kalman filter and extended Kalman filter.

3. Implementation of unscented Kalman filter

   Let us look at a non-linear system represented by the following discrete time equations

   = f(xk-1, wk-1, u k-1) (6)

   = h(xk, u k) + k (7)

   Where is the system state, is the process noise, is the observation noise. Here

   is the input, are the noisy observations. The non-linear functions f and h can be continuous or discrete. is the discrete time.

   1. The augmented state

      Simon Julier introduced the concept of augmenting 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 non- additive noises. Sigma points are computed using the augmented state. In this process the non-linear effects of noise are also captured with the same amount of accuracy as that of the original state.

      The state vector is augmented to L where L is the sum total of the dimension of the original state, process noise and observation noise. The covariance matrices are also similarly augmented to a L2 matrix. Now the augmented state and covariance matrix are represented as

      1

      Each sigma point is propagated through the non-linear function to arrive at the transformed sigma points

      i = f(i) 3)

      X-1

      a =

      and

      and the mean and covariance of the transformed sigma points are computed as

      P K-1a = E {(XK-1a – K-1a ) (XK-1a – K-1a )T }

      0

      0

      = 2

      0

      0

      Pyy= 2

      Wi i 4)

      Wi [ i- ] [ i- ] T 5)

      where P K-1a =

      1 0 0

      0 1 0

      0 0

      measurement cross correlation matrices which in turn are used to determine Kalman gain KK.

      Pzz = 2i=0 Wi i,K/K-1

      Pzz = 2i=0 Wi i,K/K-1

      L c [Z – ] [Zi,K/K-1- ]T (12)

      1

      Pxz = 2L

      Wi c[ x ,K/K-1 – X ] [Zi,K/K-1- ]T

      i=0 i K

      KK = P xz P zz -1

      Creation of 2L+1 number of sigma points is

      carried out in a way to accurately capture the mean and the covariance of the augmented state. The sigma

      Once the Kalman gain KK is computed, filtered state and its covariance are computed as

      X= X + KK (zK, – )

      a K/K K

      points are represented by the matrix whose columns are populated as

      PK/K = P K/K-1 – KK P zz KKT

      Here zK is the new measurement.

      a

      1

      1

      0,K-1 = for i=0 (9)

      a = + ( + ) for i= 1,2 L

      i, K-1 1 1 i

      a = – ( + )

   2. Unscented Kalman filter with additive

noise

i, K-1

1

1 i-L

for i=L+1 ..2L

Here subscript i stands for the ith column of the square root of the covariance matrix [8] . Parameter ( + ) stands for the spread of the

sigma 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].

i,K-1

i,K-1

i,K-1

i,K-1

The sigma point matrix ai,K-1 can be viewed as composing state x sampled process noise wi,K-1 and sampled measurement noise . Each sigma

point is also weighted. These weights are obtained from the comparison of the moments of the sigma points with a Taylor series expansion of the model. The weights for the mean and the covariance estimates are

W0 m = /( + ) for i = 0 10) W0 c = /( + ) + 1 – 2 + for i = 0

Wi m = Wc m = 1/2( + ) for i = 1,2, .. 2L

Here m and c denote the weights associated with the mean and the covariance of the state.

and are the scaling parameters. The choice of

and determines the accuracy of the higher order moments of distribution. is defined as very low in the interval 0 < < 1 signifying sigma-point spread. is used to incorporate prior knowledge of the distribution in the state prediction and update.

Authors of this paper have assumed that the process noise and measurements noise are zero mean gaussian and additive for the tracking problem[1]. For this case the state of the system need not be augmented with state pertaining to noise processes. A set of 2N+1 number of sigma points are defined for a state of dimension N.

The sigma points at time k are given by

0, K-1 = XK-1 for i=0

13)

i, K-1 = XK-1+ ( N + PK1)i for i= 1,2

N

i, K-1 = XK-1- ( N + PK1)i-N for i=N+1

.2N

The weights of the mean and covariance are calculated as before

W0 m = /( + ) for i=0 14) W0 c = /( + ) + 1- 2 + for i=0

Wi m = Wc m = 1/2( + ) for i=1,2, .. 2N The sigma points are propagated through the non- linear transformation f(x) to arrive at the new sigma

points i, K/K-1.

i, K/K-1 = f(i, K-1,uK-1 ) 15)

Weighted average of the sigma points gives the predicted state and its covariance as

These Sigma points are propagated to arrive

X = 2N

W m

K i=0

i i, K/K-1

at the predicted state through the state and

P = 2N Wi c [ i,K/K-1- X ] [ i,K/K-1- X]T + Q K+1

K i=0 K K

measurement models and to carry out their weighted averages.

Q K+1 stands for the covariance matrix of process noise.

i,

i,

ai, K/K-1 = f(x

X = 2L

K-1, uK-1, wi, K-1 ) 11)

W mx

To account for the process noise, the sigma points are

refreshed as follows

K i=0

i i, K/K-1

P = 2L

Wi c[ x K/K-1- X] [ x ,K/K-1- X]T

0, K/K-1 = for i = 0 16)

K i=0 i, K i K

Zi,K/K-1 = h(x K/K-1,v K-1 )

i, i,

2L m

= + ( N + P) for i = 1,2 N

= i=0 Wi

Zi,K/K-1

i, K/K-1 K i

Here X and are the weighted averages

K K i,K/K-1 = – ( N + P )i-N

representing predicted state and predicted measurements respectively. These predictions are used to compute measurement covariances and state-

K

for i = N+1 .2N

The new sigma points undergo the non-linear measurement function h(x), yielding another set of

K

K

sigma points i, K. Then their mean and covariance are used to compute predicted measurement z , measurement covariance Pzz, and state-measurement cross correlation Pxz,

i, K/K-1 = h(i, K/K-1 ) 17)

z = 2N W m

Nominal track data is generated at a sampling rate of 10 Hz. Random noise is assumed to be zero mean white Gaussian noise and is added to the nominal tracking data with the following

specifications to arrive at simulated track data. The 1 sigma noise in slant range, azimuth and elevation are

K i=0 i i, K/K-1

Pzz, = 2N

W c [ i,K/K-1- z ] [i, K/K-1 – z ]T + RK

r = 10 m az = 0.2 mil rad and el = 0.2 mil rad.

i=0 i K K

Pxz, = 2N W c [i K/K-1- – X ] [ i,K/K-1 – z ]T

Simulated track data is generated with 1 sigma noise

Here R

i=0 i K

ds or the cov

K

ce of mea rement

for a duration of 500 s catering to the visibility of

noise.

K stan f

arian su

Sriharikota Range. The simulated data was used to

Now the arrival of new measurement ZK is considered. Kalman gain and updates of the state are computed as follows.

KK = P xz,K P zz,K -1 18)

XK/K = X + KK (zK – z )

generate the initial conditions of the state. Process noise for the state is obtained using the model uncertainty estimation technique by processing the ideal state, a methodology proposed by S.K. Pillai,

S.S. Balakrishnan, V.Seshagiri Rao and N.Narasaiah

K K [11]. The ideal state is generated using rigid body

K

K

PK/K =

These f

P – KK Pzz KKT

equations are implemented to process the

dynamics of the rocket as described by three

ilter

noisy radar data. Results are compared with the nominal data to assess the efficacy of the methodology.

4.0 Estimation of Trajectory of a typical Launch Vehicle

Even though Kalman filter is an attractive tool in the estimation theory, the design of filter for optimal estimate needs a difficult tuning of initial state, measurement noise and process noise covariances [10]. A simple and elegant state space model is used to estimate the state of trajectory of a Polar Satellite Launch Vehicle [PSLV] using unscented Kalman filter. Performance of the unscented Kalman filter for a typical trajectory is shown in the following Figure.2 to Figure.14.

In the estimation model the state space consists of nine variables namely relative position, velocity and acceleration of the target with respect to the radar. It is expressed in the topocentric rotating coordinate system as [ , X , , , Y , , , Z , ]T

The Topocentric rotating coordinate system is defined as the radar as the origin and east, north and vertical unit vectors forming the triad [Figure.1].

The target measurements are slant range,

azimuth and elevation of the target from the radar.

Figure.1 Topocentric coordinate system

dimensional six degree of freedom (6 DOF) equations of motion. This concept is successfully used for the state estimation of the target using linear Kalman filter since 1980s at Srihaikota range. The authors have generated the model compensation values in the cartesian frame and scaled them to take care of the dynamics of the vehicle around stage separations. These scale factors are arrived at after a number of simulation runs for different levels of dynamics of the vehicle.

System model used for time update of the sigma points is

XK+ 1 = k XK 19)

0 0

Where k = 0 0

0 0

Here A is defined as

1 2

A = 0 1 2 20)

0 0 1

0 0 0

0 = 0 0 0

0 0 0

Where T is the sampling period. The authors of this paper used the Cholesky decomposition to arrive at the square root of the covariance matrix.

5.0 Results and Discussion

The relative position and velocity of the target with respect to the radar are estimated using the system model and measurement model as provided in Section 3.0. Authors have assumed that the process noise and measurements noise are zero mean

Gaussian and additive for the tracking problem. Unscented scented Kalman filter with additive noise as described from Equation 13 to Equation 20 are exercised to obtain the filtered estimate. The estimated state is used to arrive at the filtered measurements. Figures 2 to 4 provide the first differences of the raw measurements and the filtered measurements. The mean of their first differences provide the rate of the measurements and the excursions denote the noise in the measurements. It is candidly clear that the noise existent in the angles of raw measurements is around

0.2 mil rad. It is shown in red color. The blue curve is the first difference of the filtered measurements. The excursions are very small since the noise is almost filtered out. The filtered state is compared with the nominal state and the position and velocity errors are computed. These errors in the state are compared against their ± 1 theoretical bounds to establish the consistency of the performance of the filter.

Consistent performance of the unscented Kalman filter can be observed in the Figures 5 to 10. The errors in the position and velocity components i.e X and , Y and , Z and are plotted against their 1 sigma bounds obtained from the filters estimated covariance matrices.

Figure 2. First differences in slant range (Raw vs filtered)

Figure 3. First differences in Azimuth (Raw vs filtered)

Figure 4. First differences in Elevation (Raw vs filtered)

Figure 5. Error in X against its 1 sigma bounds

Figure 6. Error in VX against its 1 sigma bounds

Figure 7. Error in Y against its 1 sigma bounds

Figure 8. Error in VY against its 1 sigma bounds

Figure 9. Error in Z against its 1 sigma bounds

Figure 10. Error in VZ against its 1 sigma bounds

The fact that the errors run with their theoretical bounds confirm the consistent behavior of the filter. The seeming divergence observed in Y from T+325 s to T+365 s and divergence in Z from T+350 s to T+380 s is well captured by the filter and the errors are within their theoretical limits afterwards.

Figure 11. provides the position and velocity errors in the estimated state and they are within 5 m and 5 m/s respectively throughout the flight time except at stage burn out or stage separation events. Authors have exercised the extended Kalman filter with the same simulated track data and the accuracy of estimated state is provided in terms of position and velocity errors [Figure.12].

It is observed that with the extended Kalman filter the position error is less than 10 m and the velocity error less than 10 m/s except at instants of sudden change in the dynamics 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 comparison. The position and velocity errors are 10 m and around 5 m/s for unscented Kalman filter and 25 m and 20 m/s for extended Kalman filter respectively.

Figure 11. Position and velocity errors from Unscented Kalman Filter

Figure 12. Position and velocity errors from Extended Kalman Filter

Figure 13. Position and velocity errors for USKF with measurements of 3 noise

Figure 14. Position and velocity errors for EKF with measurements of 3 noise

Errors in velocity for extended Kalman filter are significantly large whereas they are benign in the case of unscented Kalman filter for trajectory estimation. The above analysis clearly shows that state obtained from Unscented Kalman filter is more accurate than that from extended Kalman filter and it exhibits a consistent behavior for trajectory estimation using measurements with different levels of gaussian noise.

6.0 Conclusion

In this paper an effort is made to accurately estimate the trajectory of a typical PSLV launch vehicle from noisy radar measurements using unscented Kalman filter and to compare it with extended Kalman filter. The state and the measurement model are defined in non-linear state space and the filters are exercised over the noisy radar measurements. The accuracy of the filtered estimate is provided in terms of errors in position and velocity. Complexity in filter formulation being the same for unscented Kalman filter and extended Kalman filter, the performance of the unscented Kalman filter is undoubtedly better than extended Kalman filter with an added advantage of ease of implementation without the usage of analytical derivatives such as jacobians or hessians. Performance of the filter is consistent and the implementation is elegant. This methodology can be extended for higher order non-linear systems also without losing any accuracy or stability. Also an adaptive estimate of the process noise can be studied and implemented for a complete on line application [12].

ACKNOWLEDGEMENTS

The authors wish to thank Dr.MYS Prasad, Director, SDSC SHAR for granting permission to publish this work. Thanks are also due to Dr V.Adimurthy, Dean, IIST, Trivandrum for his keen

and constructive suggestions. Authors acknowledge the contributions of Real time 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 Kalman Filter, in Kalman 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.Uhlmann and H Durant Whyte, A new approach for filtering nonlinear systems, Proceedings of American Control conference , Pages 1628-1632, 1995

3. Papoulis A , Probability, Random variables and stochastic processes. McGraw- Hill, Inc. 1965

4. S.J. Julier and J.K.Uhlmann, A new extension of Kalman filter to non-linear systems, The 11th symposium of Aero space/ Defense sensing, simulation and controls, 1997

5. Julier.S, Uhlmann.J and H.F.Durrant Whyte, A New Method for the Nonlinear Transformation of Means and Covariances in filters and Estimators, IEEE Transactions on Automatic control, vol. 45, pp. 477482, 2004.

6. Paul Zarchan, and Howard Musoff, Fundamentals of Kalman filtering: A Practical Approach, 2nd Edition, Vol 208, Progress in Aeronautics and Astronautics, 2005

7. S.J. Julier and J.K.Uhlmann, A general method for approximating nonlinear transformations of probability distributions, Technical report, Department of Engineering science, University of Oxford, 1996

8. W.H. Press, S.A.Teulosky, W.T. Vetterling and

   B.P. Flannery, Numerical recipes in C: The Art of Scientific computing, Cambridge university press, 1992

9. Ghanbarpour Asl and Portakdoust, UD covariance factorization for Unscented Kalman filter using sequential measurements update, World academy of Science, Engineering and technology, 2007

10. M.R. Ananthasayanam, The philosophy, principles and practice of Kalman filter since ancient times to the present, The 62nd International Astronautical Congress, Cape Twn, 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 estimation by Ideal state processing in Linear Kalman filter, Journal of Guidance, Control and Dynamics, Vol.15, No.3, pp 775-777, 1992

12. Ohlmeyer.E.J., P.K.Menon and Jinwhan Kim, Tracking of spiral reentry vehicles with varying frequency using the Unscented Kalman filter, AIAA Guidance, navigation and Control conference, Toronto, Canada, 2010