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

Authors: Tingting Sun; Hairong Chu; Baiqiang Zhang; Hongguang Jia; Lihong Guo; Yue Zhang; Mingyue Zhang
Journal: Mathematical Problems in Engineering
 (2015)

---

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


---

## Body

## 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. Section2 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 frameOexeyeze, body frame Obxbybzb, LOS frame Osxsyszs, and BLOS frame Olxlylzl are shown in Figure 1, where the subscripts e, b, s, and l 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 Oexeyeze is defined as follows: the origin Oe is selected as launching point, Oeye orients upwards and directs the plumb, axis Oexe directs the takeoff direction in the horizon plane, and axis Oeze forms right-handed system with Oexe and Oeye. Body frame Obxbybzb is defined as follows: the origin Ob is selected as rotating center of missile, axis Obxb directs the direct axis, Obyb is upward and perpendicular to Obxb in the longitudinal plane, and axis Oeze forms right-handed system with Obxb and Obyb. LOS frame Osxsyszs is defined as follows: the origin Os is selected as optical center of optical system, axis Osxs points to the target along the LOS, Osys is upward and perpendicular to Osxs in the plumb plane which contained Osxs, and axis Oszs forms right-handed system with Osxs and Osys. BLOS frame Olxlylzl is defined as follows: the origin Ol is selected as optical center of optical system, axis Olxl points to the target along the LOS, Olyl is upward and perpendicular to Olxl in the longitudinal plane which contained Olxl, and axis Olzl forms right-handed system with Olxl and Olyl. qγ and qλ are LOS vertical angle and azimuth angle; qα and qβ are BLOS vertical angle and azimuth angle.Figure 1
Angle definitions of frames.
(a)
Angle definitions between LOS frame and earth frame
(b)
Angle definitions between BLOS frame and body frameThe rotation relationship among frames is shown in Figure2, where qc is LOS transform angle. ϑ, ψ, and γ are pitch, yaw, and roll, respectively. L(…) is the direction cosine matrix between frames.Figure 2
Rotation relationships among coordinate 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 axisObxb and the coordinate of target on the focal plane can be calculated through the image processing algorithm. Combined with focal length f, BLOS vertical angle qα and azimuth angle qβ can be calculated. The measurement schematic is depicted as in Figure 3, where (xt,yt) expresses the image coordinate of the target.Figure 3
Schematic of BLOS angle measurement.


## 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 Figure2, 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Ωese in earth frame:(1)Ωese=CbeΩebb+Ωblb+CseΩlss,(2)Ωlss=-q˙c00T,(3)Ωblb=q˙αsin⁡qβq˙βq˙αcos⁡qβT,(4)Ωebb=ωx1ωy1ωz1T,where Ωlss is the angular rate of LOS frame relative to BLOS frame expressed in LOS frame. Ωblb is the angular rate of BLOS frame relative to body frame expressed in body frame. Ωebb 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:(5)Ωese=q˙γsin⁡qλq˙λq˙γcos⁡qλT.By synthesizing (1)–(5), the standard LOS rates q˙γ and q˙λ can be obtained:(6)
q
˙
γ
=
1
1
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
2
ω
x
1
+
q
˙
α
sin
⁡
q
β
R
22
R
31
-
R
11
R
32
sin
⁡
q
α
+
R
11
R
33
-
R
13
R
31
cos
⁡
q
α
sin
⁡
q
β
+
ω
y
1
+
q
˙
β
·
R
11
R
32
-
R
12
R
31
cos
⁡
q
α
cos
⁡
q
β
+
R
12
R
33
-
R
13
R
32
cos
⁡
q
α
sin
⁡
q
β
+
ω
z
1
+
q
˙
α
cos
⁡
q
β
·
R
11
R
33
-
R
13
R
31
cos
⁡
q
α
cos
⁡
q
β
+
R
12
R
33
-
R
13
R
32
sin
⁡
q
α
,
q
˙
λ
=
1
1
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
2
ω
x
1
+
q
˙
α
sin
⁡
q
β
R
21
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
sin
⁡
q
α
+
ω
y
1
+
q
˙
β
R
22
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
sin
⁡
q
α
+
ω
z
1
+
q
˙
α
cos
⁡
q
β
R
23
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
sin
⁡
q
α
,where Rij is the element of row i and column j of the rotational matrix Ceb from earth frame Oexeyeze to body frame Obxbybzb.To calculate LOS angle and rate,ωx1, ωy1, ωz1, ϑ, ψ, γ, qα, qβ, q˙α, and q˙β are required. In practical applications, the body angular rates ωx1, ωy1, and ωz1 can be directly measured by inertial measurement unit (IMU). The attitudes ϑ, ψ, and γ can be calculated by strapdown inertial navigation system (SINS). The BLOS angles qα and qβ 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.


