Line of Sight Rate Estimation of Strapdown Imaging Seeker Based on Particle Filter

Zhang Ying-chun, Li Jing-jing, Li Hua-yi

precision is decreased because of Truncations; Calculation of Jacobian matrices is usually very complicated and often leads to significant implemental difficulty; If sample time is not small enough, system may also be unstable. To get a better estimation, Ref.[1] used Unscented Kalman Filter(UKF) method to distill the LOS angular rate. The precision of UKF is better than EKF, it can predict the mean and covariance up to the third order precision. The limitation of UKF is that only Gaussian distributed random variable could be estimated. Compared with EKF and UKF, Particle Filter turns into an efficient method to solve the problems of nonlinearity and nonGaussian dynamic system estimation[2~4]. This method achieves a Recursive Bayesian Filter using Monte Carlo Simulations. The essence of PF is to employ a random sample spaceparticleswith corresponding weights to represent the required probability density function (pdf) [5]. The particles are propagated over time using a combination of sequential importance sampling and resampling steps. The resampling step statistically multiplies and/or discards particles at each time step to adaptively concentrate particles in regions of high posterior probability[6]. These methods are very flexible and can be easily applied to nonlinear and nonGaussian dynamic models. II. PARTICLE FILTER AND ALGORITHM Particle filter algorithm was first brought forward by Gordon in 1993, this is an optimized recursive Bayesian algorithm based on Monte Carlo method. State vectors were expressed as a set of random samples with correlated weights[7~8] in this filtering algorithm, estimation of the states can be worked out based on the weights and samples. The state estimation will be equal to posterior probability density function when number of samples is enough. i i i Assume the weights wk are normalized, { xk , wk }iN=s1 denotes the random particles which represent for pdf, and the pdf at sample step k can be approximately expressed as:
i i p( xk / z1:k ) wk ( xk xk ) . i =1 Ns

AbstractA method to estimate the line of sight(LOS) rate of strapdown imaging seeker based on Particle Filter(PF) is presented. PF algorithm is firstly put forward and then the model of strapdown imaging seeker is introduced. Because of the high nonlinearity in both process and measurement equations and more seriously nonGaussian noise in the measurements, the normally used Extended Kalman Filter(EKF) can not completely meet the requirements of filtering. Comparatively, PF is a congruent method for tracking in the conditions of nonlinearity and nonGaussian noise. At the last, PF and EKF are applied to estimate the LOS rate of strapdown imaging seeker respectively. Simulation results show that PF is more precise than EKF in LOS rate estimation.

HERE are advantages such as compact structure, high reliability, and unlimited LOS rate tracking capability in strapdown imaging seeker. However, there are two serious disadvantages in strapdown imaging guidance system. Firstly, only LOS angular information relative to body coordinate system can be measured, thus the metrical LOS angles will couple with body attitude motion which finally leads to high nonlinearity. Secondly, more serious measurement noise especially nonGaussian noise will be induced into the strapdown imaging guidance system because of the wider instantaneous field of view compared with the common guidance system. The high nonlinearity and serious measurement noise make it very difficult to distill the LOS rate which is essential to implement guidance law. To deal with nonlinearity, EKF adopts first-order Taylor series expansion of nonlinear function, it expands dynamic model as a Taylor series at the current state estimate, and measurement model at the state prediction, and then linearization is achieved by neglecting second and higher order items. It propagates statistical property of random variable by linear approximation of nonlinear function. The drawbacks[1] of EKF are obviously: estimation

I. INTRODUCTION

Manuscript received October 9, 2009. This work was supported in part by China National 863 Program under Grant No.2008AA702101. Zhang Ying-chun is with Department of Control Science and Engineer, Harbin Institute of Technology, Harbin, China(E-mail: ltz1982@sina.com; Phone: 0451-86412357). Li Jing-jing, is with the the Department of Control Science and Engineer, Harbin Institute of Technology, Harbin, China. Li Hua-yi, is with the the Department of Control Science and Engineer, Harbin Institute of Technology, Harbin, China.

(1)

i Selecting wk as importance sampling principle and assuming p( x) ( x) , it is difficult to get the sample value of

978-1-4244-6044-1/10/$26.00 2010 IEEE

191

x. Let xi ~ q ( x) , i = 1, , N s , q() is importance density function, estimation to density p( x) can be expressed as:
i p( x ) wk ( x x i ) , i =1 Ns

