A Gyro-Free INS Algorithm in the Navigation Frame and Its Application to the Spinning Vehicle with High Rotation Rate

Abstract

Conventional inertial measurement units cannot be used in the spinning vehicle with high rotation rate due to gyro's narrow operation range. By the way, angular acceleration can be measured using the accelerometer array distributed in the vehicle. This paper derives a mechanization for the gyro-free INS in the navigation frame, and proposes a gyro-free INS algorithm based on the derived mechanization. In addition, the proposed algorithm is used to estimate angular velocity, attitude, velocity, and position of a spinning vehicle with high rotation rate. A MATLAB-based software platform is configured in order to show validation of the proposed algorithm. The reference trajectory of a spinning vehicle at 3 round per second, 30 round per second are set up, and the outputs of accelerometer are generated when triads of accelerometer are located at the origin and all the axes. Navigation results of the proposed algorithm for the generated output are presented. The results show that the proposed navigation algorithm can be applied to the spinning vehicle with high rotation rate.

1. INTRODUCTION

Conventional inertial navigation systems (INSs) measure acceleration and angular velocity using an inertial measurement unit (IMU) to estimate vehicles’ position, velocity, and attitude. Gyroscopes in the IMU cannot measure angular velocities of the spinning vehicle with high rotation rate due to the narrow operation range (Mickelson 2000, Pachter et al. 2013). Therefore, it is very difficult to estimate an attitude of the vehicle using conventional IMUs when a vehicle is rotated at high rotation rate (Mickelson 2000). Since an accelerometer has a wide operation range, it can measure an angular acceleration generated by the rotational motion of the vehicle through an accelerometers array, and angular velocity can be obtained by integrating angular accelerations (Schuler et al. 1967, Padgaonkar et al. 1975). That is, an angular velocity of a high-speed spinning vehicle can be estimated from the output of a gyro-free IMU (GF-IMU) (Tan et al. 2001, Tan & Park 2002).

The method of measuring angular accelerations using accelerometers was first proposed by Schuler et al. (1967), and the estimating method for angular velocities using distributed accelerometers was proposed by Padgaonkar et al. (1975). The Army Research Laboratory in the USA carried out a research on GF-IMU application to artillery shells in 1994 (Harkins 1994). Tan & Park (2002) designed an INS based on accelerometers for ground vehicles as part of the Partners for Advanced Transit and Highways (PATH) program, and analyzed the performance. Clark (2003) estimated angular velocities and velocities of re-entry vehicle using distributed six accelerometers. Pamadi et al. (2004) performed a research on Global Positioning System (GPS)-guided spinning vehicle using an accelerometer-based IMU. Hanson & Pachter (2005) showed a research result on geometric location method of the GF-IMU.

The inertial navigation algorithm can be represented in various frames including the earth centered inertial (ECI) frame, the earth centered earth fixed (ECEF) frame, the navigation frame, and the Wander-azimuth frame (Wray & Flynn 1979, Titterton & Weston 2004). Vehicle motion in the Earth are usually expressed in the ECEF frame and navigation frame (Wei & Schwarz 1990, Titterton & Weston 2004).

Generally, a velocity of a vehicle in the Earth is expressed in the navigation frame, and a position is by latitude, longitude, and altitude (Titterton & Weston 2004). Even though the inertial navigation algorithm in the ECEF frame has a less computation burden than that in the navigation frame (Wei & Schwarz 1990), the velocity has to be converted into the velocity in the navigation frame, and the position into the latitude, longitude, and altitude (Titterton & Weston 2004). The velocity and the position can be directly obtained when the algorithm is expressed in the navigation frame. Furthermore, a simpler model of the gravity can be used in the calculation since it is only in the down axis (Titterton & Weston 2004).

The gyro-free inertial navigation algorithm can also be expressed in the ECEF frame or navigation frame as in the conventional INS. The mechanization of gyro-free inertial navigation system (GF-INS) expressed in the ECEF frame was proposed by Pachter et al. (2013).

In this paper a mechanization of the GF-INS in the navigation frame is derived. In addition, a gyro-free inertial navigation algorithm based on the derived mechanization is porposed. It is shown that the proposed algorithm can be applied to the spinning vehicle with high rotation rate.

In Section 2, a GF-INS is briefly introduced and a mechanization of the GF-INS in the navigation frame is derived. In Section 3, an algorithm that estimates angular velocity, attitude, velocity, and position is proposed based on the mechanization in the navigation frame. Section 4 shows the application of the proposed GF-INS algorithm to the spinning vehicle with high rotation rate. Finally, in Section 5, concluding remarks and future studies are presented.

2. MECHANIZATION OF THE GF-INS IN THE NAVIGATION FRAME

