Journal of Physics: Conference Series

PAPER • OPEN ACCESS

Research on infrared passive ranging algorithm based on unscented

Kalman filter and modified spherical coordinates
To cite this article: Y H Chi et al 2020 J. Phys.: Conf. Ser. 1629 012066

View the article online for updates and enhancements.

This content was downloaded from IP address 45.40.125.199 on 25/10/2020 at 09:47

ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

Research on infrared passive ranging algorithm based on

unscented Kalman filter and modified spherical coordinates

Y H Chi1, L H Hu2, X Gao1, and S F Li3

1
School of Information Engineering, Xijing University, Xi’an, Shaanxi, China
2
Xi’an Research Institute of High-Tech, Xi’an, Shaanxi, China
3
91049 Unit of PLA, Qingdao, Shandong, China

Corresponding author and e-mail: L H Hu, [email protected]

Abstract. Target range detection is one of the key technologies for intelligent unmanned
vehicle to work reliably and efficiently. Infrared passive ranging can estimate the relative
distance information of the target in real time by measuring the angle information with noise. It
has many advantages such as low cost, good real-time performance and so on, which has a
good application prospect in the range detection. In this paper, the mathematical model of
infrared passive ranging of intelligent unmanned vehicle based on modified spherical
coordinates (MSC) is established, and the algorithm flow based on unscented Kalman filter
(UKF) is given. Finally, the simulation experiment and analysis are carried out. The results
show that: in the problem of target range detection of intelligent unmanned vehicle, the method
adopted has high ranging accuracy and good stability.

1. Introduction
With the rapid development of artificial intelligence, all kinds of intelligent unmanned vehicles, such
as driverless cars, intelligent robots, unmanned aerial vehicles, are used more and more widely in
human society [1]. The range detection of the target object is one of the key technologies to ensure
that the intelligent unmanned vehicle has a safe track and works reliably and efficiently. Range
detection technology can be divided into active ranging and passive ranging. The former is to detect
the target by transmitting high-power signals, and determine the target range by analyzing the
information. The detection accuracy is high, but the detection range is limited and the cost is high [2].
The latter is to determine the target range by detecting the radiation information, angle information or
other information of the target itself and analyzing it. This technology does not need to transmit
detection signals, and can omit high-cost transmitting units. It has a high cost performance ratio, and
compared with active ranging technology, passive ranging technology has a farther detection
capability [3].
As an important passive ranging technology, infrared detection estimates the target range by
measuring the angle information with noise. Because of its low cost and good real-time performance,
it has a good application prospect in the target range detection of intelligent unmanned vehicle [4]. But
for infrared passive ranging, the relative motion equation of the target is nonlinear. In dealing with this
kind of nonlinear filtering problem, extended Kalman filter (EKF) is a classical and widely used
method. In EKF, the nonlinear function is approximated by linearization, while higher-order terms are
Content from this work may be used under the terms of the Creative Commons Attribution 3.0 licence. Any further distribution
of this work must maintain attribution to the author(s) and the title of the work, journal citation and DOI.
Published under licence by IOP Publishing Ltd 1
ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

ignored or approximated to solve the nonlinear problem. Although the application of EKF to the state
estimation of nonlinear systems has been recognized by the academic and engineering circles, it has
the following shortcomings [5]:
1) When the higher-order terms of Taylor expansion of nonlinear function cannot be ignored,
linearization will cause large errors and even divergence of the system;
2) In many practical problems, it is difficult to obtain the derivation of Jacobian matrix for
nonlinear function;
3) EKF needs to take derivatives, so it is necessary to clearly understand the specific form of
nonlinear functions, which cannot be packaged in black boxes, so it is difficult for modular application.
In order to improve the filtering effect of nonlinear problems, Julier et al. proposed UKF method
based on unscented transformation (U-transform) [6]. In order to reduce the estimation error, the U-
transform is used to process the state equation, and then the U-transform state quantity is used to
estimate the filter. Its computational complexity is of the same order as EKF, but it is easier to realize,
with higher accuracy and faster convergence [7].
In this paper, UKF is introduced to study the problem of infrared passive ranging, and MSC is used
to overcome the problem of poor filtering stability in rectangular coordinate system. The following
arrangement of the paper is as follows: firstly, the mathematical model of infrared passive ranging of
intelligent unmanned vehicle is established based on MSC, then the basic algorithm flow of UKF is
introduced briefly, and finally the simulation experiment of infrared passive ranging based on
UKF/MSC is carried out.

2. Mathematical model of infrared passive ranging

The research shows that the filtering stability in rectangular coordinate system is poor, sometimes
even divergent [8]. In order to overcome this problem, MSC is used in this paper. As shown in Figure
1, the infrared detector is located at the coordinate origin O , the target is located at ( x, y, z ) . The
distance between the target and the infrared detector is r , and the projection of r on the X  Y plane
is rh , the angle between rh and X axis is the target azimuth  , and the angle between rh and r is the
target pitch  . The target acceleration components aR , aH and aV are along the three axis
T T T

directions of the antenna coordinate system respectively, specifically: aR is along the r direction,
T

