Line-of-Sight Rate Estimation Based on UKF for Strapdown Seeker

Aiming at the problem that the strapdown imaging seeker cannot measure line-of-sight (LOS) rate directly, this paper presents an effective algorithm for LOS rate estimation. To address the problem, the reference frames and angles are defined. According to the relative kinematics and attitude relationship between missile and target, the nonlinear state equations and measurement equations of LOS rate are derived, and the estimation algorithm based on unscented Kalman filter (UKF) is proposed. Considering the estimation accuracy mainly depends on body LOS (BLOS) angle accuracy and gyro accuracy, the paper is unprecedented to research how these two factors impact the LOS angle and rate accuracy by simulation. Semiphysical simulation experiment verifies the correctness and accuracy of the algorithm, and the result shows that the estimation algorithm can meet the accuracy and realtime requirements of guidance system simultaneously. Thus LOS rate estimation algorithm based on UKF provides a theoretical basis for engineering applications of strapdown imaging seeker.


Introduction
Optical imaging seekers have been widely used in military fields due to the high precision, such as the threemode cooled imaging seekers of JAVELIN missile and the uncooled infrared strapdown seeker Direct Attack Munition Affordable Seeker (DAMASK) of JDAM missile.With the rapid development of large area-array complementary metal oxide semiconductor (CMOS) image sensors, automatic target recognition, and high-performance image trackers, the application of strapdown imaging seeker to guidance system has become an important research topic, such as the excellent application of the SPIKE missile in the United States.Strapdown imaging seeker which mounted to missile body rigidly has some advantages [1], such as compact structure, high reliability, unlimited LOS rate tracking capability, and instant field of view (FOV) equal to total FOV.However, only BLOS angle information can be measured directly; thus the LOS angles will couple with body attitude motion which finally leads to strong nonlinearity.The strong nonlinearity makes it very difficult to distill the LOS rate which is essential to the proportional guidance law.Therefore, how to estimate the LOS rate in real time to meet the accuracy requirement of guidance system is a key issue for strapdown guidance technology and application.
Researchers have studied LOS rate estimation of strapdown seeker in recent years.An "additional rate compensation + differential network" method was adopted to reconstruct the single-channel LOS rate in [2] and then offered the signal to guidance system after it passed through a low-pass filter.LOS rate generated by a self-adaptive jitter filter and differential network method [3] was only applicable for low maneuvering guided weapons.Disturbance observer was used to estimate the LOS rate in single channel [4].The relative relationship and frame transformation between missile and target were employed to derive the relationship among the LOS rate, BLOS rate, and attitude angles, where the BLOS rate can be obtained by a differential network [5].Extended Kalman filter (EKF) was utilized to estimate the inertial LOS rate [6].The target motion was negligible 2 Mathematical Problems in Engineering with respect to the missile in [7,8], the relative distance, velocity, and acceleration were assumed to be zero, and then LOS rate was estimated using UKF and Particle Filter (PF), separately.- filter [9] was employed to estimate BLOS rate, and then the LOS rate was reconstructed.Further, the filtering bandwidth can also be adjusted.The linear and nonlinear mixed differentiator based on the sliding mode [10] were adopted to estimate the LOS rate.All mentioned algorithms above are either mainly based on differential network and the reconstruction method or ignoring too much relative motion information between missile and target; then nonlinear Kalman filtering algorithms are employed to estimate LOS rate, which failed to get high accuracy and cannot meet the real-time requirement.This paper proposes an effective nonlinear algorithm to estimate the LOS rate, we take the relative motion between the missile and target and the missile attitude into account to derive the estimation model, and then LOS rate is estimated using UKF.The correctness and accuracy of the algorithm are verified through the semiphysical simulation experiment.
The remainder of this paper is organized as follows.Section 2 defines the reference frames and angles.The LOS rate model including standard LOS rate and kinematic model of LOS rate estimation were introduced in Section 3. Section 4 proposes the LOS rate estimation algorithm based on UKF.Different performance seekers and gyros impacts on the LOS rate accuracy are simulated in Section 5.The semiphysical simulation experiment is designed; result and discussion are also shown in Section 6. Section 7 illustrates the conclusion of this paper.