Fig. 1 shows the block diagram of N accelerometers distributed in the vehicle and GF-INS. As shown in Fig. 1, the GF-INS estimates angular velocities from outputs of accelerometers, and attitude, velocity, and position of the vehicle are estimated from the estimated angular velocity and the output of the accelerometers.

Fig. 1. Gyro-free inertial navigation system.

Fig. 2. Accelerometer in the vehicle.

Fig. 2 shows the N accelerometers in the vehicle. The acceleration at where accelerometer is placed, is given in Eq. (1) when the vehicle moves.

\(a_{acc}^b=a^b+{\dot{\mathbf{\omega}}}_{ib}^b\times\mathbf{r}_{acc}^b+\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{acc}^b)\)                                                                                        (1)

where \(a^b\) denotes the acceleration of the vehicle in the body frame. \(\omega_{ib}^b\) is the angular velocity in the body frame. If \(\mathbf{d}_{acc}\) is the unit vector of the accelerometer input, the measured acceleration \(y_{acc}\) can be expressed as Eq. (2).

\(y_{acc}=\mathbf{d}_{acc}^\mathbf{T}a^b+(\mathbf{r}_{acc}^b\times\mathbf{d}_{acc})^\mathbf{T}{\dot{\mathbf{\omega}}}_{ib}^b\ +\mathbf{d}_{acc}^\mathbf{T}(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{acc}^b))-\mathbf{d}_{acc}^\mathbf{T}\mathbf{g}^b\)                                                                 (2)

where \(\mathbf{f}^b\) denotes the specific force in the body frame. \(\mathbf{g}^b\) is gravity in the body frame. Measured accelerations by the N accelerometers can be written in Eq. (3) from Eq. (2).

\(\left[\begin{matrix}y_1^b\\y_2^b\\\vdots\\y_N^b\\\end{matrix}\right]=\left[\begin{matrix}(\mathbf{r}_{acc1}^b\times\mathbf{d}_{acc1})^T&\mathbf{d}_{acc1}^T\\(\mathbf{r}_{acc2}^b\times\mathbf{d}_{acc2})^T&\mathbf{d}_{acc2}^T\\\vdots&\vdots\\(\mathbf{r}_{accN}^b\times\mathbf{d}_{accN})^T&\mathbf{d}_{accN}^T\\\end{matrix}\right]\left[\begin{matrix}{\dot{\mathbf{\omega}}}_{ib}^b\\a^b\\\end{matrix}\right]-\left[\begin{matrix}\mathbf{d}_{acc1}^T\cdot\mathbf{g}^b\\\mathbf{d}_{acc2}^T\cdot\mathbf{g}^b\\\vdots\\\mathbf{d}_{accN}^T\cdot\mathbf{g}^b\\\end{matrix}\right]+\left[\begin{matrix}\mathbf{d}_{acc1}^T(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{acc1}^b))\\\mathbf{d}_{acc2}^T(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{acc2}^b))\\\vdots\\\mathbf{d}_{accN}^T(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{accN}^b))\\\end{matrix}\right]\)                                                  (3)

2.1 Angular Velocity Differential Equation

If terms in Eq. (3) is expressed in Eqs. (4-7), Eq. (3) can be written in Eq. (8).

\(Y=\left[\begin{matrix}y_1^b&y_2^b&\cdots&y_N^b\\\end{matrix}\right]^\mathbf{T}\)                                                                                              (4)

\(\mathbf{N}=\left[\begin{matrix}\mathbf{d}_{acc1}^T(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{acc1}^b))\\\mathbf{d}_{acc2}^T(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{acc2}^b))\\\vdots\\\mathbf{d}_{accN}^T(\mathbf{\omega}_{ib}^b\times(\mathbf{\omega}_{ib}^b\times\mathbf{r}_{accN}^b))\\\end{matrix}\right]\)                                                                                          (5)

\(\mathbf{H}=\left[\begin{matrix}(\mathbf{r}_{acc1}^b\times\mathbf{d}_{acc1})^T&\mathbf{d}_{acc1}^T\\(\mathbf{r}_{acc2}^b\times\mathbf{d}_{acc2})^T&\mathbf{d}_{acc2}^T\\\vdots&\vdots\\(\mathbf{r}_{accN}^b\times\mathbf{d}_{accN})^T&\mathbf{d}_{accN}^T\\\end{matrix}\right]\)                                                                                          (6)

\(\mathbf{D}\mathbf{g}^b=\left[\begin{matrix}\mathbf{d}_{acc1}^T\cdot\mathbf{g}^b&\mathbf{d}_{acc2}^T\cdot\mathbf{g}^b&\cdots&\mathbf{d}_{accN}^T\cdot\mathbf{g}^b\\\end{matrix}\right]^\mathbf{T}\)                                                                          (7)

