 Open Access
 Total Downloads : 533
 Authors : R.Varaprasad, S.V. Bhaskara Rao, D.Narayana Rao, V. Seshagiri Rao
 Paper ID : IJERTV2IS90249
 Volume & Issue : Volume 02, Issue 09 (September 2013)
 Published (First Online): 11092013
 ISSN (Online) : 22780181
 Publisher Name : IJERT
 License: This work is licensed under a Creative Commons Attribution 4.0 International License
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 / quasilinear systems. Of late unscented Kalman filter is proposed for state estimation of any nonlinear system. This methodology consists in propagating the mean and the variance through nonlinear 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.

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 nonlinear systems. It simply calls for linearization of nonlinear 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 nonlinear 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 nonlinear. Approximating nonlinear 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.

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 nonlinear 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 nonlinear 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 nonlinear 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 nonlinear 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 nonlinear 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.

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 nonlinear 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.


Implementation of unscented Kalman filter
Let us look at a nonlinear system represented by the following discrete time equations
= f(xk1, wk1, u k1) (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 nonlinear functions f and h can be continuous or discrete. is the discrete time.

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 nonlinear fashion thereby taking care of nongaussian and non additive noises. Sigma points are computed using the augmented state. In this process the nonlinear 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 nonlinear function to arrive at the transformed sigma points
i = f(i) 3)
X1
a =
and
and the mean and covariance of the transformed sigma points are computed as
P K1a = E {(XK1a – K1a ) (XK1a – K1a )T }
0
0
= 2
0
0
Pyy= 2
Wi i 4)
Wi [ i ] [ i ] T 5)
where P K1a =
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/K1
Pzz = 2i=0 Wi i,K/K1
L c [Z – ] [Zi,K/K1 ]T (12)
1
Pxz = 2L
Wi c[ x ,K/K1 – X ] [Zi,K/K1 ]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/K1 – KK P zz KKT
Here zK is the new measurement.
a
1
1
0,K1 = for i=0 (9)
a = + ( + ) for i= 1,2 L
i, K1 1 1 i
a = – ( + )

Unscented Kalman filter with additive

noise
i, K1
1
1 iL
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 [ 01]. To avoid any non local effects , the authors defined it as a low value [9].
i,K1
i,K1
i,K1
i,K1
The sigma point matrix ai,K1 can be viewed as composing state x sampled process noise wi,K1 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 sigmapoint 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, K1 = XK1 for i=0
13)
i, K1 = XK1+ ( N + PK1)i for i= 1,2
N
i, K1 = XK1 ( N + PK1)iN 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/K1.
i, K/K1 = f(i, K1,uK1 ) 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/K1
at the predicted state through the state and
P = 2N Wi c [ i,K/K1 X ] [ i,K/K1 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/K1 = f(x
X = 2L
K1, uK1, wi, K1 ) 11)
W mx
To account for the process noise, the sigma points are
refreshed as follows
K i=0
i i, K/K1
P = 2L
Wi c[ x K/K1 X] [ x ,K/K1 X]T
0, K/K1 = for i = 0 16)
K i=0 i, K i K
Zi,K/K1 = h(x K/K1,v K1 )
i, i,
2L m
= + ( N + P) for i = 1,2 N
= i=0 Wi
Zi,K/K1
i, K/K1 K i
Here X and are the weighted averages
K K i,K/K1 = – ( N + P )iN
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 nonlinear 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 statemeasurement cross correlation Pxz,
i, K/K1 = h(i, K/K1 ) 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/K1
Pzz, = 2N
W c [ i,K/K1 z ] [i, K/K1 – 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/K1 – X ] [ i,K/K1 – 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 nonlinear 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 nonlinear 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

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

S.J. Julier ,J.K.Uhlmann and H Durant Whyte, A new approach for filtering nonlinear systems, Proceedings of American Control conference , Pages 16281632, 1995

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

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

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.

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

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

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

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

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: IAC11E4.3.9

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 775777, 1992

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