Reference Frames and Angles
In order to study the LOS rate estimation algorithm for strapdown imaging seeker, the definitions of earth frame         , body frame         , LOS frame         , and BLOS frame         are shown in Figure 1, where the subscripts , , , and  represent earth, body, LOS, and BLOS.For the convenience, the influence of earth spin is neglected so that earth frame is coincident with inertial frame.Earth frame         is defined as follows: the origin   is selected as launching L q  , q  L q  , q  L q  , q

Body frame
Earth frame BLOS frame LOS frame   2, where   is LOS transform angle., , and  are pitch, yaw, and roll, respectively.(. ..) is the direction cosine matrix between frames.
In the strapdown guidance system, the seeker is fixed on the missile body rigidly, so the seeker optical axis is in accordance with body axis     and the coordinate of target on the focal plane can be calculated through the image processing algorithm.Combined with focal length , BLOS vertical angle   and azimuth angle   can be calculated.
The measurement schematic is depicted as in Figure 3, where (  ,   ) expresses the image coordinate of the target.

LOS Rate Model
3.1.The Standard LOS Rate Information.The standard information of LOS rate can be derived from the relative motion between missile and target.As shown in Figure 2, earth frame could transform to LOS frame by two different paths.
The first path is from earth frame to body frame, then from body frame to BLOS frame, and in the end from BLOS frame to LOS frame.The rotary rate from earth frame to LOS frame can be expressed as Ω   in earth frame: where Ω   is the angular rate of LOS frame relative to BLOS frame expressed in LOS frame.Ω   is the angular rate of BLOS frame relative to body frame expressed in body frame.Ω   is the angular rate of body frame relative to earth frame expressed in body frame.
The second path is according to the direct rotation relationship between LOS frame and earth frame: By synthesizing (1)-( 5), the standard LOS rates q  and q  can be obtained:  where   is the element of row  and column  of the rotational matrix    from earth frame         to body frame         .
To calculate LOS angle and rate,  1 ,  1 ,  1 , , , ,   ,   , q  , and q  are required.In practical applications, the body angular rates  1 ,  1 , and  1 can be directly measured by inertial measurement unit (IMU).The attitudes , , and  can be calculated by strapdown inertial navigation system (SINS).The BLOS angles   and   can also be measured by strapdown seeker.BLOS rates q  , q  can be estimated by methods of optical flow [11,12], differential network, or other filter techniques.

Kinematic Model of LOS Rate Estimation.
Relative kinematics relationship between missile and target in LOS frame can be shown by the following equation [13][14][15]: where We can obtain that According to the transformation between inertial frame and LOS frame, the relationship between LOS angle [  ,   ]  and the angular rate  ⇀   can be described as where (  ,   ) is the transformation matrix from inertial frame to LOS frame.Substituting ( 10) into ( 9), the movement equations of LOS can be derived: According to the geometry relationship of the relative motion between missile and target, the target coordinates in LOS frame and in BLOS frame are both equivalent to [ 0 0]  .Using the coordinate and transformation relationship between different frames,   and   can be calculated by the following equation: By representing missile attitude with Euler angles, the set of kinematic equations [16] is given as

