# PDF Archive

Easily share your PDF documents with your contacts, on the Web and Social Networks.

## IJEAS0405016 .pdf

Original filename: IJEAS0405016.pdf
Title:
Author:

This PDF 1.5 document has been generated by Microsoft® Word 2010, and has been sent on pdf-archive.com on 10/09/2017 at 17:24, from IP address 103.84.x.x. The current document download page has been viewed 252 times.
File size: 550 KB (5 pages).
Privacy: public file ### Document preview

International Journal of Engineering and Applied Sciences (IJEAS)
ISSN: 2394-3661, Volume-4, Issue-5, May 2017

Research of an Adaptive Cubature Kalman Filter for
Chen Zhao, Shuai Chen, Yiping Wang

Abstract— In view of the characteristics of Ballistic missile,
the GPS/SINS tightly integrated navigation algorithm based on
pseudo range/pseudo range rate in the Launch inertial
coordinates is studied in this paper. The coordinate
transformation method from ECEF coordinates to the Launch
inertial coordinates is derived. The strap-down inertial
navigation error equation and GPS error equation were derived
in this coordinates. The state equations and measurement
equations for GPS/SINS tightly integrated navigation system
are established. Then an adaptive cubature kalman filter
(ACKF) is employed to improve the position performance. The
numerical simulation shows the better accuracy performance of
the integrated navigation system.e and control the quality of the
vector tracking loop.

II. ERROR MODEL IN THE LAUNCH INERTIAL COORDINATES
A. State error model of the SINS
The error angle between SINS mathematical simulation
platform and the navigation coordinate is attitude
misalignment angle. It is

  [ x  y  z ]T

(1)

The symmetric matrix of the attitude misalignment angle is

 0

()    z
  y

Index Terms— Adaptive cubature kalman filter,High
dynamics,Launch inertial coordinates,Tightly integrated

 z
0

y 

 x 

(2)

0 

x

The update equations for the attitude transformation matrix
is
b
Cbn  Cbn (nb
)

Where 

I. INTRODUCTION
Global positioning system (GPS) and inertial navigation
system (INS) has an important role in the field of navigation.
GPS can provide the global position, velocity and time
information all-time and all-weather. And its precision does
not change with time. INS is a completely autonomous
navigation system. It has less susceptible to external
electromagnetic interference, high data rate, posture
information, etc. However, in practical applications, the
respective shortcoming of GPS and INS also limits their
application. Because of GPS and INS have complementary,
combination of the two can achieve performance
transcendence. The GPS/SINS tightly integrated navigation
algorithm generally based on geographic coordinates [1-5]. In
view of the characteristics of Ballistic missile, the GPS/SINS
tightly integrated navigation algorithm based on pseudo
range/ pseudo range rate in the Launch inertial coordinates is
studied in this paper. An adaptive cubature kalman filter is
employed as the integration filter[6-8]. And the numerical
simulation indicates that this GPS/SINS tightly integrated
effectively.

b
nb

(3)

is inertial navigation system sensitive to the

angular velocity .
Because of the inertial device measurement and calculation
process exist error. The following relationship exists in the
inertial navigation system between the actual output attitude
transformation

matrix

transformation matrix

Cbn and

the

ideal

attitude

Cbn .

Cbn  Cbn   Cbn  ( I   )Cbn

(4)

 Cbn  ()Cbn

(5)

Therefore,
Derivative (5) to obtain:

C bn  ()Cbn  ()C bn
b
 ()Cbn  ()Cbn (nb
)

(6)

Differentiating both sides of (3) results in:
b
b
C bn  Cbn (nb
)  Cbn (nb
)
b
b
 ()Cbn (nb
)  (nb
)

(7)

Comparing (6) and (7) can be obtained that:
b
()  Cbn (nb
)Cnb

Combined with vector operation
in:

Chen Zhao, College of Automation , National University of Science and
Technology,Nanjing ,China.
Shuai Chen, College of Automation , National University of Science and
Technology,Nanjing ,e-mail:China.
Yiping Wang, College of Automation , National University of Science
and Technology,Nanjing ,China
This work was supported by the Fundamental Research Funds for the
Central Universities (No. 30916011336), Jiangsu Planned Projects for
Postdoctoral Research Funds (No. 1501050B), and the China Postdoctoral
Science Foundation (No. 2015M580434), the Research Innovation Program
for College Graduates of Jiangsu Province (No. KYLX16_0463). This work
also got the special grade of the financial support from the China
Postdoctoral Science Foundation (No. 2016T90461).

Where

  [ x
b
nb

(8)

(ab)  a(b)aT results

  Cbnnbb
 y  z ]T are gyro drift error.

(9)

In the launch inertial coordinate, speed differential equation
is:

V  Cbn f b  ge

(10)

Differentiating both sides of (10) results in:

20

www.ijeas.org

Research of an Adaptive Cubature Kalman Filter for GPS/SINS Tightly Integrated Navigation System

 V   Cbn  f b  Cbn f b   g e

  lu   lru  u

 lru

 lru   T  ru
ru

(11)

The position error equation

 x   Vx 
 y    V 
   y
 z   Vz 

(12)

clock error,

X I  FI X I  GI wI
In the equation above
,

X I (t )  [ x  y  z Vx Vy

x y z  x  y  z  x  y  z ]
033
033
I

0615

 Cbn

GI   033
093

X G (t )  [ lu

0
FG (t )  
0


(15)

 X I (t )   FI (t )
0   X I (t ) 


FG (t )  X G (t )
 X G (t )  0
0  WI (t ) 
G (t )
 I
GG (t ) WG (t )
 0

(18)

C. Measurement equation
The pseudo-range from GPS receiver to the GPS satellite
numbered j can be expressed as follows:

T

wI  gx gy gz ax ay az 
In the equation above, w is system noise variance,  gx 、

Gj   j   lu   j

 gy 、 gz are gyro drift noise variances of three axis; ax
、 ay 、 az are accelerometers noise variances of three

In the equation above,

j

(19)

is the ideal pseudo range.

 lu

is the equivalent distance corresponding to clock error.  lu is

axis; I is identity matrix;

main error in pseudo range measurement. Therefore, when
building the pseudo range measurement model, the impact of
the error is the primary consideration;   j is pseudo range

measurement noise, caused by multipath effect, tropospheric
delay, ionospheric error. And it can be regarded as white
noise。
Satellite position get from the ephemeris is in ECEF
coordinate system, assuming that the position of the Satellite
numbered j in the ECEF coordinate system is

x

j
e

yej

zej  . To compare with the INS information, we

need to convert it to the launch inertial coordinate system:
(1) Convert Satellite position in ECEF ( e coordinate)
coordinate to launch coordinate ( l coordinate), the
conversion formula is as follows:

B. GPS error equations
When Usually we takes two time-related errors to represent
the GPS error condition: the distance error , which is
equivalent to the clock error lu ; the distance error ,which is
equivalent to clock frequency error

 lru ]T ， WG (t )  [u ru ]T
1 
1 0 
1  ， GG (t )  

0 1 
Tru 

Therefore, tightly integrated navigation system error
equation can be get from integrating the SINS and GPS error
equations:

033 

Cbn 
093 
156

f Z  fY 
 0

F1    f Z
0
f X 
 fY  f X
0 
 g x g x g x 
 x
y
z 

 g y g y g y 
F2  

y
z 
 x
 g z g z g z 

y
z 
 x

(17)

In the equation above,

033 

Cbn 
,

1515

Cbn
033
039

033
F2

is GPS clock frequency white noise.

X G (t )  FG (t ) X G (t )  GG (t )WG (t )

(14)

Vz

ru

GPS error equation can be expressed as follows:

(13)
  0,   0
The system state equation of SINS in the launch inertial
coordinate system can be obtained from 1) to 4) results in

033

F
FI   1
033



Tru is the relevant time, u is white noise of GPS

Where

Inertial device error equation

(16)

 Xl 
 X e   r0 x 
 Y   C l   Y   r 
(20)
e  e 
 l
 0y 
 Zl 
 Z e   r0 z 
In the equation above, r0 is the geocentric vector emission

lru , error model

expression as follows:

points,

21

Cel is the transpose matrix of Cle , Cle is as follows:

www.ijeas.org

International Journal of Engineering and Applied Sciences (IJEAS)
ISSN: 2394-3661, Volume-4, Issue-5, May 2017

  sin A0 sin 0  cos A0 sin B0 cos 0
C  sin A0 cos 0  cos A0 sin B0 sin 0
cos A0 cos B0
 cos A0 sin 0  sin A0 sin B0 cos 0

cos A0 cos 0  sin A0 sin B0 sin 0 

 sin A0 cos B0
e
l

cos B0 cos 0

Z  (t )  H  (t ) X (t )  V (t )

cos B0 sin 0
sin B0

Suppose that the number of effective GPS satellites is m ,
so:

(21)

Z  (t )   1  2

016

0
H  (t )   16

016

(2) Convert the position of the Satellite in Launch
coordinate system (l coordinate) to launch inertial coordinate
(g coordinate), the conversion formula is as follows:

 X g  r0 x 
 X l  r0 x 

g 
(22)
 Yg  r0 y   Cl   Yl  r0 y 
 Z g  r0 z 
 Zl  r0 z 

l
g
In the equation above, Cl is the transpose matrix of C g ,

0

exm

eym

ezm

016

ygj

  m 

T

(28)

j

veyj

vezj  , convert it to launch

j

According to carrier speed obtained by INS in launch

inertial coordinate vgx

vgy

vgz  the pseudo-range rate

can be get as follows:

 gj  e jx (vgx  vgxj )  e jy (vgy  vgyj )  e jz (vgz  vgzj ) (29)
Assumed that the real value of the carrier speed is

v
v

x

vy

x

vy

vz  ,

expand

the

equation

at

vz  in taylor formula and rounding second and

 gj   j  e jx vx  e jy v y  e jz vz

(30)

So the pseudo range rate measurement equation is

j 2
g

z  , expand the equation above at  x

 j  e jx vx  e jy v y  e jz vz   lru   j

j 2
g

y

Z  (t )  H  (t ) X (t )  V (t )

(24)

 j
 j
 j
  
x
y
z
x
y
z

Z  (t )   1  2

(25)

013

0
H  (t )   13

013

In the equation above,
j
 j yg  yg

 e jy
y
 gj

,

j
 j z g  z g

 e jz .
z
 gj

 m 

e1x
ex2

e1y
ey2

e1z
ez2

019
019

exm

eym

ezm

019

V (t )    1   2

T

  m 

0 1

0 1

0 1
T

The measurement equation of tightly integrated navigation
system can be get as follows:

The pseudo range measurement equations of tightly
integrated navigation system can be expressed as follows:

 j  e jx x  e jy y  e jz z   lu   j

(32)

Similarly, suppose that the number of effective GPS
satellites is m , so:

z  in

j

(31)

Therefore,

taylor formula and discard second and higher order terms:

,

above

higher order terms:

Assumed that the real value of the carrier position is

j
 j xg  xg

 e jx
x
 gj

vezj  .

veyj

coordinate can be obtained as follows : vex

z gj  .

  ( xg  x )  ( y g  y )  ( z g  z )

j
g

1 0 

1 0 

1 0 

inertial coordinate, the velocity of Satellite in launch inertial

is the longitude of the emission point,

j 2
g

y

016
016

So the pseudo-range from the carrier to the GPS satellite
numbered j can be get from INS:

x

e1z
ez2

ECEF coordinates is vex

rotation.
Finally, the position of the Satellite in launch inertial

j
g

e1y
ey2

Suppose that the velocity of Satellite numbered j in

A0 is the azimuth , e is the angular velocity of Earth
j

e1x
ex2

Gj   j   lru   j

by  sin B0 , bz   cos B0 sin A0 ; B0 is the latitude of

T

The pseudo range rate from GPS receiver to the GPS
satellite numbered j can be expressed as follows:

(1  bx2 )(1  cos et )
bxby (1  cos et )  bz sin et

l
Cg  by bx (1  cos et )  bz sin et 1  (1  by2 )(1  cos et )

bz bx (1  cos et )  by sin et bz by (1  cos et )  bx sin et (23)
bxbz (1  cos et )  by sin et 

by bz (1  cos et )  bx sin et 

1  (1  bz2 )(1  cos et )

In the equation above, bx  cos B0 cos A0 ,

coordinate is xg

 m 

V (t )    1   2

C gl is as follows:

the emission point,

(27)

(26)

Therefore,

22

www.ijeas.org

Research of an Adaptive Cubature Kalman Filter for GPS/SINS Tightly Integrated Navigation System

 H  (t )
V (t )
Z (t )  
 X (t )  

 H  (t )
V (t )

2n

zˆk |k 1   i Z i ,k |k 1
(5) Evaluate the innovation covariance

 H (t ) X (t )  V (t )

1
Pzz 
ak

Considering a discrete nonlinear system as

x k 1  f ( xk , k )  wk

Pxz 

zk  h( xk , k )  vk
is

1
ak

xk n , the process noise vector

i 1

i , k |k 1

ZiT,k |k 1  zˆk |k 1 zˆkT|k 1  Rk

(43)

2n

 X
i

i 1

i , k |k 1

ZiT,k |k 1  xˆk |k 1 zˆkT|k 1

K k  Pxz Pzz1
 xˆk |k 1  Kk ( zk  zˆk |k 1 )

(44)
(45)
(46)

1
Pk |k 1  K k Pzz K kT
ak

(47)

Where:

Q , i  k
E[ wk wiT ]   k
0, i  k
R , i  k
E[vk viT ]   k
; E[ wk viT ]
0, i  k

ak 

tr ( P)
tr (e(k )e(k )T )

(48)

2n

P   i Z i ,k |k 1ZiT,k |k 1  zˆk |k 1 zˆkT|k 1

(49)

i 1

Qk is the process noise covariance matrix, and

IV. THE SIMULATION AND RESULTS

Rk is the measurement noise covariance matrix.

In order to verify the correctness of the algorithm, ballistic
trajectory generator platform and numerical simulation are
designed in this paper. The missile is fired vertically, the total
flight time is 200s, the time of powered phase is 60s, the thrust

The details of the implementation of ACKF are as follows:
Step One: Initialize state vector xˆ0|0 and state covariance
matrix

i

Pk |k 

zk m . The vectors wk and vk

are zero mean Gaussian white sequences with zero
cross-correlation with each other:

Where

 Z

xk |k

wk n , the measurement noise vector is vk m , the

measurement vector is

2n

(6) Estimate the cross-covariance

III. THE ADAPTIVE CUBATURE KALMAN FILTER

Where the state vector

(42)

i 1

(33)

P0|0

2

acceleration in powered phase is 40 m / s ; the constant drift
of gyro is 0.5°/h, the white noise of gyro is 0.05/h; the
constant drift of accelerometer is 1mg, the white noise of
accelerometer is 0.5mg; The start point: latitude: 31.98°N,
longitude:118.8°E, height: 0m;
Initial speed: forward speed: 394.8917m/s, upward speed
0m/s, lateral speed 0m/s. White noise of pseudo-range is 10m,
the white noise of pseudo-range rate is 0.1m/s.
Initial attitude: pitch angle: 90°, roll angle: 0°, yaw angle:
0°, The initial attitude error is 0.1°. The numerical simulation
results are as follows:

Step Two: Time update
Factorize the covariance:

Pk 1|k 1  S k 1|k 1S kT1|k 1

(34)

Evaluate the cubature points through the process model

X i ,k 1|k 1  Sk 1|k 1i  Xˆ k 1|k 1

(35)

Estimate the propagated cubature points through the
process model

X i*,k ,k 1  f ( X i ,k 1|k 1 )

(36)

Estimate the predicted mean
2n

xˆk |k 1   i X i*,k |k 1

(37)

i 1

Estimate the predicted error covariance
2n

Pk |k 1   i X i*,k |k 1 X i*,k |k 1T  xˆk |k 1 xˆkT|k 1  Qk 1
i 1

(38)
Step Three: Measurement update
Factorize the covariance

Pk |k 1  S k |k 1S kT|k 1

(39)

Evaluate the cubature points

X i ,k |k 1  Sk |k 1i  xˆk |k 1

Evaluate the propagated
observation model

cubature

Zi ,k|k 1  h( X i ,k |k 1 )

(40)
points

through
(41)

Figure 1. The position error

Evaluate the propagated observation

23

www.ijeas.org

International Journal of Engineering and Applied Sciences (IJEAS)
ISSN: 2394-3661, Volume-4, Issue-5, May 2017










Figure 2. The velocity error

Yuan Jungang. The Study of Tightly GPS/INS Integrated Navigation
System [D]. Nanjing: Nanjing University of Aeronautics and
Astronautics, 2011.
Lei Haoran. Study on Missile-Borne SINS/GNS Integrated Navigation
System [D]. Nanjing: Nanjing University of Science &amp; Technology,
2014.
Fang Jiancheng, Ning Xiaolin, Tian Yulong. Spacecraft Autonomous
Celestial Navigation Principles and Methods [M]. Beijing: National
Defense Industry Press, 2006.
Zhang Libin. Study on navigation, midcourse correction and attitude
control of launch vehicle upper stage [D]. Harbin: Harbin Institute of
Technology.
Jiang Jinlon. Research on SINS/SAR/GPS Integrated Navigation
System [D]. Harbin: Harbin Institute of Technology.

Chen Zhao ,a master study in College of
Automation , National University of Science and
Technology,Nanjing
,China. Research on the

Shuai Chen, a associate professor of College of
Automation , National University of Science and
Technology,Nanjing ,China. Research on the integrated

Yiping Wang ,a master study in College of
Automation , National University of Science and
Technology,Nanjing ,China. Research on the integrated

Figure 3. The attitude error
According to figure 1 to figure 3, when received eight stars,
position error can converge to within 2m, speed error can
converge to 0.1m/s or less, attitude error can converge to
within 0.05 °.
V. CONCLUSIONS
According to the characteristics of ballistic missiles, a
kind of GPS/SINS tightly integrated navigation algorithm in
Launch inertial coordinate is designed. And the state
equations and measurement equations for GPS/SINS tightly
cubature kalman filter was applied to the system. The
numerical simulation indicates that this GPS/SINS tightly
accuracy effectively.
ACKNOWLEDGMENT
Chen Zhao thanks the team he belongs to,it is the team that
REFERENCES





Wu Ruixiang. Research On Key Technology In GPS/MIMU Tightly
Integrated System [D]. Nanjing: Southeast University, 2010.
Ji Feng. Research on INS/GPS Tightly Integrated Navigation System
Simulation and Key Technology [D]. Nanjing: Nanjing University of
Aeronautics and Astronautics, 2009.
Ye Ping. Research on mems IMU/GNSS ultratight integration
navigation Technology [D]. Shanghai:Shanghai Jiao Tong University,
2011.

24

www.ijeas.org