Ijeas0405016 by International Journal of Engineering & Technical Research

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 GPS/SINS Tightly Integrated Navigation System 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 navigation.

 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 navigation algorithm can improve navigation accuracy 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:

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:

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,

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 

(28)

veyj

vezj  , convert it to launch



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

vz  ,

expand

the

equation

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

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

  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

(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

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



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,

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 )

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)

 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)

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

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

(6) Estimate the cross-covariance

III. THE ADAPTIVE CUBATURE KALMAN FILTER

Where the state vector

(42)

i 1

(33)

P0|0

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

(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

www.ijeas.org

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

[5]

[6]

[7]

[8]

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 & 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 integrated navigation,the inertial navigation.

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

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

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 integrated navigation system are established. An adaptive cubature kalman filter was applied to the system. The numerical simulation indicates that this GPS/SINS tightly integrated navigation algorithm can improve navigation accuracy effectively. ACKNOWLEDGMENT Chen Zhao thanks the team he belongs to,it is the team that provides a good academic atmosphere. REFERENCES [1] [2]

[3]

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.