Angle Measurement of Objects outside the Linear Field of View of a Strapdown Semi-Active Laser Seeker

Abstract

The accurate angle measurement of objects outside the linear field of view (FOV) is a challenging task for a strapdown semi-active laser seeker and is not yet well resolved. Considering the fact that the strapdown semi-active laser seeker is equipped with GPS and an inertial navigation system (INS) on a missile, in this work, we present an angle measurement method based on the fusion of the seeker’s data and GPS and INS data for a strapdown semi-active laser seeker. When an object is in the nonlinear FOV or outside the FOV, by solving the problems of space consistency and time consistency, the pitch angle and yaw angle of the object can be calculated via the fusion of the last valid angles measured by the seeker and the corresponding GPS and INS data. The numerical simulation results demonstrate the correctness and effectiveness of the proposed method.

1. Introduction

Semi-active laser guidance, which has high precision and is easy to implement, is widely used in precision-guided weapons and equipment [1,2,3,4]. The core device is the semi-active laser seeker [5,6]. It receives the laser spot reflected by an object and detects the precise coordinates of the laser spot center by using a four-quadrant detector. It then calculates the pitch angle and the yaw angle between the object and the seeker’s optical axis. However, a semi-active laser seeker cannot measure the angles when the object is out of the linear field of view (FOV) [7]. This problem is even worse for a strapdown semi-active laser seeker [8]. It is still one of the bottlenecks that restricts the overall application of strapdown semi-active laser seekers.

1.1. Detection Principle of the Strapdown Semi-Active Laser Seeker

The strapdown semi-active laser seeker is composed of a four-quadrant detector, optical system, circuit system and shell. The four-quadrant detector consists of four ${90}^{\circ }$ photosensitive sectors with the same area and the same photoelectric response [9], which are represented by I, $II$, $III$ and $IV$, respectively (as shown in Figure 1). Let R be the radius of the photosensitive surface, r be the radius of the reflected laser spot, $(x,y)$ be the coordinates of the laser spot center and $I_{i(i=I,II,III,IV)}$ be the output current of the i-th photosensitive sectors, which is proportional to the energy of the received laser spot. There are three cases in which $(x,y)$ can be detected using a four-quadrant detector. The first meets the condition $R<2r<2R$, that is the laser spot is located in the four-quadrant detector and covers each of the four photosensitive sectors (as shown in Figure 1). Then, $(x,y)$ can be calculated by [10,11]:

$$\left\{\begin{array}{c}\hfill x=\frac{\left(I_I+I_{IV}\right)-\left(I_{II}+I_{III}\right)}{I_I+I_{II}+I_{III}+I_{IV}}\\ \hfill y=\frac{\left(I_I+I_{II}\right)-\left(I_{III}+I_{IV}\right)}{I_I+I_{II}+I_{III}+I_{IV}}\end{array}\right.$$

(1)

Obviously, $(x,y)=(0,0)$ in the case shown in Figure 1a.

The origin of the seeker’s coordinate system is the origin of the four-quadrant detector, the seeker’s X axis is along the optical axis of the optical system, and the seeker’s Y and Z axes are along the Y and X axes of the four-quadrant detector, respectively. Based on $(x,y)$, the pitch angle $\epsilon$ and the yaw angle $\theta$ of the object relative to the seeker’s coordinate system can be calculated using:

$$\begin{array}{c}\hfill \epsilon =arctan\left(\frac{y}{f}\right),\theta =arctan\left(\frac{x}{f}\right)\end{array},$$

(2)

where f is the focal length of the seeker’s optical system. In this case, the satisfied area on the four-quadrant detector is called the linear area of the detector, and the corresponding FOV of the seeker is called the linear FOV. The second case is when the laser spot is within the photosensitive surface of the four-quadrant detector, but it cannot cover all four quadrants of the photosensitive sector, as shown in Figure 2a. In this case, $\left(x,y\right)$ cannot be precisely determined using Equation (1), and it can only be known in which quadrant the laser spot center is located. This area on the four-quadrant detector is called the nonlinear area, and the corresponding FOV of the seeker is called the nonlinear FOV. The third case is when the laser spot is outside the FOV of the semi-active laser seeker, as shown in Figure 2b. In this case, the four-quadrant detector cannot detect any information regarding the laser spot. Therefore, we must ensure that the object is within the linear FOV of the semi-active laser seeker.

1.2. Related Work

To ensure that the reflected laser spot lies in the linear area of the four-quadrant detector, the traditional semi-active laser seeker adopts a platform structure, that is the four-quadrant detector is installed on a complicated and high-precision servo control system [12]. The servo control system, which is composed of inertial measurement components and a dynamic follow-up system, can isolate the attitude movements of the seeker and ensure that the object is always located in the linear FOV of the seeker [4,13]. Although the platform-type semi-active laser seeker is a mature product, it has many disadvantages, such as a complex structure, high cost and large volume.

In recent years, the strapdown semi-active laser seeker has become one of the main development directions of the semi-active seeker [14,15]. It removes the high-precision servo control system and places the four-quadrant detector directly onto the longitudinal axis of the seeker. The advantages are its simpler structure, higher reliability, smaller size, lighter weight and lower cost [16]. The main disadvantage is that due to the detector moving with the seeker, the problem of objects going outside the linear FOV is exacerbated. The current approach is to increase the linear FOV of the seeker via a special optics design [17,18,19,20]. However, there are three shortcomings to this approach. First, the amount by which the linear area of the detector can be expanded and the seeker FOV can be increased via optics design is very limited. Second, when the linear FOV increases, the angle measurement accuracy of the seeker will decrease, which will affect the guidance accuracy of the seeker [8]. Third, increasing the FOV of the seeker results in a shortening of the detection range, and the detection range is very important for the terminal guidance of a missile. Therefore, the problem of angle measurement for a strapdown laser seeker when objects are outside the linear FOV still hinders the full application of the strapdown semi-active laser seeker.

Considering the fact that the strapdown semi-active laser seeker is equipped with GPS and inertial attitude measurement equipment on the missile [21,22], in this work, we make full use of GPS and INS data and propose an angle measurement method for the strapdown semi-active laser seeker through data fusion.

2. Proposed Method

When an object is outside the linear FOV, by solving the space consistency problem and the time consistency problem, the pitch angle and the yaw angle can be calculated by fusing the following data: the current GPS and inertial attitude data, the angles measured by the seeker at the last moment when the object is in the linear FOV and the corresponding GPS and inertial attitude data at that moment. The following gives the specific details of this method.

2.1. Definition of Variables

Assume $t_0$ to be the last moment at which an object is located within the linear FOV of a strapdown semi-active laser seeker, and let the pitch angle and the yaw angle of the object measured by the seeker at $t_0$ be ${\epsilon }_0$ and ${\theta }_0$, respectively. Suppose that at time $t_1$, the object is outside the linear FOV of the seeker. Then, the pitch angle ${\epsilon }_1$ and the yaw angle ${\theta }_1$ cannot be measured by the seeker and will be calculated using the proposed method. Our method needs the following data: the object position $O_T$ (longitude ${\lambda }_T$, latitude $L_T$ and height $h_T$), which is given in advance; the position of the seeker at $t_0$ (longitude ${\lambda }_0$, latitude $L_0$ and height $h_0$); the attitude data of the seeker with the yaw-pitch-roll rotation order at time $t_0$ (yaw angle ${\phi }_0$, pitch angle ${\psi }_0$ and roll angle ${\gamma }_0$) or the quaternions at $t_0$($q_{0\_0},q_{1\_0},q_{2\_0},q_{3\_0}$); the position of the seeker at $t_1$ (longitude ${\lambda }_1$, latitude $L_1$ and height $h_1$); and theattitude data of the seeker with the yaw-pitch-roll rotation order at time $t_1$ (yaw angle ${\phi }_1$, pitch angle ${\psi }_1$ and roll angle ${\gamma }_1$) or the quaternions at $t_1$($q_{0\_1},q_{1\_1},q_{2\_1},q_{3\_1}$). The above positions and attitude data can be obtained via GPS and INS [23,24]. The problem of time consistency can be solved by precisely aligning the above data with the corresponding time.

2.2. Definitions of the Coordinate Systems

To solve the space consistency problem, we define the following coordinate systems.

The Earth-centered frame $O_e$-$X_e{Y}_e{Z}_e$: The origin $O_e$ is the center of the Earth. The axis $O_e{Z}_e$ is perpendicular to the Earth’s equatorial plane and points toward the North Pole. The axis $O_e{X}_e$ lies in the Earth’s equatorial plane and points to the Greenwich meridian. The axis $O_e{Y}_e$ is perpendicular to the plane $O_e{X}_e{Z}_e$ and forms a right-hand coordinate system with $O_e{X}_e$ and $O_e{Z}_e$.

The local navigation frame $O_n$-$X_n{Y}_n{Z}_n$: The local navigation frame is defined as having a north-up-east order. The origin $O_n$ is the centroid of the seeker. The axis $O_n{Y}_n$ is collinear with the normal of the navigation frame’s reference ellipsoid at the penetration point. The axis $O_n{X}_n$ lies in the Meridian plane, is perpendicular to $O_n{Y}_n$ and points toward the north. The axis $O_n{Z}_n$ is determined according to the right-hand rule.

The body frame $O_b$-$X_b{Y}_b{Z}_b$: The origin $O_b$ is the centroid of the seeker. The axis $O_b{X}_b$ coincides with the longitudinal axis of the seeker and points toward the forward direction. The axis $O_b{Y}_b$ lies in the longitudinal symmetry plane of the seeker, is perpendicular to $O_b{X}_b$ and points upward. The axis $O_b{Z}_b$ is perpendicular to the $O_b{X}_b{Y}_b$ plane and forms a right-hand coordinate system with $O_b{X}_b$ and $O_b{Y}_b$.

The on body line-of-sight frame $O_s$-$X_s{Y}_b{Z}_s$: The origin $O_s$ is the centroid of the seeker. $O_s{X}_s$ points toward the object along the line of sight. The $O_s{Y}_s$ axis, which points upward, is on a plane that contains $O_s{X}_s$ and is perpendicular to $O_s{X}_s$ and the plane $O_b{X}_b{Z}_b$ at the same time. $O_s{Z}_s$ is determined by the right-hand rule.

2.3. Analysis and Computation of the Proposed Method

In practice, the seeker moves with the missile all the time; thus, the position and attitude of the seeker at $t_1$ are different from those at $t_0$. As shown in Figure 3, let $O_{b0}$ be the seeker position at $t_0$ and $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$ be the body frame at $O_{b0}$. Moreover, let $O_{b1}$ be the seeker position at $t_1$ and $O_{b1}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ be the body frame at $O_{b1}$. From $t_0$ to $t_1$, the body frame has both attitude movements and position movements simultaneously. Without loss of generality, we assume that the attitude movements occur first; the body frame $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$ transforms into the intermediate body frame $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$; and the, a translational movement occurs, with $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ being translated to $O_{b1}$-$X_{b1}{Y}_{b1}{Z}_{b1}$. Therefore, the analysis and calculation required to solve the space consistency problem can be conducted in two stages.

2.3.1. First Stage of the Proposed Method

In the first stage, we consider only the variation of the pitch angle and the variation of the yaw angle caused by the seeker’s attitude motion from $t_0$ to $t_1$. In this stage, the pitch angle $\epsilon {}_1^{\prime }$ and yaw angle $\theta {}_1^{\prime }$ of the object should be calculated in the intermediate body frame $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$, with the calculation involving the following coordinate transformations: $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ is transformed into the local navigation frame $O_n$-$X_n{Y}_n{Z}_n$; then $O_n$-$X_n{Y}_n{Z}_n$ is transformed into the body frame $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$; and finally, $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$ is transformed into the on body line-of-sight frame $O_s$-$X_s{Y}_s{Z}_s$. The specific steps are as follows.

Step 1: $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ is transformed into $O_n$-$X_n{Y}_n{Z}_n$ by the roll-pitch-yaw rotation order with the rotations of $-{\gamma }_1$, $-{\phi }_1$ and $-{\psi }_1$, respectively [25]. The transform matrix $C_{b1}^{n0}$ is calculated according to Equation (3).

$$C_{b1}^{n0}=\left[\begin{array}{ccc}cos{\psi }_{1}cos{\phi }_1& -cos{\gamma }_{1}sin{\psi }_{1}cos{\phi }_1+sin{\gamma }_{1}sin{\phi }_1& sin{\gamma }_{1}sin{\psi }_{1}cos{\phi }_1+cos{\gamma }_{1}sin{\phi }_1\\ sin{\psi }_1& cos{\gamma }_{1}cos{\phi }_1& -sin{\gamma }_{1}cos{\phi }_1\\ -cos{\psi }_{1}sin{\phi }_1& cos{\gamma }_{1}sin{\psi }_{1}sin{\phi }_1+sin{\gamma }_{1}cos{\phi }_1& -sin{\gamma }_{1}sin{\psi }_{1}sin{\phi }_1+cos{\gamma }_{1}cos{\phi }_1\end{array}\right]$$

(3)

To avoid the singularity problem of the Euler angles at about ${90}^{\circ }$, $C_{b1}^{n0}$ can be calculated based on the quaternions $q_{0\_1},q_{1\_1},q_{2\_1},q_{3\_1}$ according to Equation (4).

$$C_{b1}^{n0}=\left[\begin{array}{ccc}{q}_{0\_1}{q}_{0\_1}+q_{1\_1}{q}_{1\_1}-q_{2\_1}{q}_{2\_1}-q_{3\_1}{q}_{3\_1}& 2\left(q_{1\_1}{q}_{2\_1}+q_{0\_1}{q}_{3\_1}\right)& 2\left(q_{1\_1}{q}_{3\_1}-q_{0\_1}{q}_{2\_1}\right)\\ 2\left(q_{1\_1}{q}_{2\_1}-q_{0\_1}{q}_{3\_1}\right)& q_{0\_1}{q}_{0\_1}-q_{1\_1}{q}_{1\_1}+q_{2\_1}{q}_{2\_1}-q_{3\_1}{q}_{3\_1}& 2\left(q_{2\_1}{q}_{3\_1}+q_{0\_1}{q}_{1\_1}\right)\\ 2\left(q_{1\_1}{q}_{3\_1}+q_{0\_1}{q}_{2\_1}\right)& 2\left(q_{2\_1}{q}_{3\_1}-q_{0\_1}{q}_{1\_1}\right)& q_{0\_1}{q}_{0\_1}-q_{1\_1}{q}_{1\_1}-q_{2\_1}{q}_{2\_1}+q_{3\_1}{q}_{3\_1}\end{array}\right]$$

(4)

Step 2: $O_n$-$X_n{Y}_n{Z}_n$ is transformed into $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$ by the yaw-pitch-roll rotation order with rotations of ${\psi }_0$, ${\phi }_0$ and ${\gamma }_0$, respectively. The transform matrix $C_{n0}^{b0}$ is calculated according to Equation (5).

$$C_{n0}^{b0}=\left[\begin{array}{ccc}cos{\psi }_{0}cos{\phi }_0& sin{\psi }_0& -cos{\psi }_{0}sin{\phi }_0\\ -cos{\gamma }_{0}sin{\psi }_{0}cos{\phi }_0+sin{\gamma }_{0}sin{\phi }_0& cos{\gamma }_{0}cos{\phi }_0& cos{\gamma }_{0}sin{\psi }_{0}sin{\phi }_0+sin{\gamma }_{0}cos{\phi }_0\\ sin{\gamma }_{0}sin{\psi }_{0}cos{\phi }_0+cos{\gamma }_{0}sin{\phi }_0& -sin{\gamma }_{0}cos{\phi }_0& -sin{\gamma }_{0}sin{\psi }_{0}sin{\phi }_0+cos{\gamma }_{0}cos{\phi }_0\end{array}\right]$$

(5)

Similar to Step 1, $C_{n0}^{b0}$ can be calculated based on the quaternions $q_{0\_0},q_{1\_0},q_{2\_0},q_{3\_0}$ according to Equation (6):

$$C_{b0}^{n0}=\left[\begin{array}{ccc}{q}_{0\_0}{q}_{0\_0}+q_{1\_0}{q}_{1\_0}-q_{2\_0}{q}_{2\_0}-q_{3\_0}{q}_{3\_0}& 2\left(q_{1\_0}{q}_{2\_0}-q_{0\_0}{q}_{3\_0}\right)& 2\left(q_{1\_0}{q}_{3\_0}+q_{0\_0}{q}_{2\_0}\right)\\ 2\left(q_{1\_0}{q}_{2\_0}+q_{0\_0}{q}_{3\_0}\right)& q_{0\_0}{q}_{0\_0}-q_{1\_0}{q}_{1\_0}+q_{2\_0}{q}_{2\_0}-q_{3\_0}{q}_{3\_0}& 2\left(q_{2\_0}{q}_{3\_0}-q_{0\_0}{q}_{1\_0}\right)\\ 2\left(q_{1\_0}{q}_{3\_0}-q_{0\_0}{q}_{2\_0}\right)& 2\left(q_{2\_0}{q}_{3\_0}+q_{0\_0}{q}_{1\_0}\right)& q_{0\_0}{q}_{0\_0}-q_{1\_0}{q}_{1\_0}-q_{2\_0}{q}_{2\_0}+q_{3\_0}{q}_{3\_0}\end{array}\right]$$

(6)

Step 3: $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$ is transformed into $O_s$-$X_s{Y}_s{Z}_s$ by the pitch-yaw rotation order with rotations of ${\epsilon }_0$ and ${\theta }_0$. The transform matrix $C_{b0}^s$ is:

$$C_{b0}^s=\left[\begin{array}{ccc}cos{\theta }_{0}cos{\epsilon }_0& cos{\theta }_{0}sin{\epsilon }_0& -sin{\theta }_0\\ -sin{\epsilon }_0& cos{\epsilon }_0& 0\\ sin{\theta }_{0}cos{\epsilon }_0& sin{\theta }_{0}sin{\epsilon }_0& cos{\theta }_0\end{array}\right]$$

(7)

To summarize, we can obtain the transformation matrix from $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ to $O_s$-$X_s{Y}_s{Z}_s$ a:s

$$C_{b0}^{s1}=C_{b0}^s·C_{n0}^{b0}·C_{b1}^{n0}$$

(8)

Step 4: According to Equation (8), we can calculate the pitch angle $\epsilon {}_1^{\prime }$ using [25]:

$$\epsilon {}_1^{\prime }=\left\{\begin{array}{cc}arctan\left(\frac{C_{b0}^{S1}[1,2]}{C_{b0}^{S1}[1,1]}\right),\hfill & \mathrm{if}{C}_{b0}^{S1}[1,2]\le 0\mathrm{and}{C}_{b0}^{S1}[1,1]>-C_{b0}^{S1}[1,2]\\ -arctan\left(\frac{C_{b0}^{S1}[1,1]}{C_{b0}^{S1}[1,2]}\right)+\frac{\pi }{2},\hfill & \mathrm{if}{C}_{b0}^{S1}[1,1]>0\mathrm{and}{C}_{b0}^{S1}[1,1]\le -C_{b0}^{S1}[1,2]\\ -arctan\left(\frac{C_{b0}^{S1}[1,1]}{C_{b0}^{S1}[1,2]}\right)-\frac{\pi }{2},\hfill & \mathrm{if}{C}_{b0}^{S1}[1,1]\le 0\mathrm{and}{C}_{b0}^{S1}[1,1]>C_{b0}^{S1}[1,2]\\ arctan\left(\frac{C_{b0}^{S1}[1,2]}{C_{b0}^{S1}[1,1]}\right)-\pi ,\hfill & \mathrm{if}{C}_{b0}^{S1}[1,2]<0\mathrm{and}{C}_{b0}^{S1}[1,1]\le C_{b0}^{S1}[1,2]\\ arctan\left(\frac{C_{b0}^{S1}[1,2]}{C_{b0}^{S1}[1,1]}\right)+\pi ,\hfill & \mathrm{if}{C}_{b0}^{S1}[1,2]\ge 0\mathrm{and}-C_{b0}^{S1}[1,1]>C_{b0}^{S1}[1,2]\\ -arctan\left(\frac{C_{b0}^{S1}[1,1]}{C_{b0}^{S1}[1,2]}\right)+\frac{\pi }{2},\hfill & \mathrm{if}{C}_{b0}^{S1}[1,1]<0\mathrm{and}-C_{b0}^{S1}[1,1]\le C_{b0}^{S1}[1,2]\\ -arctan\left(\frac{C_{b0}^{S1}[1,1]}{C_{b0}^{S1}[1,2]}\right)+\frac{\pi }{2},\hfill & \mathrm{if}{C}_{b0}^{S1}[1,1]\ge 0\mathrm{and}{C}_{b0}^{S1}[1,1]<C_{b0}^{S1}[1,2]\\ arctan\left(\frac{C_{b0}^{S1}[1,2]}{C_{b0}^{S1}[1,1]}\right),\hfill & \mathrm{if}{C}_{b0}^{S1}[1,2]>0\mathrm{and}{C}_{b0}^{S1}[1,1]\ge C_{b0}^{S1}[1,2]\end{array}\right.,$$

(9)

and the yaw angle $\theta {}_1^{\prime }$ using:

$$\theta {}_1^{\prime }=-\mathrm{arc}sin\left({\mathrm{C}}_{\mathrm{b}0}^{{\mathrm{s}}_1}[1,3]\right)$$

(10)

2.3.2. Second Stage of the Proposed Method

In the second stage, we analyze the variation of the pitch angle and the variation of the yaw angle caused by translating $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ to $O_{b1}$-$X_{b1}{Y}_{b1}{Z}_{b1}$. As shown in Figure 3, to calculate the vector $\overrightarrow{O_{b1}{O}_T}$, we first need to calculate the vector $\overrightarrow{O_{b0}{O}_{b1}}$ and the vector $\overrightarrow{O_{b0}{O}_T}$ in frame $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$. The steps of calculating $\overrightarrow{O_{b0}{O}_{b1}}$ in $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ are as follows.

Step 1: Calculate the radius $R_{W0}$ of curvature in the prime vertical of the Earth and the radius $R_{N0}$ of curvature in the meridian of the Earth at $O_{b0}$ by:

$$\left\{\begin{array}{cc}\hfill R_{W0}& =\frac{a_e^2}{{\left({a_e}^2{cos}^2{L}_0+{b_e}^2{sin}^2{L}_0\right)}^{1/2}}\hfill \\ \hfill R_{N0}& =R_{w0}\frac{b_e^2}{a_e^2}\hfill \end{array}\right.,$$

(11)

and calculate the radius $R_{W1}$ of curvature in the prime vertical of the Earth and the radius $R_{N1}$ of curvature in the meridian of the Earth at $O_{b1}$ by:

$$\left\{\begin{array}{cc}\hfill R_{W1}& =\frac{a_e^2}{{\left({a_e}^2{cos}^2{L}_1+{b_e}^2{sin}^2{L}_1\right)}^{1/2}}\hfill \\ \hfill R_{N1}& =R_{w1}\frac{b_e^2}{a_e^2}\hfill \end{array}\right.,$$

(12)

where $a_e=$ 6,378,140 m and $b_e=a_e(1-1/\phantom{1298.257)}298.257)$ are the lengths of the Earth’s long and short half-axles, respectively.

Step 2: Calculate the coordinates ${\left[e_{x0},e_{y0},e_{z0}\right]}^{\top }$ of $O_{b0}$ in the Earth-centered frame $O_e$-$X_e{Y}_e{Z}_e$ using:

$$\left\{\begin{array}{cc}\hfill e_{x0}& =(R_{w0}+h_0)cos{L}_{0}cos{\lambda }_0\hfill \\ \hfill e_{y0}& =(R_{w0}+h_0)cos{L}_{0}sin{\lambda }_0\hfill \\ \hfill e_{z0}& =(R_{N0}+h_0)sin{L}_0\hfill \end{array}\right.,$$

(13)

and the coordinates ${\left[e_{x1},e_{y1},e_{z1}\right]}^{\top }$ of $O_{b1}$ in $O_e$-$X_e{Y}_e{Z}_e$ using:

$$\left\{\begin{array}{cc}\hfill e_{x1}& =(R_{W1}+h_1)cos{L}_{1}cos{\lambda }_1\hfill \\ \hfill e_{y1}& =(R_{W1}+h_1)cos{L}_{1}sin{\lambda }_1\hfill \\ \hfill e_{z1}& =(R_{W1}+h_1)sin{L}_1\hfill \end{array}\right.$$

(14)

Step 3: Calculate the transformation matrix $C_e^{n0}$ from the Earth-centered frame $O_e$-$X_e{Y}_e{Z}_e$ to the local navigation frame $O_n$-$X_n{Y}_n{Z}_n$ at $O_{b0}$ by:

$$C_e^{n0}=\left[\begin{array}{ccc}-sin{L}_{0}cos{\lambda }_0& -sin{L}_{0}sin{\lambda }_0& cos{L}_0\\ cos{L}_{0}cos{\lambda }_0& cos{L}_{0}sin{\lambda }_0& sin{L}_0\\ -sin{\lambda }_0& cos{\lambda }_0& 0\end{array}\right]$$

(15)

Step 4: Calculate the transformation matrix $C_{n0}^{b1}$ from the local navigation frame at $O_{b0}$ to $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ by:

$$C_{n0}^{b1}={\left(C_{b1}^{n0}\right)}^{\top }$$

(16)

From Step 1 to Step 4, $\overrightarrow{O_{b0}{O}_{b1}}$ can be obtained by:

$$\overrightarrow{O_{b0}{O}_{b1}}=C_{n0}^{b1}·C_e^{n0}·\left[\begin{array}{c}{e}_{x1}-e_{x0}\\ e_{y1}-e_{y0}\\ e_{z1}-e_{z0}\end{array}\right]$$

(17)

The vector $\overrightarrow{O_{b0}{O}_T}$ in frame $O_{b0}$-$X_{b0}{Y}_{b0}{Z}_{b0}$ is calculated as follows.

Step 1: Calculate the radius $R_{WT}$ of curvature in the prime vertical of the Earth and the radius $R_{NT}$ of curvature in the meridian of the Earth at the object position $O_T$ by:

$$\left\{\begin{array}{cc}\hfill R_{WT}& =\frac{a_e^2}{{\left({a_e}^2{cos}^2{L}_T+{b_e}^2{sin}^2{L}_T\right)}^{1/2}}\hfill \\ \hfill R_{NT}& =R_{wT}\frac{b_e^2}{a_e^2}\hfill \end{array}\right.$$

(18)

Step 2: Calculate the coordinates ${\left[e_{xT},e_{yT},e_{zT}\right]}^{\top }$ of $O_T$ in the Earth-centered frame by:

$$\left\{\begin{array}{cc}\hfill e_{xT}& =\left(R_{wT}+h_T\right)cos{L}_{T}cos{\lambda }_T\hfill \\ \hfill e_{yT}& =\left(R_{wT}+h_T\right)cos{L}_{T}sin{\lambda }_T\hfill \\ \hfill e_{zT}& =\left(R_{NT}+h_T\right)sin{L}_T\hfill \end{array}\right.$$

(19)

Step 3: Calculate the distance $Di{s}_{t_0}$ between the seeker and the object at $t_0$ by:

$$Di{s}_{t_0}=\sqrt{{\left(e_{x0}-e_{xT}\right)}^2+{\left(e_{y0}-e_{yT}\right)}^2+{\left(e_{z0}-e_{zT}\right)}^2}$$

(20)

Step 4: Since the 3-2 order pitch angle and yaw angle of the object in $O_{b0}$-$X_{b1}{Y}_{b1}{Z}_{b1}$ are $\epsilon {}_1^{\prime }$ and $\theta {}_1^{\prime }$, respectively, $\overrightarrow{O_{b0}{O}_T}$ is obtained by:

$$\overrightarrow{O_{b0}{O}_T}=\left[\begin{array}{c}Dis\_t_0·cos\theta {}_1^{\prime }·cos\epsilon {}_1^{\prime }\\ Dis\_t_0·cos\theta {}_1^{\prime }·sin\epsilon {}_1^{\prime }\\ -Dis\_t_0·sin\theta {}_1^{\prime }\end{array}\right]$$

(21)

Step 5: The vector $\overrightarrow{O_{b1}{O}_T}$ is calculated using:

$$\overrightarrow{O_{b1}{O}_T}=\overrightarrow{O_{b0}{O}_T}-\overrightarrow{O_{b0}{O}_{b1}}$$

(22)

Let the three components of $\overrightarrow{O_{b1}{O}_T}$ be ${\Delta }_{x\_t1}$, ${\Delta }_{y\_t1}$ and ${\Delta }_{z\_t1}$; then, the pitch angle ${\epsilon }_1$ and the yaw angle ${\theta }_1$ of the object at time $t_1$ are finally determined as [25]:

$${\epsilon }_1=\left\{\begin{array}{cc}arctan\left(\frac{{\Delta }_{y\_t1}}{{\Delta }_{x\_t1}}\right),\hfill & \mathrm{if}{\Delta }_{y\_t1}\le 0\mathrm{and}{\Delta }_{x\_t1}>-{\Delta }_{y\_t1}\\ -arctan\left(\frac{{\Delta }_{x\_t1}}{{\Delta }_{y\_t1}}\right)-\frac{\pi }{2},\hfill & \mathrm{if}{\Delta }_{x\_t1}>0\mathrm{and}{\Delta }_{x\_t1}\le -{\Delta }_{y\_t1}\\ -arctan\left(\frac{{\Delta }_{x\_t1}}{{\Delta }_{y\_t1}}\right)-\frac{\pi }{2},\hfill & \mathrm{if}{\Delta }_{x\_t1}\le 0\mathrm{and}{\Delta }_{x\_t1}>{\Delta }_{y\_t1}\\ arctan\left(\frac{{\Delta }_{y\_t1}}{{\Delta }_{x\_t1}}\right)-\pi ,\hfill & \mathrm{if}{\Delta }_{y\_t1}<0\mathrm{and}{\Delta }_{x\_t1}\le {\Delta }_{y\_t1}\\ arctan\left(\frac{{\Delta }_{y\_t1}}{{\Delta }_{x\_t1}}\right)+\pi ,\hfill & \mathrm{if}{\Delta }_{y\_t1}\ge 0\mathrm{and}-{\Delta }_{x\_t1}>{\Delta }_{y\_t1}\\ -arctan\left(\frac{{\Delta }_{x\_t1}}{{\Delta }_{y\_t1}}\right)+\frac{\pi }{2},\hfill & \mathrm{if}{\Delta }_{x\_t1}<0\mathrm{and}-{\Delta }_{x\_t1}\le {\Delta }_{y\_t1}\\ -arctan\left(\frac{{\Delta }_{x\_t1}}{{\Delta }_{y\_t1}}\right)+\frac{\pi }{2},\hfill & \mathrm{if}{\Delta }_{x\_t1}\ge 0\mathrm{and}{\Delta }_{x\_t1}<{\Delta }_{y\_t1}\\ arctan\left(\frac{{\Delta }_{y\_t1}}{{\Delta }_{x\_t1}}\right),\hfill & \mathrm{if}{\Delta }_{y\_t1}>0\mathrm{and}{\Delta }_{x\_t1}\ge {\Delta }_{y\_t1}\end{array}\right.$$

(23)

and:

$${\theta }_1=\mathrm{arc}tan\left(\frac{{\Delta }_{z\_t1}}{\sqrt{{\Delta }_{x\_t1}^2+{\Delta }_{y\_t1}^2}}\right)$$

(24)

3. Numerical Simulation Results

3.1. Numerical Simulation Setups

We carry out two simulation experiments using MATLAB to verify and evaluate the proposed method. The setups of the simulations are as follows: the measurement period of the seeker is 50 ms; the linear FOV of the seeker is $\pm {10}^{\circ }$; and the FOV of the seeker is $\pm {20}^{\circ }$. In the simulations, the seeker performs a sinusoidal-like motion to place the object at different positions within the seeker’s FOV. As shown in

Table 1. Relationship between the object position and the seeker FOV.

Time t (s)	0	0.05–0.90	0.95–2.75	2.80–3.65	3.7–5.05	5.10–6.25	6.30–7.40	7.45–8.5
Position relative to FOV	Linear	Nonlinear	Outside	Nonlinear	Linear	Nonlinear	Outside	Nonlinear

, the object is in the linear FOV in the first frame, and the pitch angle and yaw angle are $6.752\circ$ and $-{7.187}^{\circ }$, respectively. The object exits the linear FOV in the second frame (0.05 s) and stays in the nonlinear FOV from 0.05 s to 0.90 s. Then, the object leaves the FOV of the seeker at 0.95 s, re-enters the nonlinear FOV at 2.80 s and stays in the nonlinear FOV until 3.65 s. Next, it enters the linear FOV again and stays in the linear FOV until 5.05 s. Subsequently, the object again exits the linear FOV at 5.10 s and stays in the nonlinear FOV until 6.25 s. The object then stays outside the FOV between 6.30 s and 7.40 s before entering the nonlinear FOV at 7.45 s. In the simulations, we calculate the pitch angle and the yaw angle via the proposed method when the object exits the linear FOV of the seeker and evaluate the method’s performance by comparing it to the ground truth.

3.2. Numerical Simulation Results

The purpose of the first numerical simulation is to verify the correctness of the proposed method. In the numerical simulation, the GPS and INS data do not contain errors. The results are shown in Figure 4. Figure 4a shows the pitch angle results, while Figure 4b shows the yaw angle results. The blue `∘’ represents the ground truth, the red `+’ the result of the proposed method and the green `*’ the error between the proposed algorithm and the ground truth. It can be seen that both the pitch angle errors and the yaw angle errors are very close to zero throughout the simulation. To be more precise, the absolute values of both the pitch angle errors and the yaw angle errors are less then $1\times {10}^{-6}$, which are caused by the numerical truncation of the simulation software. Therefore, this simulation result proves the correctness of the proposed method.

The purpose of the second numerical simulation is to evaluate the angle measurement accuracy of the proposed method when the object is outside the linear FOV. In the numerical simulation, both the GPS data and INS data contain errors. The error in terms of the GPS latitude, longitude and height is 10 m. To make this numerical simulation more challenging, we assume that the INS uses a low-precision MEMS gyroscope [26,27] and that the angle drift ratio is ${20}^{\circ }/$h. In addition, we assume that the INS has been working for 60 s after the initial alignment. Thus, the initial attitude error of the INS is ${0.333}^{\circ }$. The attitude error of the INS during the numerical simulation is represented by the green curve in Figure 5c. The simulation results are shown in Figure 5: Figure 5a shows the pitch angle results; Figure 5b shows the yaw angle results; and Figure 5c shows the error between the proposed method and the ground truth. In Figure 5a,b, the blue curve represents the ground truth, while the red curve represents the result of the proposed method. In Figure 5c, the red `+’ represents the pitch angle error, and the blue `∘’ represents the yaw angle error. It can be seen that as time progresses, the object enters the nonlinear FOV or comes out of the FOV, and the angular measurement error of this method increases with the increase of the attitude error of the INS. Specifically, within the 3.6 s when the object leaves the linear FOV for the first time, as the attitude error of the INS increases to ${0.354}^{\circ }$, the absolute value of the pitch angle error of the proposed method increases from 0 to ${0.14}^{\circ }$, and the absolute value of the yaw angle error increases from 0 to ${0.068}^{\circ }$. Furthermore, we can conclude that under the above GPS and INS error conditions, this method can ensure that the angular measurement error is less than ${0.2}^{\circ }$ in the 6.5 s when the object is outside the linear FOV.

From the theoretical derivation and simulation process, it can be seen that when the object is outside the linear FOV, the angle measurement accuracy of the method increases as the GPS accuracy and INS accuracy increase and as time reduces. In practice, the accuracy of the INS is higher than ${20}^{\circ }/$h, and the amount of time the object spends outside the linear FOV does not exceed 3.6 s. Thus, the proposed method can achieve better angle measurement performance than exhibited during the numerical simulation.

4. Conclusions

To solve the problem in which a strapdown semi-active laser seeker cannot measure the angles of objects outside the linear FOV, we make full use of GPS and INS data and propose an angle measurement method based on information fusion. When an object is within the nonlinear FOV or outside the FOV, the pitch angle and the yaw angle of the object can be calculated via a fusion of the last valid angles measured by the seeker and the corresponding GPS and INS data. The numerical simulation results show that the proposed method can tolerate a certain amount of GPS and INS errors and ensure the angular measurement error is less than ${0.2}^{\circ }$ in the 6.5 s when the object is outside the linear FOV. In general, the proposed method is simple, accurate and effective for angle measurement of objects outside the linear FOV of a strapdown semi-active laser seeker.