State Equation and Measurement Equation.
We choose LOS azimuth angle   , LOS azimuth rate q  , LOS vertical angle   , LOS vertical rate q  , and attitudes , , and  as state vector.Consider Comparing with the velocity of missile, the velocity of ground target is slow, so the missile-target relative velocity Ṙ is approximated to   .The information of relative distance  and approaching velocity Ṙ can be calculated approximately by SINS. ⇀  1 () is zero-mean uncorrelated process noise.The missile acceleration in LOS frame is where  ⇀    is the acceleration of missile measured by accelerometer.Since the target acceleration is unknown, it can be described by time-varying Gauss-Markov random vector  ⇀ V  , with time-varying statistical property According to the gyro random error and the attitude equation in (13), the attitude state equation can be given as follows: where  ⇀ () = [ 1  1  1 ]  and  ⇀  2 () is zero-mean uncorrelated process noise.So the system state equation is described as where where  ⇀ () is the real angular rate,   ⇀ () is the run-to-run bias, and  ⇀ V  is the noise of gyro output, which is determined by the gyro random error characteristic and obeys normal distribution [17,18].So the system measurement equation is described as where In (19) 4.2.UKF Estimation Algorithm.From ( 13), (14), and (20), we know that the LOS rate state equation and measurement equation of the strapdown imaging seeker are mainly composed of trigonometric functions and inverse trigonometric functions; the equations show strong nonlinearity, which cannot be directly estimated by a linear Kalman filter.The extended Kalman filter (EKF) is not in point for its big lineation error.So a nonlinear filtering method is considered.UKF proposed by Julier in [19][20][21] can be applied to nonlinear model directly, and the expected error is much lower than the EKF.The filter uses sigma sampling points by the unscented transformation to approximate the posterior distribution of the state vector, which avoids the linearization error and makes the filtering accuracy approach third-order Taylor expansion [22].In addition, UKF need not calculate the Jacobian matrix used in the EKF, even though the calculation is still larger than the EKF.However, we should still assume the posterior probability distributed as Gaussian.Here the UKF algorithm [23] is introduced.
The nonlinear system model can be given by where   ∈   is the state,   ∈   is the measurement,  −1 ∼ (0,  −1 ) is the Gaussian process noise, and   ∼ (0,   ) is the Gaussian measurement noise.
Using the matrix form of unscented transform (UT), the prediction and update steps of the UKF can be computed as follows.
(i) Prediction: compute the predicted state mean  −  and the predicted covariance  −  as where initial states vector X0 = [ 0 ] =  0 , the convenience and  is a scaling parameter which determines the spread of the sigma points and is usually set to a small positive value (e.g., 1−4 ≤  ≤ 1), and we can take it as 1−3. is a secondary scaling parameter which provides an extra degree of freedom to "fine-tune" the higher order moments of the approximation and can be used to reduce the overall prediction errors.When () is assumed Gaussian, a useful heuristic is to select  +  = 3.If a different distribution is assumed for (), then a different choice of  might be more appropriate.Vector   and matrix  are defined as follows: where the weights of sigma points are where  is a scaling parameter, which is defined as  =  2 ( + ) − . is a parameter used to incorporate any prior knowledge about the distribution of ;  = 2 is optimal for Gauss distributions.
(ii) Update: compute the predicted mean   and covariance of the measurement   , and the cross-covariance of the state and measurement   : Then compute the filter gain   and the updated state mean   and covariance   : Computational complexity is a key factor in real-time applications.Many algorithms have high filtering accuracy, but they cannot be used in practical applications because of their expense computational complexity and the large amount of calculation.The computational complexity of the UKF algorithm [24] is ( 3 ) and the float-point arithmetic operations required for UKF algorithm are where  and  are the dimensions of state vector and measurement vector and  and  are the float-point arithmetic operations required for time update and measurement update of the EKF in one step, which depend on the state and measurement equation.(20), BLOS angles   and   can be calculated with the above information.The strapdown seeker measures q and q ; then - filter is used to estimate the BLOS rates ̃q  and ̃q  .The algorithm based on UKF utilizes the information above to estimate LOS rates ̃q  and ̃q  .So we can verify the estimation algorithm by comparing the result with the standard data.The tilde superscript means sensors measurements or the results of calculation algorithm.

LOS Rate Accuracy Based on Different Seeker Accuracy.
Simulation conditions are as follows: in the earth coordinate, Assuming that the seeker FOV is 18 ∘ × 14.4 ∘ , for a seeker with camera resolution of 640 × 512, the pixel resolution is 0.028 ∘ /pix.When the measurement accuracy of BLOS angle is 1 pixel, 3 pixels, 5 pixels, and 10 pixels, respectively, the estimation curves and error curves of LOS angle and rate are shown in Figures 5 and 6, and the mean square errors are shown in Table 1 and Figure 7.
With reference to Table 1 and Figure 7, the BLOS angle accuracy impacts the estimation accuracy of LOS angle and rate directly.BLOS angle accuracy and LOS angle accuracy present a relation which is almost linear; also pitch channel and yaw channel have the same characteristics.One-pixel accuracy leads to LOS angle accuracy of 0.0137 ∘ .There is an approximate linear relationship between BLOS angle accuracy and LOS rate accuracy, and the scale is 0.015 ∘ /s/pix.As the seeker accuracy becomes worse, the LOS angle and rate Mathematical Problems in Engineering