### 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]:(7)a⃑st=a⃑sm+⁡R⃑¨s+ω~˙sR⃑s+2ω~sR⃑˙s+ω~sω~sR⃑s,where R⃑s=[R00]T is the relative distance between missile and target in LOS frame, a⃑sm and a⃑st are the accelerations of missile and target, respectively, in LOS frame, and ω~s is the rotation matrix:(8)ω~s=0-ωszωsyωsz0-ωsx-ωsyωsx0.We can obtain that(9)ω˙sz=-2R˙Rωsz+asyt-asymR-ωsxωsy,ω˙sy=-2R˙Rωsy-aszt-aszmR+ωsxωsz.According to the transformation between inertial frame and LOS frame, the relationship between LOS angle[qγ,qλ]T and the angular rate ω⃑s can be described as(10)ω⃑s=ωsxωsyωsz=Lqγ,qλ0q˙λ0+00q˙γ,where Lqγ,qλ is the transformation matrix from inertial frame to LOS frame.Substituting (10) into (9), the movement equations of LOS can be derived:(11)q¨λ=-2R˙Rq˙λ-aszt-aszmRcos⁡qγ+2q˙λq˙γtan⁡qγ,q¨γ=-2R˙Rq˙γ+asyt-asymR-q˙2λsin⁡qγcos⁡qγ.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[R00]T. Using the coordinate and transformation relationship between different frames, qα and qβ can be calculated by the following equation:(12)
q
α
=
arcsin
⁡
R
21
cos
⁡
q
γ
cos
⁡
q
λ
+
R
22
sin
⁡
q
γ
-
R
23
cos
⁡
q
γ
sin
⁡
q
λ
,
q
β
=
-
arctan
⁡
R
31
cos
⁡
q
λ
+
R
32
tan
⁡
q
γ
-
R
33
sin
⁡
q
λ
R
11
cos
⁡
q
λ
+
R
12
tan
⁡
q
γ
-
R
13
sin
⁡
q
λ
.By representing missile attitude with Euler angles, the set of kinematic equations [16] is given as (13)
ϑ
˙
ψ
˙
γ
˙
=
0
sin
⁡
γ
cos
⁡
γ
0
cos
⁡
γ
cos
⁡
ϑ
-
sin
⁡
γ
cos
⁡
ϑ
1
-
tan
⁡
ϑ
cos
⁡
γ
tan
⁡
ϑ
sin
⁡
γ
ω
x
1
ω
y
1
ω
z
1
.


## 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 Figure2, 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Ωese in earth frame:(1)Ωese=CbeΩebb+Ωblb+CseΩlss,(2)Ωlss=-q˙c00T,(3)Ωblb=q˙αsin⁡qβq˙βq˙αcos⁡qβT,(4)Ωebb=ωx1ωy1ωz1T,where Ωlss is the angular rate of LOS frame relative to BLOS frame expressed in LOS frame. Ωblb is the angular rate of BLOS frame relative to body frame expressed in body frame. Ωebb 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:(5)Ωese=q˙γsin⁡qλq˙λq˙γcos⁡qλT.By synthesizing (1)–(5), the standard LOS rates q˙γ and q˙λ can be obtained:(6)
q
˙
γ
=
1
1
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
2
ω
x
1
+
q
˙
α
sin
⁡
q
β
R
22
R
31
-
R
11
R
32
sin
⁡
q
α
+
R
11
R
33
-
R
13
R
31
cos
⁡
q
α
sin
⁡
q
β
+
ω
y
1
+
q
˙
β
·
R
11
R
32
-
R
12
R
31
cos
⁡
q
α
cos
⁡
q
β
+
R
12
R
33
-
R
13
R
32
cos
⁡
q
α
sin
⁡
q
β
+
ω
z
1
+
q
˙
α
cos
⁡
q
β
·
R
11
R
33
-
R
13
R
31
cos
⁡
q
α
cos
⁡
q
β
+
R
12
R
33
-
R
13
R
32
sin
⁡
q
α
,
q
˙
λ
=
1
1
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
2
ω
x
1
+
q
˙
α
sin
⁡
q
β
R
21
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
sin
⁡
q
α
+
ω
y
1
+
q
˙
β
R
22
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
sin
⁡
q
α
+
ω
z
1
+
q
˙
α
cos
⁡
q
β
R
23
-
R
21
cos
⁡
q
α
cos
⁡
q
β
+
R
22
sin
⁡
q
α
-
R
23
cos
⁡
q
α
sin
⁡
q
β
sin
⁡
q
α
,where Rij is the element of row i and column j of the rotational matrix Ceb from earth frame Oexeyeze to body frame Obxbybzb.To calculate LOS angle and rate,ωx1, ωy1, ωz1, ϑ, ψ, γ, qα, qβ, q˙α, and q˙β are required. In practical applications, the body angular rates ωx1, ωy1, and ωz1 can be directly measured by inertial measurement unit (IMU). The attitudes ϑ, ψ, and γ can be calculated by strapdown inertial navigation system (SINS). The BLOS angles qα and qβ 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.