\(\mathbf{Y}=\mathbf{H}\left[\begin{matrix}{\dot{\mathbf{\omega}}}_{ib}^b\\a^b\\\end{matrix}\right]-\mathbf{D}g+\mathbf{N} \)                                                                                              (8)

From Eq. (8), angular velocity and acceleration can be written in Eq. (9).

\(\left[\begin{matrix}{\dot{\mathbf{\omega}}}_{ib}^b\\a^b\\\end{matrix}\right]=(\mathbf{H}^\mathbf{T}\mathbf{H})\mathbf{H}^{-1}(\mathbf{Y}+\mathbf{D}g-\mathbf{N})\)                                                                                    (9)

If \(\mathbf{A}\) is defined as in Eq. (10), Eq. (9) can be written in Eq. (11).

\(\mathbf{A}=\left[\begin{matrix}\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}^b3\times N}\\\mathbf{A}_{a^b3\times N}\\\end{matrix}\right]=(\mathbf{H}^\mathbf{T}\mathbf{H})\mathbf{H}^{-1}\)                                                                                      (10)

\(\left[\begin{matrix}{\dot{\mathbf{\omega}}}_{ib}^b\\a^b\\\end{matrix}\right]=\left[\begin{matrix}\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}^b}\mathbf{Y}+\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}^b}\mathbf{D}g-\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}^b}\mathbf{N}\\\mathbf{A}_{a^b}\mathbf{Y}+\mathbf{A}_{a^b}\mathbf{D}g-\mathbf{A}_{a^b}\mathbf{N}\\\end{matrix}\right]\)                                                                                (11)

Since \(\mathbf{N}\) is a function of \({\mathbf{\omega}}_{ib}^b\), Eq. (11) can be re-written as in Eqs. (12) and (13) (Pachter et al. 2013).

\({\dot{\mathbf{\omega}}}_{ib}^b=\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}}\mathbf{Y}+\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}}\mathbf{D}\mathbf{g}^b-\mathbf{f}_{{\dot{\mathbf{\omega}}}_{ib}}(\mathbf{\omega}_{ib}^b)\)                                                                                    (12)

\(a^b=\mathbf{A}_{a^b}\mathbf{Y}+\mathbf{A}_{a^b}\mathbf{D}\mathbf{g}^b-\mathbf{f}_{a^b}(\mathbf{\omega}_{ib}^b) \)                                                                                      (13)

 

where \(\mathbf{f}_{{\dot{\mathbf{\omega}}}_{ib}}(\mathbf{\omega}_{ib}^b)\) and \(\mathbf{f}_{a^b}(\mathbf{\omega}_{ib}^b)\) are represented as \(\mathbf{A}_{{\dot{\mathbf{\omega}}}_{ib}^b}\mathbf{N}\) and \(\mathbf{A}_{a^b}\mathbf{N}\) in order to emphasize functions of \(\mathbf{\omega}_{ib}^b\), respectively.

It can be seen in Eq. (12) that \(\mathbf{g}^b\) should be calculated first in order to obtain a solution of the angular velocity differential equation. When the INS is represented in the navigation frame, the gravity can be calculated using a simpler model since the gravity is only in the down axis (Wei & Schwarz 1990, Titterton & Weston 2004).

2.2 Attitude Differential Equation

The attitude differential equations in the ECEF frame and the navigation frame are given in Eqs. (14) and (15), respectively (Wei & Schwarz 1990, Titterton & Weston 2004).

\({\dot{\mathbf{C}}}_b^e=\mathbf{C}_b^e\mathbf{\Omega}_{eb}^b \)                                                                                                               
\(=\mathbf{C}_b^e(\mathbf{\Omega}_{ib}^b-\mathbf{\Omega}_{ie}^b)\)                                                                                              (14)

\({\dot{\mathbf{C}}}_b^n=\mathbf{C}_b^n\mathbf{\Omega}_{nb}^b \)                                                                                                               
\( =\mathbf{C}_b^n\mathbf{\Omega}_{ib}^b-\mathbf{C}_b^n(\mathbf{\Omega}_{ie}^b-\mathbf{\Omega}_{en}^b)\)                                                                                (15)

where \(\mathbf{C}_b^e\) denotes the Direction Cosine Matrix (DCM) which defines the attitude of the body frame with respect to the ECEF frame. \(\mathbf{\Omega}_{eb}^b\), \(\mathbf{\Omega}_{ib}^b\), and \(\mathbf{\Omega}_{ie}^b\) denotes the skew-symmetric matrix of \(\mathbf{\omega}_{eb}^b\), angular velocity of the body frame with respect to the ECEF frame in the body frame, \(\mathbf{\omega}_{ib}^b\), angular velocity of the body frame with respect to the inertial frame in the body frame, and \(\mathbf{\omega}_{ie}^b\) angular velocity of the ECEF frame with respect to the inertial frame in the body frame, respectively. \(\mathbf{C}_b^n\) is the DCM which defines the attitude of the body frame with respect to the navigation frame. \(\mathbf{\Omega}_{nb}^b\) and \(\mathbf{\Omega}_{en}^b\) are the skew-symmetric matrices of \(\mathbf{\omega}_{nb}^b\), angular velocity of the body frame with respect to the navigation frame in the body frame, and \(\mathbf{\omega}_{en}^b\), angular velocity of the navigation frame with respect to the ECEF frame in the body frame, respectively.