LOS Rate Accuracy due to Different Gyro Accuracy.
MEMS gyros are applied to strapdown missile due to its small size, light weight, and low cost.In this section, we have simulated four gyros of different accuracy to study how gyro accuracy impacts on LOS angle and rate accuracy.Table 2 shows the gyro parameters.The estimation curves of LOS angle and rate are shown in Figure 8, and the mean square errors are shown in Table 3.We can see from Figure 8 that as gyro bias stability grows worse, LOS angle accuracy grows worse too.Under the condition of gyro accuracy 50 ∘ /h, LOS angle and rate estimated accuracy are 0.06 ∘ and 0.005 ∘ /s, respectively.The periodic oscillation curves are produced by gyro first-order   Markov process, and the period of oscillation is equal to the first-order Markov time constant.As shown in Table 3 and Figure 9, the relation between gyro accuracy and LOS angle is approximately linear, and the impact on the LOS rate has the same characteristics.The slope of gyro-LOS angle line is 0.0012 ∘ / ∘ /h and the slope of gyro-LOS rate line is 0.0002 ∘ /s/ ∘ /h separately.LOS angle and rate accuracy improve when gyro accuracy improves, because equivalent bias of gyro directly impacts the attitude, and the increase of attitude error will increase LOS angle and rate error, which is in accordance with the simulation result.

Experimental Verification
6.1.Experiment Design.In order to verify the estimation accuracy of LOS rate based on UKF in practical application, semiphysical simulation experiment platform is designed as in Figure 10, which mainly consists of real-time simulation computer, triaxial flight simulation turntable, graphic workstation, projector and screen, strapdown seeker, MEMS SINS, and so forth.The seeker and MEMS SINS are fixed on the inner frame of turntable.The distance between the seeker and screen and the target size produced by graphic workstation can meet the requirement of seeker FOV and operating range.The simulation computer calculates the flight trajectory in real time using Matlab xPC, which sends the attitude commands [, , ] to the flight simulation turntable through reflection memory network (RMN) and sends the specific force information [ ].In the semiphysical simulation experiment, the actual target size is about 3 m × 3 m, the distance from seeker to screen is about 5 m, and the target initial size in the screen is about 0.012 m × 0.012 m.The accuracy of BLOS angle actually measured by seeker is 3 pixels.STIM202 from Sensonor Company is used for MEMS IMU and its equivalent bias calibrated through experiment is 60 ∘ /h.DSP TMS320C6713 from TI Company is selected as processor.Turntable specifications are shown in Table 4.The parameters of strapdown seeker and gyro are shown in Table 5.

Result and Discussion
. The semiphysical simulation experiment is carried out for verifying LOS estimated algorithm, and the results are shown in Figures 11 and 12 and Table 6.
From Figure 12, we can see that the estimated LOS angle and rate could well approach the true value.The maximum errors of LOS vertical angle and azimuth angle are 0.345 ∘ and 0.352 ∘ , and the estimation accuracy is 0.100 ∘ and 0.097 ∘ ,  respectively.The maximum errors of LOS vertical rate and azimuth rate are 0.478 ∘ /s and 0.366 ∘ /s, and the estimation accuracy is 0.110 ∘ /s and 0.096 ∘ /s, respectively.The process noise  has an impact on the convergence rate: the smaller  is, the faster convergence rate is.Because of the hypothesized slow target motion on the ground, the parameters of target motion can be ignored in practical application.From (14), process noise  is initialized by the relative distance of missile and target , velocity, and acceleration of missile.The algorithm running time is about 2.3 ms in DSP TMS320C6713, which could meet the guidance time requirement of 20 ms.
The correctness of the estimation algorithm based on UKF is verified by the semiphysical simulation experiment, and the estimation accuracy can meet the LOS rate accuracy requirement of guidance system.

Conclusion
Strapdown imaging seeker has been widely used due to its simple structure, high reliability, and low cost.However, it cannot output the LOS rate directly.In this paper, considering the relative motion relationship between missile and target,  applications of strapdown imaging seeker and other nonlinear state estimation fields.In the future studies, by applying the LOS angular rate estimate algorithm to the missile guidance system hardware in loop simulation experiment, we can further verify the practical application ability of the algorithm.For non-Gaussian noise, our work will focus on analyzing how the presence of non-Gaussian variables affects the estimation algorithm, and another filter will be employed to overcome this problem.Also another method such as integrated navigation may be introduced to improve the LOS accuracy.The accelerations of missile and target, respectively, in LOS frame Therun-to-runbias  ⇀ V  :