large weights. The resampling step involves[9] generating a new i set { xk }iN=s1 by resampling (with replacement) N s times from an approximate discrete representation of p ( xk / z0:k ) given by Eq.(9).The commonly used resampling methods[2] include Multinomial Resampling, Residual Resampling, Stratified Resampling and Systematic Resampling. According to above illustration, standard particle filter algorithm[10~11] can be achieved as follows: For sample steps k = 0,1, 2, Step 1 Initialization: k = 0; i Draw the samples x0 ~ p( x0 ) , i = 1, 2, , N s Step 2 Calculate the importance weights: Enactment: k=k+1, i i Sampling: xk ~ q( xk / x0:k , z0:k ) , i = 1, 2, , N s Calculate the importance weights: the same as (7). Normalize the importance weights: the same as (8). Step 3 Resampling: i Generate a new particle set xk i = 1, 2, N s by
i Resampling from the previous set xk ; i = 1, 2, i k

(2)

Where,
i wk

p( xi ) . q( xi )

(3)

i If samples xk come from importance density function i q( xk / z0:k ) , the weights wk in Eq.3 may be defined as:
i wk i p ( xk / z0:k ) . i q ( xk / z0:k )

(4)

Importance density function q( xk / z0:k ) can be disassembled as follows: (5) q( xk / z0:k ) = q ( xk / xk 1 , z0:k ) q ( xk 1 / z0:k 1 ) . Probability density function p( xk / z0:k ) can be disassembled as: p ( zk / xk , z0:k 1 ) p ( xk / z0:k 1 ) p ( xk / z0:k ) = p ( zk / z0:k 1 )
p ( zk / xk , z0:k 1 ) p ( xk / xk 1 , z0:k 1 ) p ( xk 1 / z0:k 1 ) = p ( zk / xk ) p ( zk / xk ) p( xk / xk 1 ) p( xk 1 / z0:k 1 ) . Based on Eqs.(5) and (6), Eq.(4) can be rewritten as: i i i i p( zk / xk ) p( xk / xk 1 ) p( xk 1 / z0:k 1 ) i wk i i i q( xk / xk 1 , z0:k ) q( xk 1 / z0:k 1 )
i = wk 1

Ns according

(6)

i to the importance weights w , reset the weights wk = 1/ N s . Step 4 Outputs: State estimation:
i ( x = wki ) xk . i =1 Ns

(10)

Variance estimation:
i i ( Pk = wki ) ( xk xk )( xk xk )T . i =1 Ns

p ( zk / x ) p ( x / x i i q ( xk / xk 1 , z0:k )
i k i k

i k 1

(11)

i i i p( zk / xk ) p ( xk / xk 1 ) . i i q ( xk / xk 1 , z0:k ) Equation (7) suggests the update process of weights. i Normalize the importance weights wk : i = wk 1

(7)

Step 5 Repeat steps 2 to 4. Traditional particle filter algorithm adopts importance density function as: (12) q ( xk / xk 1 , z0:k ) = p ( xk / xk 1 ) . III. MODEL OF STRAPDOWN IMAGING GUIDANCE SYSTEM The geometric relationship of missile(M) and target(T) is showed in Fig.1.

i wk =

i wk wki

(8)

Thus probability density function p ( xk / z0:k ) can be estimated:

p ( xk / z0:k ) w ( xk x ) .
i =1 i k i k Ns

ye

(9)

r
M

When N s , estimation Eq.9 will be close to actual probability density function p ( xk / z0:k ) . However this approach leads to degeneracy problem when some of the particles have zero weights relative to the others. To reduce the effects of degeneracy problem, resampling methods are introduced. The basic idea of resampling is to eliminate particles with small weights and to concentrate on particles with

xe

ze
Fig. 1 geometric relationship of missile(M) and target(T)

LOS angular rate directly measured in strapdown imaging seeker contains information of body movement of the missile. Observation equations can be established using the

192

measurements q q as observation variables in which noises are included. State equations can be also established when taking LOS elevation and elevation rate, LOS azimuth and azimuth rate as state variables. axs a ys azs are relative acceleration vectors of missile and target in LOS coordinate. For simplicity, we assume that target and missile do not maneuver. q and q are LOS elevation and azimuth respectively.
L( , , ) is rotation matrix from the inertial coordinate to

nonGaussian noise.
x2 ), (16) 2 2 2 x 1 f l ( x) = (17) exp( ) . 2 The laplace probability density function and the laplace noise are showed in Fig.2 and Fig.3: f g ( x) =