It can be seen in Eq. (15) that angular velocity in the navigation frame with respect to the ECEF frame should be calculated first to obtain the solution of the attitude differential equation in the navigation frame. Therefore, a computation burden is lower than that of Eq. (15) when a solution of the attitude differential equation in the ECEF frame is calculated as in Eq. (14) (Wei & Schwarz 1990).

The roll, pitch, and yaw of the vehicle can be directly obtained from \(\mathbf{C}_b^n\) which in a solution of Eq. (15). However, the attitude of the vehicle in Euler angle cannot be obtained directly from the solution of Eq. (14) (Wei & Schwarz 1990, Titterton & Weston 2004).

2.3 Velocity Differential Equation

Before the velocity differential equation is derived in the navigation frame that in the ECEF frame is derived. The relationship between the position vector \(\mathbf{R}^i\) in the inertial frame and the position vector \(\mathbf{R}^e\) in the ECEF frame is given in Eq. (16).

\(\mathbf{R}^i=\mathbf{C}_e^i \mathbf{R}^e\)                                                                                                      (16)

By differentiating Eq. (16), Eq. (17) is obtained.

\({\dot{\mathbf{R}}}^i=\mathbf{C}_e^i\mathbf{\Omega}_{ie}^e\mathbf{R}^e+\mathbf{C}_e^i{\dot{\mathbf{R}}}^e\)                                                                                              (17)

By differentiating Eq. (17) again, Eq. (18) is obtained.

\({\ddot{\mathbf{R}}}^i=\mathbf{C}_e^i\mathbf{\Omega}_{ie}^e\mathbf{\Omega}_{ie}^e\mathbf{R}^e+2\mathbf{C}_e^i\mathbf{\Omega}_{ie}^e{\dot{\mathbf{R}}}^e+\mathbf{C}_e^i{\dot{\mathbf{\Omega}}}_{ie}^e\mathbf{R}^e+\mathbf{C}_e^i{\ddot{\mathbf{R}}}^e\)                                                                      (18)

From Eq. (18), the velocity differential equation in the ECEF frame is given in Eq. (19) (Wei & Schwarz 1990, Titterton & Weston 2004).

\({\dot{\mathbf{v}}}_e^e=\mathbf{C}_b^ea^b-\mathbf{\omega}_{ie}^e \times(\mathbf{\omega}_{ie}^e\times \mathbf{R}^e)-2\mathbf{\omega}_{ie}^e\times\)                                                                                  (19)

where \(\mathbf{v}_e^e\) is the ground velocity in the ECEF frame.

By substituting Eq. (13) into Eq. (19), the velocity differential equation in the ECEF frame is obtained as Eq. (20) (Pachter et al. 2013).

\({\dot{\mathbf{v}}}_e^e=\mathbf{C}_b^e(\mathbf{A}_{a^b}\mathbf{Y}+\mathbf{A}_{a^b}\mathbf{C}_e^b \mathbf{D}\mathbf{g}^e-\mathbf{f}_{a^b}(\mathbf{\omega}_{ib}^b))-2\mathbf{\omega}_{ie}^e\times\mathbf{v}_e^e -\mathbf{\omega}_{ie}^e\times(\mathbf{\omega}_{ie}^e\times\mathbf{R}^e)\)                                                      (20)

The relationship between velocity \(\mathbf{v}_e^n\) in the navigation frame and that in the ECEF frame is given in Eq. (21).

\(\mathbf{v}_e^n=\mathbf{C}_e^n\mathbf{v}_e^e\)                                                                                                              (21)

where \(\mathbf{C}_e^n\) is the DCM which defines the attitude of the ECEF frame with respect to the navigation frame.

By differentiating Eq. (21), Eq. (22) is obtained.

\({\dot{\mathbf{v}}}_e^n=\mathbf{C}_e^n{\dot{\mathbf{v}}}_e^e-\mathbf{C}_e^n\mathbf{\Omega}_{en}^e\mathbf{v}_e^e\)                                                                                                  (22)

By substituting Eq. (19) to Eq. (22), Eq. (23) is obtained.

  \({\dot{\mathbf{v}}}_e^n=\mathbf{C}_b^na^b-(2\mathbf{\omega}_{ie}^n+\mathbf{\omega}_{en}^n)\times\mathbf{v}_e^n-\mathbf{\omega}_{ie}^n\times(\mathbf{\omega}_{ie}^n \times\mathbf{R}^n) \)                                                                              (23)