## 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]:(7)a⃑st=a⃑sm+⁡R⃑¨s+ω~˙sR⃑s+2ω~sR⃑˙s+ω~sω~sR⃑s,where R⃑s=[R00]T is the relative distance between missile and target in LOS frame, a⃑sm and a⃑st are the accelerations of missile and target, respectively, in LOS frame, and ω~s is the rotation matrix:(8)ω~s=0-ωszωsyωsz0-ωsx-ωsyωsx0.We can obtain that(9)ω˙sz=-2R˙Rωsz+asyt-asymR-ωsxωsy,ω˙sy=-2R˙Rωsy-aszt-aszmR+ωsxωsz.According to the transformation between inertial frame and LOS frame, the relationship between LOS angle[qγ,qλ]T and the angular rate ω⃑s can be described as(10)ω⃑s=ωsxωsyωsz=Lqγ,qλ0q˙λ0+00q˙γ,where Lqγ,qλ is the transformation matrix from inertial frame to LOS frame.Substituting (10) into (9), the movement equations of LOS can be derived:(11)q¨λ=-2R˙Rq˙λ-aszt-aszmRcos⁡qγ+2q˙λq˙γtan⁡qγ,q¨γ=-2R˙Rq˙γ+asyt-asymR-q˙2λsin⁡qγcos⁡qγ.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[R00]T. Using the coordinate and transformation relationship between different frames, qα and qβ can be calculated by the following equation:(12)
q
α
=
arcsin
⁡
R
21
cos
⁡
q
γ
cos
⁡
q
λ
+
R
22
sin
⁡
q
γ
-
R
23
cos
⁡
q
γ
sin
⁡
q
λ
,
q
β
=
-
arctan
⁡
R
31
cos
⁡
q
λ
+
R
32
tan
⁡
q
γ
-
R
33
sin
⁡
q
λ
R
11
cos
⁡
q
λ
+
R
12
tan
⁡
q
γ
-
R
13
sin
⁡
q
λ
.By representing missile attitude with Euler angles, the set of kinematic equations [16] is given as (13)
ϑ
˙
ψ
˙
γ
˙
=
0
sin
⁡
γ
cos
⁡
γ
0
cos
⁡
γ
cos
⁡
ϑ
-
sin
⁡
γ
cos
⁡
ϑ
1
-
tan
⁡
ϑ
cos
⁡
γ
tan
⁡
ϑ
sin
⁡
γ
ω
x
1
ω
y
1
ω
z
1
.


## 4. LOS Rate Estimation Based on UKF

### 4.1. State Equation and Measurement Equation

We choose LOS azimuth angleqλ, LOS azimuth rate q˙λ, LOS vertical angle qγ, LOS vertical rate q˙γ, and attitudes ϑ, ψ, and γ as state vector. Consider X⃑=[X⃑1X⃑2]T, where X⃑1=[qλq˙λqγq˙γ]T and X⃑2=[ϑψγ]T.According to (11), we can obtain(14)
q
˙
λ
q
¨
λ
q
˙
γ
q
¨
γ
=
x
˙
1
x
˙
2
x
˙
3
x
˙
4
=
x
2
-
2
R
˙
R
x
2
+
a
s
z
m
R
cos
⁡
x
3
+
2
x
2
x
4
tan
⁡
x
3
x
4
-
2
R
˙
R
x
4
-
a
s
y
m
R
-
x
2
2
sin
⁡
x
1
cos
⁡
x
3
+
0
-
a
s
z
t
R
cos
⁡
x
3
0
a
s
y
t
R
,
(15)
X
⃑
˙
1
t
=
f
1
X
⃑
1
t
,
R
t
,
R
˙
t
,
a
⃑
s
m
t
,
t
+
w
⃑
1
t
.Comparing with the velocity of missile, the velocity of ground target is slow, so the missile-target relative velocityR˙ is approximated to Vm. The information of relative distance R and approaching velocity R˙ can be calculated approximately by SINS. w⃑1t is zero-mean uncorrelated process noise. The missile acceleration in LOS frame is(16)a⃑sm=Cesa⃑em=CesCbea⃑bm,where a⃑bm 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⃑k, with time-varying statistical property q⃑k=Ev⃑k.According to the gyro random error and the attitude equation in (13), the attitude state equation can be given as follows:(17)X⃑˙2=f2X⃑2,ω⃑t,t+w⃑2t,where ω⃑t=[ωx1ωy1ωz1]T and w⃑2t is zero-mean uncorrelated process noise.So the system state equation is described as(18)X⃑˙1tX⃑˙2t=f1X⃑1t,Rt,R˙t,a⃑smt,tf2X⃑2t,ω⃑t,t+w⃑1tw⃑2t,(19)X⃑˙t=fX⃑t,Rt,R˙t,a⃑smt,ω⃑t,t+w⃑t,where w⃑=[w⃑1w⃑2]T.The strapdown imaging seeker can directly measure BLOS anglesqα and qβ. Taking BLOS angle as measurement information z⃑1=[qαqβ]T, from (12), we can get(20)qαqβ=arcsin⁡R21cos⁡x3cos⁡x1+R22sin⁡x3-R23cos⁡x3sin⁡x1-arctan⁡R31cos⁡x1+R32tan⁡x3-R33sin⁡x1R11cos⁡x1+R12tan⁡x3-R13sin⁡x1+r1r2,(21)z⃑1=h1X⃑1t,Rij,t+v⃑q,where v⃑q=[r1r2]T, Rij=fij(x5,x6,x7) is the function of state vector, r1 and r2 are measurement noise of the seeker, which obey normal distribution N(0,r), and the value of r 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 asz⃑2=[ω~x1ω~y1ω~z1]T, (22)z⃑2=ω⃑t+δω⃑t+v⃑ω,where ω⃑t is the real angular rate, δω⃑t 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(23)z⃑1z⃑2=h1X⃑1t,Rij,th2ω⃑t,t+v⃑qv⃑ω,(24)Z⃑t=hX⃑t,ω⃑t,δω⃑t,t+v⃑t,where Z⃑t=[z⃑1z⃑2]Tand v⃑t=[v⃑qv⃑ω]T.In (19) and (24), w⃑t and v⃑t are the Gaussian process noise, w⃑t~N0,Q⃑wt, v⃑t~N0,R⃑vt, and they are related with initial state X⃑0, which has mean value X⃑0 and covariance P⃑0. For t≥t0,(25)Ew⃑t=Ev⃑t=0⃑Ew⃑tw⃑Tτ=Q⃑wtδt-τEv⃑tv⃑Tτ=R⃑vtδt-τEw⃑tv⃑Tτ=EX⃑0w⃑Tτ=EX⃑0v⃑Tτ=0⃑.