aH T is in the horizontal plane and parallel to  . The acceleration components of the infrared detector
on the three axes of the antenna coordinate system are aR , aH and aV , respectively.
O O O

aVT aRT
Z
aHT
Target (x,y,z)

Detector O  Y


rh
X

Figure 1. Modified spherical coordinates (MSC).

Define the state vector as:
X  [ x1 x2 x3 x4 x5 x6 ]T  [   
r 1T
] (1)
r r
where,     cos  .

2
ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

According to Figure 1, it can get:

x  r cos  cos 
y  r cos  sin  (2)
z  r sin 
The second derivative of equation (2) can be obtained:
x  r cos  cos   2r sin  cos   2r cos  sin 
 r sin  cos   r 2 cos  cos   2r sin  sin 
 r cos  sin   r 2 cos  cos 
y  r cos  sin   2r sin  sin   2r cos  cos 
 r sin  sin   r 2 cos  sin   2r sin  cos  (3)

 r cos  cos   r 2 cos  sin 

z  r sin   2r cos   r cos   r 2 sin 
The coordinate rotation matrix from rectangular coordinate system to antenna coordinate system
can be expressed as:
 aR  aR   cos  cos  cos  sin  sin    x 
 T O
 (4)
 aHT  aH O     sin  cos  0   y 
 
 aVT  aVO    sin  cos   sin  sin  cos    z 

Substituting the expression of x, y, z in equation (3) into equation (4), we can get:
aRT  aRO  r  r 2  r 2
aHT  aH O  2r  r tan   r (5)
aVT  aVO  2r  r  r 2 tan 
therefore:
r aR  aRO
 2  2  T
r r
r aH T  aH O
   tan   2   (6)
r r
r aVT  aVO
  2    2 tan  
r r
According to equation (1) and equation (6), the equation of motion state is as follows:
 x2 
 x1   
 x   2 x2 x5  x4 tan x1  x6 (aV  aV ) 
2
 2  T O
 (7)
x   x4 sec x1 
X   3   
x
  
4  2 x4 x5  x 2 x4 tan x1  x6 ( a H T
 a H )
O 
 x5   
   x22  x42  x52  x6 (aRT  aRO )

 x6   
  x5 x6
In practical application, because the acceleration of the target is unknown, aR , aH T , aV are
T T
generally treated as process noise.

3. The Unscented Kalman Filter

For discrete-time nonlinear system:
xk 1  F ( xk , uk , wk )
zk  H ( xk , vk )

3
ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

where, k  N is the time indicator, xk  Rn is the state quantity at time k , wk is the n -dimension
process noise, zk  Rm is the measurement value of system state at time k , and vk is the m -dimension
measurement noise.
Initially, process noise and measurement noise are independent of each other and are satisfied:
E ( x0 )  x0 , cov( x0 )  P0
cov(wk )  Qk , cov(vk )  Rk
Then the general steps of UKF algorithm can be described as follows:
(1) Expand the dimension of the state vector, and let
x a  [ xT vT  T ]T
(2) Initial conditions
x0a  E[ xa ]  [ x0T 0 0]T
 P0 0 0
P0a  E[( x0a  x0a )( x0a  x0a )T ]   0 Q0 0 
 0 0 R0 
(3) For k  {1, , N }
xka1  [ xka1 xka1  ( ( L   ) Pka1 )i xka1  ( ( L   ) Pka1 )i ]
where, represents the i column of the square root of matrix.
( ( L   ) Pka1 )i

(4) Time update

 kx|k 1  F [ kx1  kv1 ]
2L
xk   i( m )ix,k |k 1
i 0
2L
Pk   i( c ) [ix,k |k 1  xk ][ix,k |k 1  xk ]T
i 0

 k |k 1  H [ kx|k 1  kn|k 1 ]
2L
zk   i( m ) i ,k |k 1
i 0

(5) Measurement update

2L
Pzk =  i(c) [ i ,k |k 1 -zk－ ][ i ,k |k 1 -zk－ ]T
i=0
2L
Pxk zk   i( c ) [i ,k |k 1  xk ][ i ,k |k 1  zk ]T
i 0

Kk  Pxk zk Pzk 1
xk  xk  Kk ( zk  zk )
Pk  Pk  K k Pzk K kT
where,    2 (n   )  n is the scaling factor, i( m) is the weight used for mean weighting, and i(c ) is the
weight used for covariance weighting.

4. Results and discussion

Generally speaking, for the target with low mobility, the 3-dimensional motion can be divided into
two 2-dimensional motions, namely r   2-dimensional motion and r   2-dimensional motion, so as
to reduce the computation. Because of the similarity of two 2-dimensional motions, it is enough to
select a 2-dimensional motion for simulation experiment. Next, take r   2-dimensional motion as an
example for simulation experiment.
The state quantity of the target is:
r 1T
X  [ x1, x2 , x3 , x4 ]T  [ ,  , , ]
r r
Then, the equation of nonlinear continuous motion can be obtained from equation (7):

4
ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

 x1   x2   0 
 x   2 x x  a  aH O 
X   2   2
2 3
 x  HT
 x3   x2  x32  4 a  aRO 
     RT 
 4    x3 x4 