By substituting Eq. (15) to Eq. (23), the velocity differential equation in the navigation frame is obtained as Eq. (24).

\({\dot{\mathbf{v}}}_e^n=\mathbf{C}_b^n(\mathbf{A}_{a^b}\mathbf{Y}+\mathbf{A}_{a^b}\mathbf{C}_n^b\mathbf{D}\mathbf{g}^n-\mathbf{f}_{a^b}(\mathbf{\omega}_{ib}^b))-(2\mathbf{\omega}_{ie}^n+\mathbf{\omega}_{en}^n)\times\mathbf{v}_e^n-\mathbf{\omega}_{ie}^n\times(\mathbf{\omega}_{ie}^n\times\mathbf{R}^n)\)                                                   (24)

2.4 Position Differential Equation

The position differential equation in the ECEF frame is given in Eq. (25) (Wei & Schwarz 1990, Pachter et al. 2013).

\({\dot{\mathbf{R}}}^e=\mathbf{v}_e^e\)                                                                                                                (25)

The position differential equation in the navigation frame is given in Eqs. (26-28) (Titterton & Weston 2004).

\(\dot{L}=\frac{(\mathbf{v}_e^n)_N}{R_m+h}\)                                                                                                               (26)

\(\dot{l}=\frac{(\mathbf{v}_e^n)_E}{(R_t+h)\cos {L}}\)                                                                                                          (27)

\(\dot{h}=-(\mathbf{v}_e^n)_D\)                                                                                                             (28)

where \(L\) denotes the latitude. \(l\) denotes the longitude. \(h\) denotes the altitude. \((\mathbf{v}_e^n)_N\) is the north axis component of the velocity vector in the navigation frame. \((\mathbf{v}_e^n)_E\) is the east axis component. \((\mathbf{v}_e^n)_D\) is the down axis component. \(R_m\) is the meridional radius of the Earth, and \(R_i\) is the tangential radius of the Earth.

As shown in Eqs. (26-28), the latitude, longitude, and altitude can be calculated directly from the velocity through position differential equation expressed in the navigation frame (Titterton & Weston 2004).

3. Navigation algorithm

3.1 Angular Velocity Estimation Algorithm

Attitude can be estimated from the outputs of the accelerometers using the angular velocity differential equation Eq. (12). The angular velocity is estimated by integrating the outputs from accelerometer array.

Let the sampling time be and be the value of at time . The angular velocity differential equation Eq. (12) is approximated by the Euler method as Eq. (29).

\(\mathbf{\omega}_{ib}^b[k+1]=\omega_{ib}^b[k]+(\mathbf{A}_{\dot{\omega}_{ib}}\mathbf{Y}[k]+\mathbf{A}_{\dot{\omega}_{ib}}\mathbf{Dg}[k]-\mathbf{f}_{\dot{\omega}_{ib}}(\omega_{ib}^b[k]))\Delta t\)                                                                      (29)

The angular velocity \(\mathbf{\omega}_{ib}^b[k+1]\) at the current time can be calculated from the array of the accelerometers, gravity, the accelerometer output \(\mathbf{Y}[k]\), and angular velocity \(\mathbf{\omega}_{ib}^b[k]\) at the previous time using Eq. (29).

3.2 Attitude Estimation Algorithm

The attitude can be estimated from the angular velocity using the attitude differential equation Eq. (15). The solution is obtained using the attitude differential equation Eq. (30) represented by the quaternion since it is known to have the least numerical error (Titterton & Weston 2004).

\({\dot{\mathbf{Q}}}_b^n=\frac{1}{2}\mathbf{W}\mathbf{Q}_b^n\)                                                                                                      (30)

where \(\mathbf{W}\) is given in Eq. (31).

\(\mathbf{W}=\left[\begin{matrix}0&-\omega_{nb,x}^b&-\omega_{nb,y}^b&-\omega_{nb,z}^b\\\omega_{nb,x}^b&0&\omega_{nb,z}^b&-\omega_{nb,y}^b\\\omega_{nb,y}^b&-\omega_{nb,z}^b&0&\omega_{nb,x}^b\\\omega_{nb,z}^b&\omega_{nb,y}^b&-\omega_{nb,x}^b&0\\\end{matrix}\right]\)                                                                                  (31)

where \(\mathbf{\omega}_{nb}^b\) is the angular velocity of the body frame with respect to the navigation frame in the body frame. Assuming that the angular velocity , is constant from to , and its integration is , can be represented in Eq. (32) (Bose 1997).