### 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 by(26)xk=fxk-1,k-1+qk-1,yk=hxk,k+rk,where xk∈Rn is the state, yk∈Rm is the measurement, qk-1~N0,Qk-1 is the Gaussian process noise, and rk~N0,Rk 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 meanmk- and the predicted covariance Pk- as(27)Xk-1=mk-1⋯mk-1+c0Pk-1-Pk-1,X^k=fXk-1,k-1,mk-=X^kwm,Pk-=X^kWX^kT+Qk-1,where initial states vector X^0=EX0=m0, the convenience P0=E[(X0-X^0)(X0-X^0)T], c=α2(n+κ), and α is a scaling parameter which determines the spread of the sigma points and is usually set to a small positive value (e.g., 1e-4≤α≤1), and we can take it as 1e-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 x(k) is assumed Gaussian, a useful heuristic is to select n+κ=3. If a different distribution is assumed for x(k), then a different choice of κ might be more appropriate. Vector wm and matrix W are defined as follows:(28)wm=Wm0⋯Wm2nT,W=I-wm⋯wmdiag⁡Wc0⋯Wc2n·I-wm⋯wmT,where the weights of sigma points are(29)Wm0=λn+λ,Wc0=λn+λ+1-α2+β2,Wmi=λ2n+λ,i=1,…,2n,Wci=λ2n+λ,i=1,…,2n,where λ is a scaling parameter, which is defined as λ=α2(n+κ)-n. β is a parameter used to incorporate any prior knowledge about the distribution of x; β=2 is optimal for Gauss distributions.(ii) Update: compute the predicted meanμk and covariance of the measurement Sk, and the cross-covariance of the state and measurement Ck:(30)Xk-=mk-⋯mk-+c0Pk--Pk-,Yk-=hXk-,k,μk=Yk-wm,Sk=Yk-WYk-T+Rk,Ck=Xk-WYk-T.Then compute the filter gainKk and the updated state mean mk and covariance Pk:(31)Kk=CkSk-1,mk=mk-+Kkyk-μk,Pk=Pk--KkSkKkT.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 O(n3) and the float-point arithmetic operations required for UKF algorithm are(32)N=4n3+4+5mn2+6n+2m2+5mn+m+2n+1A+B,where n and m are the dimensions of state vector and measurement vector and A and B 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.


## 4.1. State Equation and Measurement Equation

