Abstract
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 real-time requirements of guidance system simultaneously. Thus LOS rate estimation algorithm based on UKF provides a theoretical basis for engineering applications of strapdown imaging seeker.
1. Introduction
Optical imaging seekers have been widely used in military fields due to the high precision, such as the three-mode 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 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.
2. 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 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, 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.
(a) Angle definitions between LOS frame and earth frame
(b) Angle definitions between BLOS frame and body frame
The rotation relationship among frames is shown in Figure 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.
3. 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 and 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, , , , , , , , , , and are required. In practical applications, the body angular rates , , and 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 , can be estimated by methods of optical flow [11, 12], differential network, or other filter techniques.
3.2. Kinematic Model of LOS Rate Estimation
Relative kinematics relationship between missile and target in LOS frame can be shown by the following equation [13–15]:where is the relative distance between missile and target in LOS frame, and are the accelerations of missile and target, respectively, in LOS frame, and is the rotation matrix:
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 aswhere 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 . 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
4. LOS Rate Estimation Based on UKF
4.1. State Equation and Measurement Equation
We choose LOS azimuth angle , LOS azimuth rate , LOS vertical angle , LOS vertical rate , and attitudes , , and as state vector. Consider , where and .
According to (11), we can obtain
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. is zero-mean uncorrelated process noise. The missile acceleration in LOS frame iswhere 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 , 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 and is zero-mean uncorrelated process noise.
So the system state equation is described aswhere .
The strapdown imaging seeker can directly measure BLOS angles and . Taking BLOS angle as measurement information , from (12), we can getwhere , is the function of state vector, and are measurement noise of the seeker, which obey normal distribution , 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 , where is the real angular rate, is the run-to-run bias, and 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 aswhere and .
In (19) and (24), and are the Gaussian process noise, , , and they are related with initial state , which has mean value and covariance . For ,
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–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 bywhere is the state, is the measurement, is the Gaussian process noise, and 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 aswhere initial states vector , 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., ), and we can take it as . 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 . 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 arewhere is a scaling parameter, which is defined as . is a parameter used to incorporate any prior knowledge about the distribution of ; 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 and the float-point arithmetic operations required for UKF algorithm arewhere 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.
5. Simulation
5.1. Simulation Principle
As is known from the LOS rate model in Section 3, LOS rate accuracy is influenced by BLOS angle accuracy and the bias stability of gyro. In order to research LOS rate estimation algorithm and how the performance of seeker and gyro impacts on the estimation accuracy, the simulation model is given as in Figure 4. and are the relative acceleration between missile and target in inertial frame and body frame, which are produced by the relative motion model. and can be directly obtained by the relative motion model in earth frame. The attitude dynamics and motion module outputs triaxial angular acceleration , angular rate , and attitude angles , , and . According to (20), BLOS angles and can be calculated with the above information. The strapdown seeker measures and ; then α-β filter is used to estimate the BLOS rates and . The algorithm based on UKF utilizes the information above to estimate LOS rates and . 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.
5.2. Result and Discussion
5.2.1. LOS Rate Accuracy Based on Different Seeker Accuracy
Simulation conditions are as follows: in the earth coordinate, initial relative position is m, m, and = 300 m; initial relative velocity is = −200 m/s, = −150 m/s, and = −20 m/s; initial relative acceleration is = −6 m/s2, = −4.5 m/s2, and = −3.17 m/s2; initial attitude angle is , , and ; initial rotation angular rates of missile are zeros; moment of inertia is = 0.15 kg m2, = = 1.5 kg m2; external moment imposed on missile is , , and ; simulation step is 20 ms; simulation time is 10 s. Assuming that the seeker FOV is , for a seeker with camera resolution of , 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.
(a) LOS vertical angle estimation
(b) LOS vertical rate estimation
(c) LOS vertical angle estimation error
(d) LOS vertical rate estimation error
(a) LOS azimuth angle estimation
(b) LOS azimuth angle rate estimation
(c) LOS azimuth angle estimation error
(d) LOS azimuth rate estimation error
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 accuracy becomes worse, too. So, it is necessary to improve the measured BLOS angle accuracy of strapdown seeker to improve LOS rate accuracy.
5.2.2. 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.
(a) LOS angle estimation curves
(b) LOS rate estimation curves
(c) LOS angle estimation error curves
(d) LOS rate estimation error curves
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.
6. 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 to the processing computer through CAN bus simultaneously; it also computes the relative motion model of target and missile and sends velocity and position information to graphic workstation through RS422 serial interface. Graphic workstation observes the motion of target in BLOS frame and shadows the image on the screen by projector. The triaxial turntable simulates the body attitude according to the attitude commands made by the simulation computer. The gyro measures the angular rate of the turntable . The seeker tracks the target on the screen and outputs BLOS angle . The processing computer reads the body angular rate from MEMS gyro STIM202; thus it can compute LOS rate in real time with the information of .
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.
6.2. 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.
(a) LOS angle estimation curves
(b) LOS rate estimation curves
(a) LOS angle estimation error curve
(b) LOS rate estimation error curve
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.
7. 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, the nonlinear state model of LOS rate estimation is derived. To deal with strong nonlinearity and real-time performance, estimation algorithm based on UKF is proposed. Hitherto unknown, we have proposed and analyzed the idea that the BLOS angle accuracy and gyro accuracy are the main factors impacting on the LOS angle and rate accuracy. Simulation results show that BLOS angle accuracy and LOS rate accuracy present an approximate linear relationship; gyro accuracy and LOS rate accuracy have almost the same characteristics, 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 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.
Nomenclature
| : | LOS vertical angle and azimuth angle |
| : | BLOS vertical angle and azimuth angle |
| : | Euler angles in body axis (pitch, yaw, and roll) |
| : | LOS transform angle |
| : | Image coordinates of the target |
| : | Angular rate of frame relative to frame expressed in frame |
| : | Rotation matrix from frame to frame |
| : | The element of row and column of the rotational matrix |
| : | Body angular rates in inertial frame |
| : | Body angular rates measured in real time by gyros |
| : | Relative distance and relative velocity between the missile and target |
| : | The accelerations of missile and target, respectively, in LOS frame |
| : | The acceleration of missile measured by accelerometer |
| : | A time-varying Gauss-Markov random vector |
| : | Time-varying statistical property |
| : | Zero-mean uncorrelated process noise |
| : | Measurement noise of the seeker, which obeys normal distribution |
| : | The run-to-run bias |
| : | The noise of gyro output |
| : | The noise of strapdown seeker output |
| : | The Gaussian process noise |
| : | The process noise covariance matrix of nonlinear LOS rate estimated model |
| : | The measurement noise covariance matrix of nonlinear LOS rate estimated model |
| : | Initial state mean |
| : | Initial state covariance |
| : | The state of the system on the time step |
| : | The measurement on the time step |
| : | The process noise on the time step |
| : | The process noise covariance matrix |
| : | The measurement noise on the time step |
| : | The measurement noise covariance matrix. |
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported by the Major Science Innovation Foundation of Chinese Academy of Sciences (YYYJ-1122). The authors thank the reviewers and editors for their helpful and constructive comments.