\(\Delta\theta_{nb}(k\Delta t)=\int_{k\Delta t}^{(k+1)\Delta t}{\mathbf{\omega}_{nb}^b\left(k\Delta t\right)dt}=\mathbf{\omega}_{nb}^b((k+1)\Delta t-k\Delta t)=\mathbf{\omega}_{nb}^b[k]⋅Δt \)                                                      (32)

Using Eq. (32), \(\Delta\theta_{nb}[k]\) and \(\Delta\theta_{nb}[k-1]\) can be calculated as in Eqs. (33) and (34), respectively.

\(\Delta\theta_{nb}[k]=\omega^{nb}_b [k]⋅Δt \)                                                                                                  (33)

\(\Delta\theta_{nb}[k-1]=\omega_{nb}^b[k-1]⋅\Delta t\)                                                                                            (34)

If \(\phi(t)\) is the rotation vector at time \(t\), the differential equation with respect to \(\phi(t)\) can be written in Eq. (35).

\(\dot{\phi}(t)=\mathbf{\omega}_{nb}^b(t)+\frac{1}{2}\phi(t)\times\mathbf{\omega}_{nb}^b(t)\)                                                                                          (35)

where \(\times\) denotes the vector outer-product operator.

Letting \(\Delta\phi[k]\) be integration of \(\dot{\phi}(t)\) from \(t=k\Delta t\) to \(t=k\Delta t\), Eq. (36) is obtained from Eq. (35).

\(\Delta\phi[k]=\int_{k\Delta t}^{(k+1)\Delta t}\dot {\phi}(t)dt= \int_{k\Delta t}^{(k+1)\Delta t}\omega_{nb}^b(t)dt+ \int_{k\Delta t}^{(k+1)\Delta t} {1 \over 2}(\phi(t)\times \omega_{nb}^b(t))dt\)                                                          (36)

Eq. (37) is obtained by re-arranging Eq. (36).

\(\Delta\phi[k]=\Delta\theta_{nb}[k]+{1 \over 12}\Delta\theta_{nb}[k-1]\times \Delta\theta_{nb}[k] \)                                                                                    (37)

The solution can be updated as Eq. (38) using \(\Delta\phi[k]\) in Eq. (37) (Bose 1997, Titterton & Weston 2004).

\(Q[k]=e^{\Delta \phi[k] \over 2} \cdot Q[k-1]\)                                                                                                      (38)

where ‘\(\cdot\)’ is the quaternion product operator.

3.3 Velocity Estimation Algorithm

Eq. (24) is approximated to Eq. (39) using the Euler method.

\(\Delta\mathbf{V}_e^n[k]=(\mathbf{C}_b^n[k-1]\mathbf{a}^b[k-1]-(2\omega_{ie}^n [k-1]+\omega_{en}^n[k-1])\times \mathbf{V}_e^n[k-1])\Delta t\)                                                          (39)

The solution is given in Eq. (40).

\(\mathbf{V}_e^n[k]=\mathbf{V}_e^n[k-1]+ {(\Delta \mathbf{V}_e^n[k]+\Delta \mathbf{V}_e^n[k-1]) \over 2}\)                                                                                            (40)

3.4 Position Estimation Algorithm

In order to obtain the position of the vehicle, the attitude differential equation given in Eq. (41) is solved.

\({\dot{\mathbf{C}}}_n^e=\mathbf{C}_n^e\mathbf{\Omega}_{en}^n\)                                                                                                                (41)

The latitude and longitude of the vehicle is obtained from the solution \({\dot{\mathbf{C}}}_n^e\) and the altitude from Eq. (28).

Assume that the angular velocity \(\mathbf{\omega}_{en}^n(t)\) is constant from \(t=k\Delta t\) to \(t=(k+1)\Delta t\). Assuming that the rotation vector of \(\mathbf{\omega}_{en}^n[k]\) is \(\zeta(t)\), integration of \(\zeta(t)\) from \(t=k\Delta t\) to \(t=(k+1)\Delta t\), \(\Delta\zeta[k]\) can be obtained. The attitude \(\Delta\mathbf{C}_k^{k-1}\) between the time \(k-1\) and the time \(k\) can be obtained as in Eq. (42) from \(\Delta\zeta[k]\) (Bose 1997).

\(\Delta\mathbf{C}_k^{k-1}=\mathbf{I}+{\sin | \Delta\zeta[k]| \over |\Delta\zeta[k]|} (\Delta\zeta[k]×)+{(1-\cos | \Delta\zeta[k]|) \over |\Delta\zeta[k]|^2}(\Delta\zeta[k]\times )2\)                                                                          (42)

The solution can be updated as Eq. (43) using \(\Delta\mathbf{C}_k^{k-1}\) (Bose 1997).

\(\mathbf{C}_n^e[k]=\mathbf{C}_n^e[k-1]\Delta\mathbf{C}_k^{k-1}\)                                                                                                        (43)