exp(

180 160 140 120 100 80 60

body coordinate. R ij (i = 1, 2,3; j = 1, 2,3) are corresponding elements in L( , , ) . Defining x = [q

q q q ]T , state

equations can be expressed according to Ref [1] as follows: x1 = x2

2r 2 x2 = x2 x4 sin x1 cos x1 r x3 = x4 x4 = 2 x2 x4 tan x1 2r x4 r

13

40 20 0 -0.03 -0.02 -0.01 0 0.01 0.02 0.03

and observation equations: y1 = arcsin(R21cosx1cosx3 +R22sinx1 R23cosx1sinx3 ) + v1

y2 = arctan( R33sinx3 R31cosx3 R32 tanx1 ) + v2 R11cosx3 +R12 tanx1 R13sinx3

Laplace Probability Density Function Fig.2 Laplace pdf

10

x 10

-3

14
8 6

where v1 and v2 are measurement noise vectors. The main advantage in strapdown imaging guidance system is the wider instantaneous view field than commonly used seeker, however, more noises especially nonGaussian noises are induced. Traditional method(EKF) to estimate LOS angular rate can not completely meet the requirements of precision. Equations (13) and (14) also imply that there is high nonlinearity in observation equations and state equations. As EKF adopts one order linearly approximation of model, there will be more estimation error. Particle filter algorithm can make up the disadvantages of EKF, and it is an efficient method to solve the problems of optimized estimation to nonlinearity and nonGaussian dynamic systems. In this paper, inertial LOS rate is estimated by using of PF to cope with the high nonlinearity and serious measurement noise contained in the strapdown imaging guidance system. IV. NONGAUSSIAN NOISES As is mentioned above, the measurement noise is serious nonGaussian noise in strapdown imaging guidance system. The nonGaussian noise usually is described by[12~13] : f ( x) = (1 ) f g ( x) + fl ( x) . (15) Where f g ( x) and f l ( x) are Gaussian and Laplace probability density function respectively. [0,1] , denotes the intensity of

-2

-4

10

20

30

40

50

60

70

80

90

100

Laplace Noise Record Fig.3 Laplace noise

V. SIMULATION AND ANALYSIS According to the dynamic model of above strapdown imaging system, PF and EKF are performed to estimate the LOS rate respectively. The moments of inertia are J x = 0.15kg m 2 , J y = J z = 1.5kg m 2 respectively, the torques applied on the missile are
M x = 0.05sin(2 t ) N m ,

M y = M z = 0.5sin(2 t ) N m , initial attitude are = 0rad

= 0rad = / 3rad initial state estimation is

[0.37379 0.0285 0.1974 0.0055]T , initial relative range is 1000m. The number of sampling particles is 500 and 50 Monte Carlo simulations were performed. To compare EKF and PF, firstly assume the measurement

193

RMSE of elevation angular rate(rad/s)

noise is absolutely Gaussian noise[14], covariance matrix of measurement noise is [9e-4 0;0 9e-4]T , is set to zero, The sample step of missile is set to 0.1s, the simulation results are showed in Fig.4 and Fig.5:
RMSE of elevation angular rate(rad/s)
4 3.8 3.6 3.4 3.2 3 2.8 2.6 2.4 x 10
-3

simulation results are showed in Fig.6 and Fig.7:

0.014 EKF RMSE PF RMSE

0.012

0.01

EKF RMSE PF RMSE

0.008

0.006

0.004

0.002

10

15

20

25

30

35

40

45

50

no of runs
Fig.6 RMSE of LOS elevation rate
5.5 5 4.5 4 3.5 3 2.5 2 1.5 1 0.5 0 5 10 15 20 25 30 35 40 45 50 x 10
-3

RMSE of azimuth angular rate(rad/s)

10

15

20

25

30

35

40

45

50

no of runs
Fig.4 RMSE of LOS elevation rate
3 x 10
-3

EKF RMSE PF RMSE

RMSE of azimuth angular rate(rad/s)

EKF RMSE PF RMSE 2.5

1.5

no of runs
Fig.7 RMSE of LOS azimuth rate

0.5

10

15

20

25

30

35

40

45

50

Mean and standard deviation of RMSE are showed in TABLE III and TABLE IV respectively
TABLE III MEAN OF RMSE Elevation rate EKF PF 0.01241 0.0012607 Azimuth rate 0.0045489 0.00082231

no of runs
Fig.5 RMSE of LOS azimuth rate TABLE I

Mean and standard deviation of RMSE are showed in and TABLE II respectively
TABLE I MEAN OF RMSE Elevation rate EKF PF 0.002997 0.0028611 TABLE II STANDARD DEVIATION OF RMSE Elevation rate EKF PF 0.00021948 1.1613e-005 Azimuth rate 0.00035552 0.00012438 Azimuth rate 0.0020339 0.00065084

TABLE IV STANDARD DEVIATION OF RMSE Elevation rate EKF PF 0.00035134 9.9742e-005 Azimuth rate 0.00041568 0.00017565

For comparison, sample time is shortened to 0.01s, the

It is obvious that, when the measurement noise is Gaussian noise, the RMSE of PF is much smaller than that of EKF. In such instances, the estimation error is mainly caused by the states noise and nonlinearity of equations. To study the performance of the filter when measurement

194

noise is nonGaussian noise, assume =1, =0.003, take elevation angular rate for example, the simulation results are in Fig.8:
RMSE of elevation angular rate(rad/s)
0.018 0.016 0.014 0.012 0.01 0.008 0.006 0.004 0.002 EKF RMSE PF RMSE

is also less than EKF, this means the RMSE diversification of PF is more steady. VI. CONCLUSIONS On account of the high nonlinearity and serious measurement nonGaussian noise in dynamic equations of strapdown imaging seeker, particle filter is employed to estimate inertial LOS rate in this paper. Through comparison of EKF and PF results after simulations, we can conclude that the PF is superior to the EKF in both precision of estimation and stability of estimation error. With the enhancement of computer capability, PF will turn into an efficient method for the application of LOS rate estimation in strapdown imaging guidance system. REFERENCES
[1]

10

15

20

25

30

35

40

45

50

no of runs
Fig.8 RMSE of LOS elevation rate

The mean of RMSE are 0.017169 and 0.002125 respectively for EKF and PF, the standard deviation of RMSE are 0.00027346 and 0.00010114 respectively for EKF and PF. To increase the nonGaussian noise, is set to 0.03.The simulation results are in Fig.9:
RMSE of elevation angular rate(rad/s)
0.018 0.016 0.014 0.012 0.01 0.008 0.006 0.004 0.002 EKF RMSE PF RMSE

[2] [3] [4] [5] [6] [7] [8] [9] [10]

10

15

20

25

30

35

40

45

50

[11] [12] [13]

no of runs
Fig.9 RMSE of LOS elevation rate

The mean of RMSE are 0.016987 and 0.0021418 respectively for EKF and PF, the standard deviation of RMSE are 0.00032585 and 0.0001007 respectively for EKF and PF. It is obvious that, when the measurement noise is assumed to be absolutely laplace noise, the RMSE and standard deviation of RMSE of PF is smaller than that of EKF, the performance of PF is still better than that of EKF. The simulation results show that, in both instances of Gaussian and nonGaussian measurement noise, the RMSE of LOS angular rate estimation using PF is far less than EKF. What is more, the standard deviation of RMSE when using PF

[14]

Guojiang Zhang,Yu Yao and Kemao Ma,Line of sight rate estimation of strapdown imaging guidance system based on unscented kalman filter, Proceedings of the Fourth International Conference on Machine Learning and Cybernetics, Guangzhou, pp.1574-1578,18-21 August 2005. Doucet A and Gordon N. Sequential Monte Carlo Methods in Practice. New York: Springer Verlag, 2001. Carpenter J and Clifford P. Imroved particle filter for nonlinear problems,. IEE Proc of Radar, Sonar and Navigation, 1999, 146 (1) : 227. Ho Y. C.and Lee R. C. K, A Bayesian approach to problems in stochastic estimation and control, IEEE Trans. Automat. Contr., vol. 9, pp. 333339, Oct. 1964. Gordon N. J, Salmond D. J. and Smith A. F. M., Novel approach to nonlinear/nonGaussian Bayesian state estimation, Proc. Inst. Elect.Eng. F, vol. 140, pp. 107113, 1993. Dan Crisan and Arnaud Doucet, A Survey of Convergence Results on Particle Filtering Methods for Practitioners, IEEE Trans. Signal Processing, vol. 50, no. 3,pp.736-746, March2002. Chen Y, Sequential importance sampling with resampling: Theory and applications, Ph.D. thesis, Stanford Univ., 2001. Bolic M, Djuric P.M..and Hong S. Resampling Algorithms and Architectures for Distributed Particle Filters. IEEE Trans. Signal Processing, vol. 53, no. 7,pp.24422450,2005. Arulampalam S, Maskell S, Gordon N. and Clapp T, A tutorial on particle filters for on-line non-linear/non-gaussian bayesian tracking. Shiqiang Hu and Zhongliang Jing Overview of particle filter algorithm, Control and Decision.vol.20,no.4,April 2005. Crisan D, Particle filters - A theoretical perspective, in Sequential Monte Carlo Methods in Practice, A. Doucet, J. F. G. de Freitas, N. J. Gordon, Eds. Berlin: Springer Verlag, 2001. Kostantinos N.P, Dimitris H. Advanced Ssignal Processing Handbook. Boca Raton: CRC Press LLC,2001. Hu H.T and Jing Z.L,An MCMC-Based Particle Filter for Tracking Target in Glint Noise Environment, .Proceedings of the Seventh International Conference on Information Fusion. Tockholm, Sweden, 2004. International Society of Information Fusion, Fairborn, OH 45324,United States.2004:922~927. Kotecha J. H and Djuric P. M, Gaussian sum particle filtering for dynamic state space models, in Proc. ICASSP2001, pp. 34653468, 2001.

195