Nomenclature
The noise of gyro output  ⇀ V  : The noise of strapdown seeker output  ⇀ ()  ⇀ V (): The Gaussian process noise  ⇀   (): The process noise covariance matrix of nonlinear LOS rate estimated model  ⇀  V (): The measurement noise covariance matrix of nonlinear LOS rate estimated model  ⇀  0 : I n i t i a ls t a t em e a n  ⇀  0 : I n i t i a ls t a t ec o v a r i a n c e   : The state of the system on the time step    : The measurement on the time step   −1 : The process noise on the time step  − 1  −1 : The process noise covariance matrix   : The measurement noise on the time step    : The measurement noise covariance matrix.

Figure 5 :
Figure 5: LOS vertical angle and rate estimation and error curves due to different BLOS angle accuracy.

Figure 6 :
Figure 6: LOS azimuth angle and rate estimation error curves due to different BLOS angle accuracy.

Figure 7 :
Figure 7: LOS angle and rate accuracy influenced by BLOS accuracy.

Figure 8 :
Figure 8: LOS angle and rate estimation and error curves due to different accuracy of gyro.

Figure 11 :
Figure 11: LOS angle and rate estimation curves.

Figure 12 :
Figure 12: LOS angle and rate estimation error curves.
is upward and perpendicular to     in the plumb plane which contained     , and axis     forms right-handed system with     and     .BLOS frame         is defined as follows: the origin   is selected as optical center of optical system, axis     points to the target along the LOS,     is upward and perpendicular to     in the longitudinal plane which contained     , and axis     forms right-handed system with     and     .and   are LOS vertical angle and azimuth angle;   and   are BLOS vertical angle and azimuth angle.The rotation relationship among frames is shown in Figure point,     orients upwards and directs the plumb, axis     directs the takeoff direction in the horizon plane, and axis     forms right-handed system with     and     .Body frame         is defined as follows: the origin   is selected as rotating center of missile, axis     directs the direct axis,     is upward and perpendicular to     in the longitudinal plane, and axis     forms right-handed system with     and     .LOS frame         is defined as follows: the origin   is selected as optical center of optical system, axis     points to the target along the LOS,
.The strapdown imaging seeker can directly measure BLOS angles   and   .Taking BLOS angle as measurement information  ⇀  1 = [    ]  , from (12), we can get  21 cos  3 cos  1 +  22 sin  3 −  23 cos  3 sin  1 ) − arctan  31 cos  1 +  32 tan  3 −  33 sin  1  11 cos  1 +  12 tan  3 −  13 sin  1 5,  6 ,  7 ) is the function of state vector,  1 and  2 are measurement noise of the seeker, which obey normal distribution (0, ), and the value of  is determined by the seeker's FOV and resolution and the performance of image tracker.During the guidance process, missile angular rate can be measured in real time by MEMS gyros.Gyros output can be described as

Table 1 :
LOS angle and rate accuracy with different BLOS angle accuracy.
accuracy becomes worse, too.So, it is necessary to improve the measured BLOS angle accuracy of strapdown seeker to improve LOS rate accuracy.

Table 2 :
Parameters of gyro random error model.

Table 3 :
LOS angle and rate accuracy with different gyro accuracy.
, q ].The processing computer reads the body angular rate ⇀    from MEMS gyro STIM202; thus it can compute LOS rate [ ̃q  , ̃q  ] in real time with the information of [q  , q , ⇀ 
too.Semiphysical simulation experiment result shows that the LOS rate estimation accuracy can meet the accuracy requirement of guidance system.Algorithm running time in DSP can also meet the requirement of guidance time.Consequently, the LOS rate estimation algorithm based on UKF provides a strong theoretical basis for engineering

Table 5 :
Seeker and gyro parameters table.

Table 6 :
Table of experiment result.
,   : LOS vertical angle and azimuth angle   ,   : BLOS vertical angle and azimuth angle , , : Euler angles in body axis (pitch, yaw, and roll)   : LOStransformangle   ,   : Image coordinates of the target Ω   : Angularrateof frame relative to  frame expressed in  frame    : Rotation matrix from  frame to  frame   : The element of row  and column  of the rotational matrix     1 ,  1 ,  1 : Body angular rates in inertial frame ω1 , ω1 , ω1 : Body angular rates measured in real time