The latitude and longitude is calculated as Eqs. (44) and (45) (Titterton & Weston 2004).

\(L[k]=\sin-1(-\mathbf{C}_n^e[k](3,3))\)                                                                                                      (44)

\(l[k]=\tan^{-1}\left ( {-\mathbf{C}_n^e[k](2,3) \over -\mathbf{C}_n^e[k](1,3)} \right )\)                                                                                                         (45)

The altitude can be obtained as Eq. (46).

\(h[k]=h[k-1]+{1 \over 2}(-(\mathbf{V}_e^n)_D[k]-(\mathbf{V}_e^n)_D[k-1])\Delta t\)                                                                              (46)

4. APPLICATION OF THE GF-INS ALGORITHM TO A SPINNING VEHICLE WITH HIGH ROTATION RATE

4.1 Software Platform

In order to observe the navigation result of the proposed GF-INS algorithm to a spinning vehicle with high rotation rate, a software platform based on MATLAB was configured as in Fig. 3. Since the target vehicle is rotated at a six or more round/s such as artillery projectiles and guided spinning projectiles, the angular velocity is difficult to measure using conventional gyroscopes (Mickelson 2000, Doty & McGraw 2003). The software platform consists of a sensor output generator, an angular velocity estimation algorithm, and an attitude, velocity, and position estimation algorithm. The sensor output generator includes a reference trajectory generator, an angular velocity generator, and a GF-IMU output generator. The reference trajectory generator generates a motion trajectory of the rotating vehicle. The rotation rate can be adjusted from 1 round/s to 30 round/s. The angular velocity is generated from a motion trajectory of the vehicle. The GF-IMU output is generated from a motion trajectory of the vehicle as shown in Fig. 3. The distance from the center of gravity of the vehicle to the accelerometer is adjustable. The angular velocity is estimated from the output of the accelerometer array and the attitude, velocity, and position are estimated from the estimated angular velocity. The estimated angular velocity, attitude, and position estimation performance can be evaluated by comparing with the reference trajectory. In order to evaluate the proposed algorithm for the spinning vehicle with 30 round/s rotation rate, accelerometer outputs were generated for the arrangement given in Fig. 4.

Fig. 3. Software platform for evaluation of GF-INS algorithm

Fig. 4. 4 accelerometer-triads arrangement.

4.2 Reference Trajectory and Results of the Performance Evaluation on the Proposed GF-INS Algorithm

The three-dimensional position trajectory of the vehicle is given in Fig. 5. Fig. 6 shows the reference trajectories of the vehicles with 3 round/s and 30 round/s. In order to compare the result of the proposed GF-INS algorithm with the conventional INS algorithm, the trajectory of the vehicle with 3 round/s was generated. For the conventional INS, gyroscope outputs were generated. Fig. 7 shows enlarged roll trajectories for 1 second. The initial position of the vehicle is 36° of latitude, 127°of longitude, and 0 m of altitude.

Fig. 5. 3 Dimensional reference position trajectory of the vehicle.

Fig. 6. Reference trajectories of the vehicle.

Fig. 7. Expanded trajectory plot of the roll during 1 sec.

Table 1. Specification of MPU-9250.

		Accelerometer	Gyroscope
Range Noise density Bias instability	x axis y axis z axis	±16 g 300 μg/√Hz ±60 mg ±80 mg ±80 mg	±2000°/s 0.01°/s/√Hz ±5°/s ±5°/s ±5°/s

 

Performance evaluation on the navigation algorithm was performed for MPU-9250, which is a Micro Electro Mechanical Systems (MEMS) IMU manufactured by Invensense Inc. Characteristics of the MPU-9250 is given in Table 1.

Before evaluating the performance of GF-INS algorithm for spinning vehicle with a high rotation rate, the performance of GF-INS algorithm was compared with that of a conventional INS algorithm when the vehicle rotates with a low rotation rate.

When the vehicle is rotated at 3 round/s and there is no sensor error, the navigation results are shown in Fig. 8, Fig. 9, and Table 2. The initial angular velocity error was set to zero. It can be seen from Fig. 9 and Table 2 that the angular velocity error in the x-axis of the GF-INS algorithm is less than 10^-7 °/s, and y-axis and z-axis less than 10^-2 °/s. The errors are caused by numerical computation. It can be seen from Figs. 8 and 9 that the roll error is both less than 0.5°, and pitch error is both less than 0.01° when angular velocity is measured by the gyro and angular velocity is estimated by the GF-INS. It also can be seen that the heading error is larger when angular velocity is estimated by the GF-INS.

Fig. 8. Navigation result without sensor error.

Fig. 9. Navigation result without sensor error.

Table 2. Navigation result without sensor error.

		Using Gyroscopes	GF-INS