x  0 
If the infrared detector moves at a constant speed, i.e. aH O  aRO  0 , the discrete motion equation is:
 x2 (k )   0 0 
 2 x (k ) x (k )   0  a 
X (k  1)  X (k )  T  2  T 
2 3 x4 (k )  RT
 x (k )  x (k )   
2  x4 (k ) 0   aHT 
 2 3   
  x3 (k ) x4 (k )   0 0 
Where, w(k )  [ aR , aH ]T is the process noise.
T T

The system measurement equation is:

Z (k )  HX (k )  v(k )
Where, v(k ) is the measurement noise, and v(k ) is a Gaussian white noise sequence independent of
w(k ) . H  [1, 0, 0, 0] is the measurement matrix.
Theconditions of simulation as follows: targetinitial position is [ x0 , y0 ]  [9000m,0m] , its speed is

v  500m / s ,and speed direction is . Then the target's trajectory is shown in Figure 2:
j
e 6
4
x 10
6

4
y /m

0
0 1 2 3 4 5 6 7 8 9 10
x /m 4
x 10

Figure 2. The target's trajectory.

The simulation parameters are set as follows: the standard deviation of process noise are
 R   H  0.5m / s 2 , the standard deviation of measurement noise is   0.001rad , the simulation cycle
T T

is , the simulation time is 200 s , the covariance matrix for the initial state is
T  1s
p0  diag (1 ,0.012 ,0.012 ,0.00012 ) 。 The angle estimation error curve, distance estimation error curve,
2

distance estimation relative error curve and distance tracking curve are shown in Figure 3~6.
x 10
-3
error of  error of r
1.5 12000

1 10000

0.5 8000

0 6000
 /rad

r /m

-0.5 4000

-1 2000

-1.5 0

-2 -2000
0 20 40 60 80 100 120 140 160 180 200 0 20 40 60 80 100 120 140 160 180 200
t /s t /s

Figure 3. The angle estimation error curve. Figure 4. The distance estimation error curve.

5
ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

4
x 10
14 10
real r
filter r
12
9

10

8
8

r /m
%

6 7

4
6

5
0

-2 4
0 20 40 60 80 100 120 140 160 180 200 0 20 40 60 80 100 120 140 160 180 200
t /s t /s

Figure 5. The distance estimation relative error curve. Figure 6. The distance tracking curve.
According to the simulation results, the relative distance error after filtering convergence is better
than 1% , and the ranging accuracy is high. Moreover, in many simulation experiments, the filter has
good convergence and stability.

5. Conclusions
In this paper, the infrared passive ranging algorithm based on UKF/MSC is discussed in detail and
verified by simulation. Simulation results show that the algorithm has high ranging accuracy and good
stability for low maneuvering moving targets. However, because the motion equation of the target in
MSC is generally a complex nonlinear equation, the passive ranging effect of the high maneuvering
target is not very ideal. In addition, for the passive detection using only angle information, there are
inevitable nonlinear problems, and the amount of information available is small, so the convergence
speed of the filter is slow. These problems need further study.

Acknowledgement
This research was funded by the national natural science foundation of China (grant number:
61401470), and by the foundation of Shaanxi Key Laboratory of Integrated and Intelligent Navigation
(grant number: SKLIIN-20180206).

References
[1] Yu H Y, Li G R, Zhang W G, et al. The Unmanned Aerial Vehicle Benchmark: Object
Detection, Tracking and Baseline[J]. Int. J. of Computer Vision, 2020, 128,5:1141-
1159.
[2] Shin J, Yi S Y. Active Ranging Sensors Based on Structured Light Image for Mobile
Robot[M]// Multimedia and Ubiquitous Engineering. Springer Netherlands, 2013.
[3] Fu X N, Chen L Q, Dong Q. Passive Ranging Algorithm Based on Improved Elman
Neural Network[J]. Infrared Technology, 2019,41(6): 540-544. (In Chinese)
[4] Fu X N, Wu D H. A 3D passive infrared ranging algorithm based on bearing-only with
monocular[J]. Acta Armamentarii, 2008,10: 1188-119. (In Chinese)
[5] Su T L, Tang Z Y, Peng L Y. Model updating for real time dynamic substructures based
on UKF algorithm[J]. Earthquake Engineering and Engineering Vibration, 2020,
19(2): 413-421.
[6] Simon J, Jeffrey K. Uhlmann. A general method for approximating nonlinear
transformations of probability distributions. Technical Report, Robotics Research
Group, Department of Engineering Science, University of Oxford, 1996.
[7] Shen B,Zhu B,Long B. Research of passive measuring distance technique based on
continuously-measuring angle. Electronic Design Engineering, 2010, 18(12): 45-

6
ICAMLDS 2020 IOP Publishing
Journal of Physics: Conference Series 1629 (2020) 012066 doi:10.1088/1742-6596/1629/1/012066

48,53. (In Chinese)

[8] Aidala, V. Kalman Filter Behavior in Bearings-Only Tracking Applications[J]. IEEE
Transactions on Aerospace and Electronic Systems, 1979, 15(1): 29-39.