We choose LOS azimuth angleqλ, LOS azimuth rate q˙λ, LOS vertical angle qγ, LOS vertical rate q˙γ, and attitudes ϑ, ψ, and γ as state vector. Consider X⃑=[X⃑1X⃑2]T, where X⃑1=[qλq˙λqγq˙γ]T and X⃑2=[ϑψγ]T.According to (11), we can obtain(14)
q
˙
λ
q
¨
λ
q
˙
γ
q
¨
γ
=
x
˙
1
x
˙
2
x
˙
3
x
˙
4
=
x
2
-
2
R
˙
R
x
2
+
a
s
z
m
R
cos
⁡
x
3
+
2
x
2
x
4
tan
⁡
x
3
x
4
-
2
R
˙
R
x
4
-
a
s
y
m
R
-
x
2
2
sin
⁡
x
1
cos
⁡
x
3
+
0
-
a
s
z
t
R
cos
⁡
x
3
0
a
s
y
t
R
,
(15)
X
⃑
˙
1
t
=
f
1
X
⃑
1
t
,
R
t
,
R
˙
t
,
a
⃑
s
m
t
,
t
+
w
⃑
1
t
.Comparing with the velocity of missile, the velocity of ground target is slow, so the missile-target relative velocityR˙ is approximated to Vm. The information of relative distance R and approaching velocity R˙ can be calculated approximately by SINS. w⃑1t is zero-mean uncorrelated process noise. The missile acceleration in LOS frame is(16)a⃑sm=Cesa⃑em=CesCbea⃑bm,where a⃑bm 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⃑k, with time-varying statistical property q⃑k=Ev⃑k.According to the gyro random error and the attitude equation in (13), the attitude state equation can be given as follows:(17)X⃑˙2=f2X⃑2,ω⃑t,t+w⃑2t,where ω⃑t=[ωx1ωy1ωz1]T and w⃑2t is zero-mean uncorrelated process noise.So the system state equation is described as(18)X⃑˙1tX⃑˙2t=f1X⃑1t,Rt,R˙t,a⃑smt,tf2X⃑2t,ω⃑t,t+w⃑1tw⃑2t,(19)X⃑˙t=fX⃑t,Rt,R˙t,a⃑smt,ω⃑t,t+w⃑t,where w⃑=[w⃑1w⃑2]T.The strapdown imaging seeker can directly measure BLOS anglesqα and qβ. Taking BLOS angle as measurement information z⃑1=[qαqβ]T, from (12), we can get(20)qαqβ=arcsin⁡R21cos⁡x3cos⁡x1+R22sin⁡x3-R23cos⁡x3sin⁡x1-arctan⁡R31cos⁡x1+R32tan⁡x3-R33sin⁡x1R11cos⁡x1+R12tan⁡x3-R13sin⁡x1+r1r2,(21)z⃑1=h1X⃑1t,Rij,t+v⃑q,where v⃑q=[r1r2]T, Rij=fij(x5,x6,x7) is the function of state vector, r1 and r2 are measurement noise of the seeker, which obey normal distribution N(0,r), and the value of r 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 asz⃑2=[ω~x1ω~y1ω~z1]T, (22)z⃑2=ω⃑t+δω⃑t+v⃑ω,where ω⃑t is the real angular rate, δω⃑t 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(23)z⃑1z⃑2=h1X⃑1t,Rij,th2ω⃑t,t+v⃑qv⃑ω,(24)Z⃑t=hX⃑t,ω⃑t,δω⃑t,t+v⃑t,where Z⃑t=[z⃑1z⃑2]Tand v⃑t=[v⃑qv⃑ω]T.In (19) and (24), w⃑t and v⃑t are the Gaussian process noise, w⃑t~N0,Q⃑wt, v⃑t~N0,R⃑vt, and they are related with initial state X⃑0, which has mean value X⃑0 and covariance P⃑0. For t≥t0,(25)Ew⃑t=Ev⃑t=0⃑Ew⃑tw⃑Tτ=Q⃑wtδt-τEv⃑tv⃑Tτ=R⃑vtδt-τEw⃑tv⃑Tτ=EX⃑0w⃑Tτ=EX⃑0v⃑Tτ=0⃑.