Angular rate error (deg/s, RMSE)	x y z	0 0 0	0.000000056 0.0097 0.0097
Attitude error (deg, RMSE)	Roll Pitch Heading	0.0450 0.0011 0.1099	0.3144 0.0048 0.7015
Velocity error (m/s, RMSE)	North East Down	1.7353 6.7288 0.1376	5.1354 6.4556 0.1448
Position error (m, RMSE)	North East Down	114.9788 497.0630 6.3627	218.3997 412.2946 7.2036

 

When the vehicle is rotated at 3 round/s and the noise and bias error in Table 1 are included in the sensor outputs, the navigation results are shown in Fig. 10, Fig. 11, and Table 3. The initial angular velocity errors were set to 2 °/s in all the axes. It can be seen from Fig. 11 and Table 3 that the angular velocity error of the GF-INS algorithm are larger than 10⁶ °/s and attitude errors in all the axes are larger than 60°. The errors are due to initial angular velocity errors and accelerometer errors. It can be observed from Figs. 10 and 11 and Table 3 that errors are smaller when gyros are used to have angular velocities.

Fig. 10. Navigation result with sensor error.

Fig. 11. Navigation result with sensor error.

Table 3. Navigation result with sensor error.

		Using Gyroscopes	GF-INS
Angular rate error (deg/s, RMSE)	x y z	1.6671 1.5097 1.7890	128240 166440 195080
Attitude error (deg, RMSE)	Roll Pitch Heading	57.9043 28.5607 63.2604	102.7170 68.6790 115.1441
Velocity error (m/s, RMSE)	North East Down	152.3793 146.5936 127.0294	209.9362 160.2155 247.3606
Position error (m, RMSE)	North East Down	7650.9 7655.8 5822.2	13799 10205 9084.2

 

When the vehicle moves with 30 round/s rotation rate, only accelerometer array outputs were generated. When there is no sensor error, the navigation results are shown in Fig. 12 and Table 4. The initial angular velocity error was set to zero. It can be seen from Fig. 12 and Table 4 that the angular velocity error in the x-axis of the GF-INS algorithm is less than 10^-8 °/s, and y-axis and z-axis less than 10^-2 °/s. The errors are caused by numerical computation.

Fig. 12. Navigation result without sensor error.

Table 4. Navigation result without sensor error.

Angular rate error (deg/s, RMSE)	x y z	0.000000071 0.0743 0.0743
Attitude error (deg, RMSE)	Roll Pitch Heading	2.4351 0.1294 5.2991
Velocity error (m/s, RMSE)	North East Down	37.3522 27.6929 1.0340
Position error (m, RMSE)	North East Down	1165.0 539.5082 53.1754

 

When the noise and bias error in Table 1 are included in the sensor outputs, the navigation results are shown in Fig. 13 and Table 5. The initial angular velocity errors were set to 2°/s in all the axes. It can be seen from Fig. 11 and Table 3 that the angular velocity errors of the GF-INS algorithm are larger than 10⁶°/s in all the axes and the attitude errors in all the axes are larger than 60°. It can be seen that the accelerometer error and attitude error affects the velocity and position estimation.

Fig. 13. Navigation result with sensor error.

Table 5. Navigation result with sensor error.

Angular rate error (deg/s, RMSE)	x y z	186670 134460 103190
Attitude error (deg, RMSE)	Roll Pitch Heading	104.4672 68.9455 90.3799
Velocity error (m/s, RMSE)	North East Down	172.3704 112.2375 247.4588
Position error (m, RMSE)	North East Down	10987.0 6615.2 8777.4

 

When the vehicle is rotated at a low rotation rate 3 round/s, an angular velocity can be measured using a gyro. When the vehicle is rotated at a high rotation rate, angular velocities can be estimated using the proposed algorithm. However, navigation performance is significantly degraded when there is the initial angular velocity error and sensor errors. In that case, a Kalman filter can be used (Santiago 1992, Edwan et al. 2011), or aiding sensors as GPS can be used to compensate the errors (Titterton & Weston 2004, Pachter et al. 2013).

5. CONCLUDING REMAKRS AND FURTHER STUDIES

A mechanization for the GF-INS in the navigation frame was derived and GF-INS algorithm based on the derived equation was proposed. The proposed algorithm was used to estimate angular velocity, attitude, velocity, and position of spinning vehicle with high rotation rate. The navigation results of the proposed GF-INS algorithm and those when gyros are used were presented. It was shown that the proposed algorithm could be used in the navigation algorithm of the spinning vehicle with high rotation rate. Although the GF-INS navigation algorithm can be used in the navigation algorithm of spinning vehicle with high rotation rate, the result showed the navigation performance is significantly degraded due to sensor errors and initial angular velocity error.

As further studies, algorithm that estimates an angular velocity using an extended Kalman filter, and integrated navigation system of GPS and GF-INS will be carried out to compensate the navigation error.