## 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 by(26)xk=fxk-1,k-1+qk-1,yk=hxk,k+rk,where xk∈Rn is the state, yk∈Rm is the measurement, qk-1~N0,Qk-1 is the Gaussian process noise, and rk~N0,Rk 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 meanmk- and the predicted covariance Pk- as(27)Xk-1=mk-1⋯mk-1+c0Pk-1-Pk-1,X^k=fXk-1,k-1,mk-=X^kwm,Pk-=X^kWX^kT+Qk-1,where initial states vector X^0=EX0=m0, the convenience P0=E[(X0-X^0)(X0-X^0)T], c=α2(n+κ), and α is a scaling parameter which determines the spread of the sigma points and is usually set to a small positive value (e.g., 1e-4≤α≤1), and we can take it as 1e-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 x(k) is assumed Gaussian, a useful heuristic is to select n+κ=3. If a different distribution is assumed for x(k), then a different choice of κ might be more appropriate. Vector wm and matrix W are defined as follows:(28)wm=Wm0⋯Wm2nT,W=I-wm⋯wmdiag⁡Wc0⋯Wc2n·I-wm⋯wmT,where the weights of sigma points are(29)Wm0=λn+λ,Wc0=λn+λ+1-α2+β2,Wmi=λ2n+λ,i=1,…,2n,Wci=λ2n+λ,i=1,…,2n,where λ is a scaling parameter, which is defined as λ=α2(n+κ)-n. β is a parameter used to incorporate any prior knowledge about the distribution of x; β=2 is optimal for Gauss distributions.(ii) Update: compute the predicted meanμk and covariance of the measurement Sk, and the cross-covariance of the state and measurement Ck:(30)Xk-=mk-⋯mk-+c0Pk--Pk-,Yk-=hXk-,k,μk=Yk-wm,Sk=Yk-WYk-T+Rk,Ck=Xk-WYk-T.Then compute the filter gainKk and the updated state mean mk and covariance Pk:(31)Kk=CkSk-1,mk=mk-+Kkyk-μk,Pk=Pk--KkSkKkT.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 O(n3) and the float-point arithmetic operations required for UKF algorithm are(32)N=4n3+4+5mn2+6n+2m2+5mn+m+2n+1A+B,where n and m are the dimensions of state vector and measurement vector and A and B 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 Section3, 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. a⃑i and a⃑bare the relative acceleration between missile and target in inertial frame and body frame, which are produced by the relative motion model. q˙γ and q˙λ can be directly obtained by the relative motion model in earth frame. The attitude dynamics and motion module outputs triaxial angular acceleration ω˙⃑mi, angular rate ω⃑mi, and attitude angles ϑ, ψ, and γ. According to (20), BLOS angles qα and qβ 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.Figure 4
Block diagram of LOS rate decoupling 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 isx=2000 m, y=1500 m, and z = 300 m; initial relative velocity is vx = −200 m/s, vy = −150 m/s, and vz = −20 m/s; initial relative acceleration is ax = −6 m/s2, ay = −4.5 m/s2, and az = −3.17 m/s2; initial attitude angle is ϑ=0°, ψ=0°, and γ=0°; initial rotation angular rates of missile are zeros; moment of inertia is Jx = 0.15 kg m2, Jy = Jz = 1.5 kg m2; external moment imposed on missile is Mx=0.01sin⁡(2πt)N·m, My=0.05sin⁡(2πt)N·m, and Mz=0.1sin⁡(2πt)N·m; simulation step is 20 ms; simulation time is 10 s. 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.Table 1
LOS angle and rate accuracy with different BLOS angle accuracy.
Accuracy
q
γ (°)
q
˙
γ (°/s)
q
λ (°)
q
˙
λ (°/s)
1 pix
0.014
0.055
0.014
0.033
3 pixs
0.042
0.071
0.043
0.050
5 pixs
0.068
0.092
0.069
0.085
10 pixs
0.140
0.162
0.143
0.167Figure 5
LOS vertical angle and rate estimation and error curves due to different BLOS angle accuracy.
(a)
LOS vertical angle estimation
(b)
LOS vertical rate estimation
(c)
LOS vertical angle estimation error
(d)
LOS vertical rate estimation errorFigure 6
LOS azimuth angle and rate estimation error curves due to different BLOS angle accuracy.
(a)
LOS azimuth angle estimation
(b)
LOS azimuth angle rate estimation
(c)
LOS azimuth angle estimation error
(d)
LOS azimuth rate estimation errorFigure 7
LOS angle and rate accuracy influenced by BLOS accuracy.With reference to Table1 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. Table2 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.Table 2
Parameters of gyro random error model.
Gyro
Random constant
First-order Markov process
White noise
(°/h)
Sampling time (s)
Standard deviation (°/h)
Time constant (s)
Standard deviation (°/h)
Type 1
10
0.005
10
1.43
10
Type 2
50
0.005
50
1.43
50
Type 3
100
0.005
100
1.43
100
Type 4
300
0.005
300
1.43
300Table 3
LOS angle and rate accuracy with different gyro accuracy.
Gyro
q
γ (°)
q
˙
γ (°/s)
q
λ (°)
q
˙
λ (°/s)
Type 1
0.020
0.003
0.046
0.012
Type 2
0.063
0.005
0.090
0.015
Type 3
0.143
0.011
0.175
0.025
Type 4
0.365
0.045
0.406
0.062Figure 8
LOS angle and rate estimation and error curves due to different accuracy of gyro.
(a)
LOS angle estimation curves
(b)
LOS rate estimation curves
(c)
LOS angle estimation error curves
(d)
LOS rate estimation error curvesWe can see from Figure8 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 Table3 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.Figure 9
LOS angle and rate accuracy influenced by gyro accuracy.


## 5.1. Simulation Principle

As is known from the LOS rate model in Section3, 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. a⃑i and a⃑bare the relative acceleration between missile and target in inertial frame and body frame, which are produced by the relative motion model. q˙γ and q˙λ can be directly obtained by the relative motion model in earth frame. The attitude dynamics and motion module outputs triaxial angular acceleration ω˙⃑mi, angular rate ω⃑mi, and attitude angles ϑ, ψ, and γ. According to (20), BLOS angles qα and qβ 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.Figure 4
Block diagram of LOS rate decoupling 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 isx=2000 m, y=1500 m, and z = 300 m; initial relative velocity is vx = −200 m/s, vy = −150 m/s, and vz = −20 m/s; initial relative acceleration is ax = −6 m/s2, ay = −4.5 m/s2, and az = −3.17 m/s2; initial attitude angle is ϑ=0°, ψ=0°, and γ=0°; initial rotation angular rates of missile are zeros; moment of inertia is Jx = 0.15 kg m2, Jy = Jz = 1.5 kg m2; external moment imposed on missile is Mx=0.01sin⁡(2πt)N·m, My=0.05sin⁡(2πt)N·m, and Mz=0.1sin⁡(2πt)N·m; simulation step is 20 ms; simulation time is 10 s. 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.Table 1
LOS angle and rate accuracy with different BLOS angle accuracy.
Accuracy
q
γ (°)
q
˙
γ (°/s)
q
λ (°)
q
˙
λ (°/s)
1 pix
0.014
0.055
0.014
0.033
3 pixs
0.042
0.071
0.043
0.050
5 pixs
0.068
0.092
0.069
0.085
10 pixs
0.140
0.162
0.143
0.167Figure 5
LOS vertical angle and rate estimation and error curves due to different BLOS angle accuracy.
(a)
LOS vertical angle estimation
(b)
LOS vertical rate estimation
(c)
LOS vertical angle estimation error
(d)
LOS vertical rate estimation errorFigure 6
LOS azimuth angle and rate estimation error curves due to different BLOS angle accuracy.
(a)
LOS azimuth angle estimation
(b)
LOS azimuth angle rate estimation
(c)
LOS azimuth angle estimation error
(d)
LOS azimuth rate estimation errorFigure 7
LOS angle and rate accuracy influenced by BLOS accuracy.With reference to Table1 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. Table2 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.Table 2
Parameters of gyro random error model.
Gyro
Random constant
First-order Markov process
White noise
(°/h)
Sampling time (s)
Standard deviation (°/h)
Time constant (s)
Standard deviation (°/h)
Type 1
10
0.005
10
1.43
10
Type 2
50
0.005
50
1.43
50
Type 3
100
0.005
100
1.43
100
Type 4
300
0.005
300
1.43
300Table 3
LOS angle and rate accuracy with different gyro accuracy.
Gyro
q
γ (°)
q
˙
γ (°/s)
q
λ (°)
q
˙
λ (°/s)
Type 1
0.020
0.003
0.046
0.012
Type 2
0.063
0.005
0.090
0.015
Type 3
0.143
0.011
0.175
0.025
Type 4
0.365
0.045
0.406
0.062Figure 8
LOS angle and rate estimation and error curves due to different accuracy of gyro.
(a)
LOS angle estimation curves
(b)
LOS rate estimation curves
(c)
LOS angle estimation error curves
(d)
LOS rate estimation error curvesWe can see from Figure8 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 Table3 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.Figure 9
LOS angle and rate accuracy influenced by gyro accuracy.


## 5.2.1. LOS Rate Accuracy Based on Different Seeker Accuracy

Simulation conditions are as follows: in the earth coordinate, initial relative position isx=2000 m, y=1500 m, and z = 300 m; initial relative velocity is vx = −200 m/s, vy = −150 m/s, and vz = −20 m/s; initial relative acceleration is ax = −6 m/s2, ay = −4.5 m/s2, and az = −3.17 m/s2; initial attitude angle is ϑ=0°, ψ=0°, and γ=0°; initial rotation angular rates of missile are zeros; moment of inertia is Jx = 0.15 kg m2, Jy = Jz = 1.5 kg m2; external moment imposed on missile is Mx=0.01sin⁡(2πt)N·m, My=0.05sin⁡(2πt)N·m, and Mz=0.1sin⁡(2πt)N·m; simulation step is 20 ms; simulation time is 10 s. 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.Table 1
LOS angle and rate accuracy with different BLOS angle accuracy.
Accuracy
q
γ (°)
q
˙
γ (°/s)
q
λ (°)
q
˙
λ (°/s)
1 pix
0.014
0.055
0.014
0.033
3 pixs
0.042
0.071
0.043
0.050
5 pixs
0.068
0.092
0.069
0.085
10 pixs
0.140
0.162
0.143
0.167Figure 5
LOS vertical angle and rate estimation and error curves due to different BLOS angle accuracy.
(a)
LOS vertical angle estimation
(b)
LOS vertical rate estimation
(c)
LOS vertical angle estimation error
(d)
LOS vertical rate estimation errorFigure 6
LOS azimuth angle and rate estimation error curves due to different BLOS angle accuracy.
(a)
LOS azimuth angle estimation
(b)
LOS azimuth angle rate estimation
(c)
LOS azimuth angle estimation error
(d)
LOS azimuth rate estimation errorFigure 7
LOS angle and rate accuracy influenced by BLOS accuracy.With reference to Table1 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. Table2 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.Table 2
Parameters of gyro random error model.
Gyro
Random constant
First-order Markov process
White noise
(°/h)
Sampling time (s)
Standard deviation (°/h)
Time constant (s)
Standard deviation (°/h)
Type 1
10
0.005
10
1.43
10
Type 2
50
0.005
50
1.43
50
Type 3
100
0.005
100
1.43
100
Type 4
300
0.005
300
1.43
300Table 3
LOS angle and rate accuracy with different gyro accuracy.
Gyro
q
γ (°)
q
˙
γ (°/s)
q
λ (°)
q
˙
λ (°/s)
Type 1
0.020
0.003
0.046
0.012
Type 2
0.063
0.005
0.090
0.015
Type 3
0.143
0.011
0.175
0.025
Type 4
0.365
0.045
0.406
0.062Figure 8
LOS angle and rate estimation and error curves due to different accuracy of gyro.
(a)
LOS angle estimation curves
(b)
LOS rate estimation curves
(c)
LOS angle estimation error curves
(d)
LOS rate estimation error curvesWe can see from Figure8 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 Table3 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.Figure 9
LOS angle and rate accuracy influenced by gyro accuracy.


## 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 Figure10, 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 axb,ayb,azb 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 ω⃑i,ϑ,ψ,γ,V⃑m,P⃑m,V⃑t,P⃑t 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 ω⃑~mi. The seeker tracks the target on the screen and outputs BLOS angle q~α,q~β. The processing computer reads the body angular rate ω⃑~mi from MEMS gyro STIM202; thus it can compute LOS rate q˙~γ,q˙~λ in real time with the information of [q~α,q~β,ω⃑~mi].Figure 10
Semiphysical simulation experiment platform.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 Table4. The parameters of strapdown seeker and gyro are shown in Table 5.Table 4
Turntable specifications.
Inner axis
Middle axis
Outer axis
Angular freedom
0.0000°~359.9999°
0.0000°~359.9999°
0.0000°~359.9999°
Position
Accuracy
0.0001°
0.0001°
0.0001°
Command resolution
0.0001°
0.0001°
0.0001°
Repeatability
±2′′
±2′′
±2′′
Rate
Range
±0.001°/s~±1000°/s
±0.001°/s~±300°/s
±0.001°/s~±200°/s
Stability over 360 deg.
1 × 10−4
1 × 10−4
1 × 10−4
Stability over 10 deg.
1 × 10−3
1 × 10−3
1 × 10−3
Command resolution
0.0005°/s
0.0005°/s
0.0005°/s
Dynamic
Bandwidth
≥12 Hz
≥10 Hz
≥8 Hz
Acceleration
≥3000°/s2
≥1500°/s2
≥800°/s2Table 5
Seeker and gyro parameters table.
Model item
Parameters
Strapdown seeker
Bias
Uniform distribution U(0, 0.028°)
Image processing uncertainty
Normal distribution N(0, 0.028°)
Quantized error
Uniform distribution U(0, 0.028°)
Frame frequency
50 Hz
Gyro STIM202
Bias
60°/h
Bias stability
60°/h
Scale factor accuracy
200 ppm


### 6.2. Result and Discussion

The semiphysical simulation experiment is carried out for verifying LOS estimated algorithm, and the results are shown in Figures11 and 12 and Table 6.Table 6
Table of experiment result.
LOS angle and rate
q
~
γ
q
~
λ
q
˙
~
γ
q
˙
~
λ
Max estimation error
0.345°
0.352°
0.478°/s
0.366°/s
Estimation accuracy (1σ)
0.100°
0.097°
0.110°/s
0.096°/sFigure 11
LOS angle and rate estimation curves.
(a)
LOS angle estimation curves
(b)
LOS rate estimation curvesFigure 12
LOS angle and rate estimation error curves.
(a)
LOS angle estimation error curve
(b)
LOS rate estimation error curveFrom Figure12, 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 q has an impact on the convergence rate: the smaller q 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 q is initialized by the relative distance of missile and target R, 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.


## 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 Figure10, 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 axb,ayb,azb 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 ω⃑i,ϑ,ψ,γ,V⃑m,P⃑m,V⃑t,P⃑t 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 ω⃑~mi. The seeker tracks the target on the screen and outputs BLOS angle q~α,q~β. The processing computer reads the body angular rate ω⃑~mi from MEMS gyro STIM202; thus it can compute LOS rate q˙~γ,q˙~λ in real time with the information of [q~α,q~β,ω⃑~mi].Figure 10
Semiphysical simulation experiment platform.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 Table4. The parameters of strapdown seeker and gyro are shown in Table 5.Table 4
Turntable specifications.
Inner axis
Middle axis
Outer axis
Angular freedom
0.0000°~359.9999°
0.0000°~359.9999°
0.0000°~359.9999°
Position
Accuracy
0.0001°
0.0001°
0.0001°
Command resolution
0.0001°
0.0001°
0.0001°
Repeatability
±2′′
±2′′
±2′′
Rate
Range
±0.001°/s~±1000°/s
±0.001°/s~±300°/s
±0.001°/s~±200°/s
Stability over 360 deg.
1 × 10−4
1 × 10−4
1 × 10−4
Stability over 10 deg.
1 × 10−3
1 × 10−3
1 × 10−3
Command resolution
0.0005°/s
0.0005°/s
0.0005°/s
Dynamic
Bandwidth
≥12 Hz
≥10 Hz
≥8 Hz
Acceleration
≥3000°/s2
≥1500°/s2
≥800°/s2Table 5
Seeker and gyro parameters table.
Model item
Parameters
Strapdown seeker
Bias
Uniform distribution U(0, 0.028°)
Image processing uncertainty
Normal distribution N(0, 0.028°)
Quantized error
Uniform distribution U(0, 0.028°)
Frame frequency
50 Hz
Gyro STIM202
Bias
60°/h
Bias stability
60°/h
Scale factor accuracy
200 ppm


## 6.2. Result and Discussion

The semiphysical simulation experiment is carried out for verifying LOS estimated algorithm, and the results are shown in Figures11 and 12 and Table 6.Table 6
Table of experiment result.
LOS angle and rate
q
~
γ
q
~
λ
q
˙
~
γ
q
˙
~
λ
Max estimation error
0.345°
0.352°
0.478°/s
0.366°/s
Estimation accuracy (1σ)
0.100°
0.097°
0.110°/s
0.096°/sFigure 11
LOS angle and rate estimation curves.
(a)
LOS angle estimation curves
(b)
LOS rate estimation curvesFigure 12
LOS angle and rate estimation error curves.
(a)
LOS angle estimation error curve
(b)
LOS rate estimation error curveFrom Figure12, 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 q has an impact on the convergence rate: the smaller q 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 q is initialized by the relative distance of missile and target R, 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.



---

*Source: 185149-2015-08-05.xml*