Extended Kalman Filter Based GF-INS Angular Velocity Estimation Algorithm

Abstract

When a vehicle moves with a high rotation rate, it is not easy to measure the angular velocity using an off-the-shelf gyroscope. If the angular velocity is estimated using the extended Kalman filter in the gyro-free inertial navigation system, the effect of the accelerometer error and initial angular velocity error can be reduced. In this paper, in order to improve the navigation performance of the gyro-free inertial navigation system, an angular velocity estimation method is proposed based on an extended Kalman filter with an accelerometer random bias error model. In order to show the validity of the proposed estimation method, angular velocities and navigation outputs of a vehicle with 3 rev/s rotation rate are estimated. The results are compared with estimates by other methods such as the integration and an extended Kalman filter without an accelerometer random bias error model. The proposed method gives better estimation results than other methods.

1. INTRODUCTION

Gyroscopes in the conventional Inertial Measurement Unit (IMU) may not be suitable for the angular velocity measurement of a vehicle with high rotation rates due to its range of operation. That is, the conventional Strap-Down Inertial Navigation System (SDINS) may not be appropriate to the vehicle with high rotation rates (Padgaonkar et al. 1975, Mickelson 2000, Costello & Webb 2003). In order to resolve this problem, the Gyro-Free Inertial Navigation System (GF-INS), in which only accelerometers are used instead of gyroscopes, has been proposed (Schuler et al. 1967, Hanson & Pachter 2005). Schuler et al. (1967) showed that rotational accelerations were measured from accelerometers arranged on a vehicle and angular velocities of the vehicle could be obtained by integrating measured rotational accelerations. This accelerometers arrangement on the vehicle is called Gyro-Free IMU (GF-IMU) (Chen et al. 1994, Hanson & Pachter 2005). Theoretically, the angular velocity vector in the three dimensional space can be obtained from outputs of six accelerometers (Schuler et al. 1967).

When angular velocities are estimated from six accelerometers, it is difficult to accurately determine the angular velocities if a direct impact, i.e., a high linear acceleration or a high angular acceleration is applied to the vehicle (Padgaonkar et al. 1975, Santiago 1992). To avoid this difficulty, a method using nine accelerometers was proposed (Padgaonkar et al. 1975). On the contrary, Chen et al. (1994) showed that angular velocities could be obtained by placing one accelerometer at the center of each face of a cube even though a direct impact is applied to the vehicle. In this arrangement, the sensing axis of each accelerometer is along the diagonal of respective cube face.

If angular velocities are estimated by integrating outputs of accelerometers, measurement errors of accelerometers are accumulated (Padgaonkar et al. 1975). In this case, navigation errors of the GF-INS increase more rapidly than those of SDINS, in which the angular velocity is measured using a gyroscope (Park et al. 2005). Due to this characteristic, a method for reducing angular velocity estimation error is required in order to apply the GF-INS to long-term navigation.

Algrain & Saniie (1991) estimated the angular velocity using a static model between angular velocity and accelerometer output instead of integrating the accelerometer output. They showed that effect of the Gaussian noise to the angular velocity estimate could be reduced by using a linear Gaussian estimator based on the linear model with the Gaussian noise.

Edwan et al. (2011) estimated angular velocities from accelerometer outputs using an extended Kalman filter with an accelerometer noise model. They showed that initial angular velocity error as well as the effect of the accelerometer noise could be reduced by considering dynamic characteristic between the angular velocity and accelerometer output. In addition to this, they showed through simulations that the random bias error of the accelerometer could be compensated by using an extended Kalman filter with only accelerometer noise model when a higher-grade accelerometer than the tactical-grade one is used.

Since the Micro-Electro Mechanical Systems (MEMS) accelerometer is smaller-sized and consumes lower power, it is suitable for very small-sized vehicles. Even though performance of the MEMS acceChenlerometer has been improved and many research results on MEMS accelerometer based GF-INSChen can be found in literatures (Hanson 2005, Pachter et al. 2013, Cucci et al. 2016, Nilsson & Skog 2016, Chatterjee et al 2017), performance of the MEMS accelerometer still does not reach that of a conventional high-grade accelerometer (Hanson & Pachter 2005, Cucci et al. 2016). Therefore, in order for the GF-INS using MEMS accelerometer to give the same or similar performance of that using a high- grade accelerometer, the effect of the MEMS accelerometer error should be reduced more when the angular velocity is estimated (Cucci et al. 2016). As a method to this end, the angular velocity can be estimated from the accelerometer output using an extended Kalman filter with an accelerometer random bias error model.

In this paper, a GF-INS angular velocity estimation method is proposed based on an extended Kalman filter with an accelerometer random bias error model. The estimated angular velocity and navigation results are compared with those by other methods.

In the sequel, the angular velocity estimation algorithm using an extended Kalman filter is described. in Section 2. In Section 3, performance evaluation results of the proposed estimation method are presented. Finally, in Section 4, concluding remarks and further studies are given.

2. ANGULAR VELOCITY ESTIMATION USING EXTENDED KALMAN FILTER

2.1 Gyro-free INS Mechanization

Fig. 1. Accelerometer arrangement in the vehicle.

An arrangement of N accelerometers in a vehicle is shown in Fig. 1. When the vehicle moves, the acceleration of the point where the accelerometer is located is given in Eq. (1) (Schuler et al. 1967).

\(\begin{align}\label{eq1}a_{a cc j}^{b}=a^{b}+\omega{'}_{i b}^{b} \times \mathbf{r}_{a c c j}^{b}+\omega_{i b}^{b} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c c j}^{b}\right)\end{align}\)       (1)

where \(a^b\), \(\omega ^{b}_{ib}\)and \(r^{b}_{accj}\)( =1, 2, ···, N) are acceleration of the center of gravity of the vehicle represented in the body frame, the angular velocity of the body frame with respect to the inertial frame represented in the body frame and the vector from the center of gravity of the vehicle to the   accelerometer represented in the body frame, respectively. Provided that the input axis orientation of the accelerometer is \(d^{T}_{accj'}\), the acceleration measured by the   accelerometer is given in Eq. (2).

\(\begin{aligned} y_{a c c j} &=\mathbf{d}_{a c c j}^{\mathrm{T}} \boldsymbol{a}^{b}+\left(\mathbf{r}_{a c c j}^{b} \times \mathbf{d}_{a c c j}\right)^{\mathrm{T}} \mathbf{\omega{'}}_{i b}^{b} \\ &+\mathbf{d}_{a c c j}^{\mathrm{T}}\left(\mathbf{\omega}_{i b}^{b} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c c j}^{b}\right)\right)-\mathbf{d}_{a c c j}^{\mathrm{T}} \mathbf{g}^{b} \end{aligned}\)       (2)

where \(g^{b}\) denotes the gravitation, vector represented in the body frame. The measured acceleration vector by all accelerometers is given in Eq. (3).

\(\left[\begin{array}{c}{y_{a c c1}^{b}} \\ {y_{a c c2}^{b}} \\ {\mathbf{M}} \\ {y_{a cc N}^{b}}\end{array}\right]=\left[\begin{array}{cc}{\left(\mathbf{r}_{a cc 1}^{b} \times \mathbf{d}_{a cc 1}\right)^{T}} & {\mathbf{d}_{a cc 1}^{T}} \\ {\left(\mathbf{r}_{ac c 2}^{b} \times \mathbf{d}_{a cc2}\right)^{T}} & {\mathbf{d}_{a cc 2}^{T}} \\ {\mathbf{M}} & {\mathbf{M}} \\ {\left(\mathbf{r}_{a cc N}^{b} \times \mathbf{d}_{a cc N}\right)^{T}} & {\mathbf{d}_{a cc N}^{T}}\end{array}\right] \left[\begin{array}{c}{\mathbf{\omega{'}}_{i b}^{b}} \\ {\boldsymbol{a}^{b}}\end{array}\right]- \left[\begin{array}{c}{\mathbf{d}_{a c c1}^{T} \mathbf{g}^{b}} \\ {\mathbf{d}_{a cc 2}^{T} \mathbf{g}^{b}} \\ {\mathbf{M}}\\ {\mathbf{d}_{a cc N}^{T} \mathbf{g}^{b}}\end{array}\right]+\left[\begin{array}{c}{\mathbf{d}_{a c c 1 }^{T}\left(\mathbf{\omega}_{ib}^{b} \times\left(\mathbf{\omega}_{ib}^{b} \times \mathbf{r}_{a cc 1}^{b}\right)\right)} \\ {\mathbf{d}_{a c c 2}^{T}\left(\mathbf{\omega}_{ib}^{b} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c c2}^{b}\right)\right)} \\ {\mathbf{M}} \\ {\left.\mathbf{d}_{a c cN}^{T} ( \omega^b_{ib} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c cN}^{b}\right)\right)}\end{array}\right]\)       (3)

Let the terms in Eq. (3) be represented as Eqs. (4-7).

\(\mathbf{Y}=\left[\begin{array}{llll}{y_{a c c 1}^{b}} & {y_{a c c 2}^{b}} & {\mathrm{L}} & {y_{a c c N}^{b}}\end{array}\right]^{\mathrm{T}}\)       (4)

\(\mathbf{N}=\left[\begin{array}{c}{\mathbf{d}_{a c c}^{T}\left(\boldsymbol{\omega}_{i b}^{b} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c c 1}^{b}\right)\right)} \\ {\mathbf{d}_{a c c 2}^{T}\left(\boldsymbol{\omega}_{i b}^{b} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c c 2}^{b}\right)\right)} \\ {\mathbf{M}} \\ {\mathbf{d}_{a c c N}^{T}\left(\mathbf{\omega}_{i b}^{b} \times\left(\mathbf{\omega}_{i b}^{b} \times \mathbf{r}_{a c c N}^{b}\right)\right)}\end{array}\right]\)       (5)

\(\mathbf{B}=\left[\begin{array}{cc}{\left(\mathbf{r}_{a c c1}^{b} \times \mathbf{d}_{a c c1}\right)^{T}} & {\mathbf{d}_{a c c 1}^{T}} \\ {\left(\mathbf{r}_{a cc 2}^{b} \times \mathbf{d}_{a c c 2}\right)^{T}} & {\mathbf{d}_{a cc 2}^{T}} \\ {M} & {M} \\ {\left(\mathbf{r}_{a cc N}^{b} \times \mathbf{d}_{a c c N}\right)^{T}} & {\mathbf{d}_{a c c N}^{T}}\end{array}\right]\)       (6)

\(\mathbf{D g}=\left[\begin{array}{llll}{\mathbf{d}_{a c c l}^{T} \mathbf{g}^{b}} & {\mathbf{d}_{a c c 2}^{T} \mathbf{g}^{b}} & {L} & {\mathbf{d}_{a cc N}^{T} \mathbf{g}^{b}}\end{array}\right]^{\mathbf{T}}\)       (7)

Let’s define   as Eq. (8).

\(\mathbf{A}=\left[\begin{array}{c}{\mathbf{A}_{\mathbf{\omega{'}}_{i b}^{b} 3 \times N}} \\ {\mathbf{A}_{a^{b} 3 \times N}}\end{array}\right]=\left(\mathbf{B}^{\mathrm{T}} \mathbf{B}\right) \mathbf{B}^{-1}\)       (8)

Angular velocity differential equation Eq. (9) and acceleration equation Eq. (10) can be obtained from Eq. (3).

\(\mathbf{\omega}_{i b}^{b}=\mathbf{A}_{\mathbf{\omega{'}}_{ib}^{b} 3 \mathbf{\times} N} \mathbf{Y}+\mathbf{A}_{\mathbf{\omega{'}}_{i b}^{b} 3 \mathbf{\times} N} \mathbf{D g}^{b}-\mathbf{A}_{\mathbf{\omega{'}}_{ib}^{b}3 \mathbf{\times} N} {N}\)       (9)

\(a^b =A_{a^b3 \times N}Y + A_{a^b 3 \times N}Dg^b - A_{a^b 3 \times N} N\)       (10)

2.2 Four Accelerometer-triads on the Center of Gravity and Three Axes

Consider the arrangement of accelerometers in Fig. 2. Four accelerometer-triads are in the center of gravity and three axes (Hanson 2005, Edwan et al. 2011). In this case, the vectors from the center of gravity to the j-th accelerometer, \(r^b_{accj}\) j=(1,2,...,12),are given in Eqs. (11-14).

\(\mathbf{r}_{a c c 1}^{b}=\mathbf{r}_{a c c 2}^{b}=\mathbf{r}_{a c c 3}^{b}=\left[\begin{array}{lll}{0} & {0} & {0}\end{array}\right]\)       (11)

\(\mathbf{r}_{a c c 4}^{b}=\mathbf{r}_{a c c 5}^{b}=\mathbf{r}_{a c c 6}^{b}=\left[\begin{array}{lll}{ } & {0} & {0}\end{array}\right]\)       (12)

\(\mathbf{r}_{a c c 7}^{b}=\mathbf{r}_{a c c 8}^{b}=\mathbf{r}_{a c c 9}^{b}=\left[\begin{array}{lll}{0} & {l} & {0}\end{array}\right]\)       (13)

\(\mathbf{r}_{a c c 10}^{b}=\mathbf{r}_{a c c 11}^{b}=\mathbf{r}_{a c c 12}^{b}=\left[\begin{array}{lll}{0} & {0} & {l}\end{array}\right]\)       (14)

where \( l \) denotes the distance from the center of gravity to the accelerometer in the axis. Input axis orientations \(d^T_{accj}\)(j = 1, 2, …, 12) are given in Eqs. (15-17).

\(\mathbf{d}_{a c c 1}^{\mathrm{T}}=\mathbf{d}_{a c c 4}^{\mathrm{T}}=\mathbf{d}_{a c c7 }^{\mathrm{T}}=\mathbf{d}_{a c c 10}^{\mathrm{T}}=\left[\begin{array}{ccc}{1} & {0} & {0}\end{array}\right]\)       (15)

\(\mathbf{d}_{a c c 2}^{\mathrm{T}}=\mathbf{d}_{a c c 5}^{\mathrm{T}}=\mathbf{d}_{a c c 8}^{\mathrm{T}}=\mathbf{d}_{a c c 11}^{\mathrm{T}}=\left[\begin{array}{ccc}{0} & {1} & {0}\end{array}\right]\)       (16)

\(\mathbf{d}_{a c c 3}^{\mathrm{T}}=\mathbf{d}_{a c c 6}^{\mathrm{T}}=\mathbf{d}_{a c c9 }^{\mathrm{T}}=\mathbf{d}_{a c c 12}^{\mathrm{T}}=\left[\begin{array}{ccc}{0} & {0} & {1}\end{array}\right]\)       (17)

Then, Eq. (18) is obtained from Eq. (8).

Fig. 2. 4 accelerometer-triads at the center of gravity and three axes.

\(\mathbf{A}_{\omega{'}_{ib}^{b}, 3 \times 12}=\frac{1}{2 l}\left[\begin{array}{cccccccccccc}{0} & {1} & {-1} & {0} & {0} & {0} & {0} & {0} & {1} & {0} & {-1} & {0} \\ {-1} & {0} & {1} & {0} & {0} & {-1} & {0} & {0} & {0} & {1} & {0} & {0} \\ {1} & {-1} & {0} & {0} & {1} & {0} & {-1} & {0} & {0} & {0} & {0} & {0}\end{array}\right]\)       (18)

Inserting Eq. (18) into Eq. (9), Eq. (19) is obtained (Algrain & Saniie 1991).

\(\left[\begin{array}{c}{\left(\mathbf{\omega{'}}_{i b}^{b}\right)_{x}} \\ {\left(\mathbf{\omega{'}}_{i b}^{b}\right)_{y}} \\ {\left(\mathbf{\omega{'}}_{i b}^{b}\right)_{z}}\end{array}\right]=\frac{1}{2 l}\left[\begin{array} &{0} & {1} & {-1} & {0} & {0} & {0} & {0} & {0} & {1} & {0} & {1} & {0} \\ {-1} & {0} & {1} & {0} & {0} & {-1} & {0} & {0} & {0} & {1} & {0} & {0} \\ {0} & {1} & {-1} & {0} & {0} & {1} & {0} & {-1} & {0} & {0} & {0} & {0} & \end{array}\right] \left[\begin{array}{l}{y_{a cc 1}^{b}} \\ {y_{a cc 2}^{b}} \\ {y_{a cc 3}^{b}} \\ {y_{a cc 4}^{b}} \\ {y_{a cc5}^{b}} \\ {y_{a cc6}^{b}} \\ {y_{a c c7}^{b}} \\ {y_{a c c8}^{b}} \\ {y_{a c c9}^{b}} \\ {y_{a c c10}^{b}} \\ {y_{a c c11}^{b}} \\ {y_{a c c12}^{b}} \end{array}\right]\)       (19)

where \((\omega_{ib}^b)_x, (\omega_{ib}^b)_y\) and \((\omega_{ib}^b)_z\) are x, y, and z axis component of \(\omega _{ib}^b\)respectively. Inserting Eqs. (11-17) into Eq. (10), Eqs. (20-25) are obtained.

\(\left(\mathbf{\omega}_{i b}^{b}\right)_{x}\left(\mathbf{\omega}_{i b}^{b}\right)_{y}=\frac{1}{2 l}\left(y_{a c c 5}^{b}-y_{a c c 2}^{b}+y_{a c c 7}^{b}-y_{a c c 1}^{b}\right)\)       (20)

\(\left(\mathbf{\omega}_{i b}^{b}\right)_{x}\left(\mathbf{\omega}_{i b}^{b}\right)_{z}=\frac{1}{2 l}\left(y_{a c c 6}^{b}-y_{a c c 3}^{b}+y_{a c c 10}^{b}-y_{a c c 1}^{b}\right)\)       (21)

\(\left(\mathbf{\omega}_{i b}^{b}\right)_{y}\left(\mathbf{\omega}_{i b}^{b}\right)_{z}=\frac{1}{2 l}\left(y_{a c c 9}^{b}-y_{a c c 3}^{b}+y_{a c c 11}^{b}-y_{a c c 2}^{b}\right)\)       (22)

\(\left(\left(\mathbf{\omega}_{i b}^{b}\right)_{x}\right)^{2}=\frac{1}{2 l}\left(y_{a c c 4}^{b}-y_{a c c 1}^{b}-y_{a c c 8}^{b}+y_{a c c 2}^{b}-y_{a c c 12}^{b}+y_{a c c 3}^{b}\right)\)       (23)

\(\left(\left(\mathbf{\omega}_{i b}^{b}\right)_{y}\right)^{2}=\frac{1}{2 l}\left(y_{a c c 8}^{b}-y_{a c c 2}^{b}-y_{a c c 4}^{b}+y_{a c c 1}^{b}-y_{a c c 12}^{b}+y_{a c c 3}^{b}\right)\)       (24)

\(\left(\left(\mathbf{\omega}_{i b}^{b}\right)_{z}\right)^{2}=\frac{1}{2 l}\left(y_{a c c 12}^{b}-y_{a c c 3}^{b}-y_{a c c 4}^{b}+y_{a c c 1}^{b}-y_{a c c 8}^{b}+y_{a c c 2}^{b}\right)\)       (25)

2.2.1 Extended Kalman filter without accelerometer random bias error model

Let the state for the Kalman filter be given in Eq. (26).

\(\mathbf{x}=\left[x_{1} x_{2} x_{3}\right]^{\mathrm{T}}=\left[\left(\omega_{\mathrm{ib}}^{b}\right)_{x} \quad \left(\omega_{\mathrm{ib}}^{b}\right)_{y}\quad \left(\omega_{\mathrm{i} b}^{b}\right)_{z}\right]^{\mathrm{T}}\)       (26)

If the probability distribution of the noise in the accelerometer output \( y_{accj}\)(j = 1, 2, …, 12) is white Gaussian, the process model Eq. (27) is obtained from Eq. (19).

\(\left[\begin{array}{c}{\left(\mathbf{\omega{'}}_{i b}^{b}\right)_{x}} \\ {\left(\mathbf{\omega{'}}_{i b}^{b}\right)_{y}} \\ {\left(\mathbf{\omega{'}}_{i b}^{b}\right)_{z}}\end{array}\right]=\frac{1}{2 l}\left[\begin{array} &{0} & {1} & {-1} & {0} & {0} & {0} & {0} & {0} & {1} & {0} & {1} & {0} \\ {-1} & {0} & {1} & {0} & {0} & {-1} & {0} & {0} & {0} & {1} & {0} & {0} \\ {0} & {1} & {-1} & {0} & {0} & {1} & {0} & {-1} & {0} & {0} & {0} & {0} & \end{array}\right] \left[\begin{array}{l}{y_{a cc 1}^{b}} \\ {y_{a cc 2}^{b}} \\ {y_{a cc 3}^{b}} \\ {y_{a cc 4}^{b}} \\ {y_{a cc5}^{b}} \\ {y_{a cc6}^{b}} \\ {y_{a c c7}^{b}} \\ {y_{a c c8}^{b}} \\ {y_{a c c9}^{b}} \\ {y_{a c c10}^{b}} \\ {y_{a c c11}^{b}} \\ {y_{a c c12}^{b}} \end{array}\right] + \frac{1}{2 l}\left[\begin{array}{c}{w_{a c c 2}^{\prime}-w_{a c c 3}^{\prime}+w_{a c c 9}^{\prime}+w_{a c c 12}^{\prime}} \\ {-w_{a c c 1}^{\prime}+w_{a c c 3}^{\prime}-w_{a c c 6}^{\prime}+w_{a c c 10}^{\prime}} \\ {w_{a c c 1}^{\prime}-w_{a c c 2}^{\prime}+w_{a c c 5}^{\prime}-w_{a c c 7}^{\prime}}\end{array}\right]\)       (27)

where \(w'_{accj}\)(j = 1, 2, …, 12) denotes white Gaussian noise of the j-th accelerometer. Relations between angular velocity and accelerometer output can be written in Eqs. (28-33) from Eqs. (20-25).

\(\begin{aligned}\left(\mathbf{\omega}_{i b}^{b}\right)_{x}\left(\mathbf{\omega}_{i b}^{b}\right)_{y}=& \frac{1}{2 l}\left(y_{a c c5}^{b}-y_{a c c 2}^{b}+y_{a cc 7}^{b}-y_{a c c 1}^{b}\right) \\ &+\frac{1}{2 l}\left(w_{5}^{\prime}-w_{2}^{\prime}+w_{7}^{\prime}-w_{1}^{\prime}\right) \end{aligned}\)       (28)

\(\begin{aligned}\left(\mathbf{\omega}_{i b}^{b}\right)_{x}\left(\mathbf{\omega}_{i b}^{b}\right)_{z}=& \frac{1}{2 l}\left(y_{a c c6}^{b}-y_{a c c 3}^{b}+y_{a cc 10}^{b}-y_{a c c 1}^{b}\right) \\ &+\frac{1}{2 l}\left(w_{6}^{\prime}-w_{3}^{\prime}+w_{10}^{\prime}-w_{1}^{\prime}\right) \end{aligned}\)       (29)

\(\begin{aligned}\left(\mathbf{\omega}_{i b}^{b}\right)_{y}\left(\mathbf{\omega}_{i b}^{b}\right)_{z}=& \frac{1}{2 l}\left(y_{a c c9}^{b}-y_{a c c 3}^{b}+y_{a cc117}^{b}-y_{a c c 2}^{b}\right) \\ &+\frac{1}{2 l}\left(w_{9}^{\prime}-w_{3}^{\prime}+w_{11}^{\prime}-w_{2}^{\prime}\right) \end{aligned}\)       (30)

\(\begin{aligned}\left(\left(\mathbf{\omega}_{i b}^{b}\right)_{x}\right)^{2}=& \frac{1}{2 l}\left(y_{a c c 4}^{b}-y_{a c c 1}^{b}-y_{a c c 8}^{b}+y_{a c c 2}^{b}-y_{a c c 12}^{b}+y_{a c c 3}^{b}\right) \\ &+\frac{1}{2 l}\left(w_{4}^{\prime}-w_{1}^{\prime}-w_{8}^{\prime}+w_{2}^{\prime}-w_{12}^{\prime}+w_{3}^{\prime}\right) \end{aligned}\)       (31)

\(\begin{aligned}\left(\left(\mathbf{\omega}_{i b}^{b}\right)_{y}\right)^{2}=& \frac{1}{2 l}\left(y_{a c c 8}^{b}-y_{a c c 2}^{b}-y_{a c c 4}^{b}+y_{a c c 1}^{b}-y_{a c c 12}^{b}+y_{a c c 3}^{b}\right) \\ &+\frac{1}{2 l}\left(w_{8}^{\prime}-w_{2}^{\prime}-w_{4}^{\prime}+w_{1}^{\prime}-w_{12}^{\prime}+w_{3}^{\prime}\right) \end{aligned}\)       (32)

\(\begin{aligned}\left(\left(\mathbf{\omega}_{i b}^{b}\right)_{z}\right)^{2}=& \frac{1}{2 l}\left(y_{a c 12}^{b}-y_{a c c 3}^{b}-y_{a c c 4}^{b}+y_{a c c1}^{b}-y_{a c c 8}^{b}+y_{a c c 2}^{b}\right) \\ &+\frac{1}{2 l}\left(w_{12}^{\prime}-w_{3}^{\prime}-w_{4}^{\prime}+w_{1}^{\prime}-w_{8}^{\prime}+w_{2}^{\prime}\right) \end{aligned}\)       (33)

The measurement model Eq. (34) is obtained from Eqs. (28-33).

\(\mathbf{z}=\boldsymbol{h}(\mathbf{x})+\mathbf{v}=\left[\begin{array}{llllll}{x_{1} x_{2}} & {x_{1} x_{3}} & {x_{2} x_{3}} & {x_{1}^{2}} & {x_{2}^{2}} & {x_{3}^{2}}\end{array}\right]^{\mathrm{T}}+\mathbf{v} \)       (34)

where v is given in Eq. (35)

\(\mathbf{v}=\left[\begin{array}{c}{w_{\operatorname{acc5}}^{\prime}-w_{\operatorname{acc} 2}^{\prime}+w_{\operatorname{acc} 7}^{\prime}-w_{\operatorname{acc} 1}^{\prime}} \\ {w_{\operatorname{acc6}}^{\prime}-w_{\operatorname{acc} 3}^{\prime}+w_{\operatorname{acc10}}^{\prime}-w_{\operatorname{acc} 1}^{\prime}} \\ {w_{\operatorname{acc9}}^{\prime}-w_{\operatorname{acc} 3}^{\prime}+w_{\operatorname{acc11}}^{\prime}-w_{\operatorname{acc} 2}^{\prime}} \\ {w_{\operatorname{acc4}}^{\prime}-w_{\operatorname{acc1}}^{\prime}-w_{\operatorname{acc8}}^{\prime}+w_{\operatorname{acc2}}^{\prime}-w_{\operatorname{acc} 12}^{\prime}+w_{\operatorname{acc3}}^{\prime}} \\ {w_{\operatorname{acc} 8}^{\prime}-w_{\operatorname{acc} 2}^{\prime}-w_{\operatorname{acc4}}^{\prime}+w_{\operatorname{acc} 1}^{\prime}-w_{\operatorname{acc12}}^{\prime}-w_{\operatorname{acc3}}^{\prime}} \\ {w_{\operatorname{acc} 12}^{\prime}-w_{\operatorname{acc3}}^{\prime}-w_{\operatorname{acc4}^{\prime}}^{\prime}+w_{\operatorname{acc} 1}^{\prime}-w_{\operatorname{acc} 8}^{\prime}+w_{\operatorname{acc} 2}^{\prime}}\end{array}\right]\)       (35)

The process model Eq. (27) can be discretized into Eq. (36) (Brown & Hwang 1997).

\(\mathbf{x}_{k+1}=\mathbf{\Phi}_{k} \mathbf{x}_{k}+\mathbf{\Gamma}_{k} \mathbf{u}_{k}+\mathbf{w}_{k}\)       (36)

where \(\mathbf{x_k, Φ_k, u_k, Γ_k}\) and wk are given in Eqs. (37-41), respectively.

\(\mathbf{x}_{k}=\left[\begin{array}{cc}{\left(\mathbf{\omega}_{i b}^{b}\right)_{x, k}} & {\left(\mathbf{\omega}_{i b}^{b}\right)_{y, k}} & {\left.\left(\mathbf{\omega}_{i b}^{b}\right)_{z, k}\right]^{\mathbf{T}}}\end{array}\right.\)       (37)

\(\mathbf{\Phi}_{k}=\left[\begin{array}{lll}{1} & {0} & {0} \\ {0} & {1} & {0} \\ {0} & {0} & {1}\end{array}\right]\)       (38)

\(\mathbf{u}_{k}=\left[\begin{array}{llll}{y_{a c c 1, k}^{b}} & {y_{a c c 2, k}^{b}} & {\mathrm{L}} & {y_{a c c 12, k}^{b}}\end{array}\right]^{\mathrm{T}}\)       (39)

\(\mathbf{\Gamma}_{k}=\frac{\Delta t}{2 l}\left[\begin{array}{cccccccccccc}{0} & {1} & {-1} & {0} & {0} & {0} & {0} & {0} & {1} & {0} & {-1} & {0} \\ {-1} & {0} & {1} & {0} & {0} & {-1} & {0} & {0} & {0} & {1} & {0} & {0} \\ {1} & {-1} & {0} & {0} & {1} & {0} & {-1} & {0} & {0} & {0} & {0} & {0}\end{array}\right]\)       (40)

\(\mathbf{w}_{k}=\left[\begin{array}{c}{w_{a cc 2, k}^{\prime}-w_{a cc 3, k}^{\prime}+w_{a c c 9, k}^{\prime}+w_{a cc 12, k}^{\prime}} \\ {-w_{a cc 1, k}^{\prime}+w_{a c c3 , k}^{\prime}-w_{a c c6, k}^{\prime}+w_{a cc10, k}^{\prime}} \\ {w_{a cc 1, k}^{\prime}-w_{a c c 2, k}^{\prime}+w_{a cc 5, k}^{\prime}-w_{a c c7, k}^{\prime}}\end{array}\right]\)       (41)

The measurement \(\mathbf{z}_k\) can be written in Eq. (42) from Eq. (34).

\(\begin{aligned} \mathbf{z}_{k}=\boldsymbol{h}\left(\mathbf{x}_{k}\right)+\mathbf{v}_{k}=\left[\begin{array}{lllll}{x_{1, k} x_{2, k}} & {x_{1, k} x_{3, k}} & {x_{2, k} x_{3, k}} & {x_{1, k}^{2}} & {x_{2, k}^{2}} & {x_{3, k}^{2}}\end{array}\right]^{\mathrm{T}} \\+\left[\begin{array}{llll}{v_{1, k}} & {v_{2, k}} & {v_{3, k}} & {v_{4, k}} & {v_{5, k}} & {v_{6, k}}\end{array}\right]^{\mathrm{T}} \end{aligned}\)       (42)

where \(\mathbf {v}_k\) is given in Eq.(43) 

\(\mathbf{v}_{k}=\left[\begin{array}{c}{w_{a c c5, k}^{\prime}-w_{a c c2, k}^{\prime}+w_{a c c7, k}^{\prime}-w_{a cc 1, k}^{\prime}} \\ {w_{a c c 6, k}^{\prime}-w_{a cc 3, k}^{\prime}+w_{a c c 10, k}^{\prime}-w_{a c c1, k}^{\prime}} \\ {w_{a c c 9, k}^{\prime}-w_{a c c3, k}^{\prime}+w_{a cc11, k}^{\prime}-w_{a c c2, k}^{\prime}} \\ {w_{a c c4, k}^{\prime}-w_{a c c1, k}^{\prime}-w_{a c c8, k}^{\prime}+w_{a cc 2, k}^{\prime}-w_{a cc 12, k}^{\prime}+w_{a cc 3, k}^{\prime}} \\ {w_{a cc 8, k}^{\prime}-w_{a cc 2, k}^{\prime}-w_{a cc4, k}^{\prime}+w_{a cc 1, k}^{\prime}-w_{a cc 12, k}^{\prime}-w_{a c 2, k}^{\prime}} \\ {w_{a cc 12, k}^{\prime}-w_{a cc3, k}^{\prime}-w_{a cc4, k}^{\prime}+w_{a c c1, k}^{\prime}-w_{a c c8, k}^{\prime}+w_{a cc2, k}^{\prime}}\end{array}\right]\)       (43)

2.2.2 Extended Kalman filter with accelerometer random bias error model

Let the state for the Kalman filter be given in Eq. (44) when the accelerometer random bias errors are included in the state vector.

\(\mathbf{x} = [x_1 \quad x_2 \quad \mathsf{L} \quad x_{15} ]^ \mathbf{T} \\ \ \ = [ (\omega_{ib}^b)_x \quad (\omega_{ib}^b)_y \quad (\omega_{ib}^b)_z \quad b_1 \quad b_2 \quad \mathsf{L} \quad B_{12} ]^ \mathbf{T}\)             (44)

where \(b_j\)(j =1, 2,…, 12) denotes the random bias error of the j-th accelerometer. If the noise of the accelerometer is represented as a random bias error and white Gaussian noise, the process model Eq. (45) is obtained.

\(\left[\begin{matrix}({\dot{\mathbf{\omega}}}_{ib}^b)_x\\({\dot{\mathbf{\omega}}}_{ib}^b)_y\\({\dot{\mathbf{\omega}}}_{ib}^b)_z\\{\dot{b}}_1\\{\dot{b}}_2\\{\dot{b}}_3\\{\dot{b}}_4\\{\dot{b}}_5\\{\dot{b}}_6\\{\dot{b}}_7\\{\dot{b}}_8\\{\dot{b}}_9\\{\dot{b}}_{10}\\{\dot{b}}_{11}\\{\dot{b}}_{12}\\\end{matrix}\right]=\left[\begin{matrix}0&0&0&0&1&-1&0&0&0&0&0&1&0&1&0\\0&0&0&-1&0&1&0&0&-1&0&0&0&1&0&0\\0&0&0&1&-1&0&0&1&0&-1&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0&0&0&0\\\end{matrix}\right]\left[\begin{matrix}(\mathbf{\omega}_{ib}^b)_x\\(\mathbf{\omega}_{ib}^b)_y\\(\mathbf{\omega}_{ib}^b)_z\\b_1\\b_2\\b_3\\b_4\\b_5\\b_6\\b_7\\b_8\\b_9\\b_{10}\\b_{11}\\b_{12}\\\end{matrix}\right]+ \)
\( \frac{1}{2l}\left[\begin{matrix}0&1&-1&0&0&0&0&0&1&0&1&0\\-1&0&1&0&0&-1&0&0&0&1&0&0\\1&-1&0&0&1&0&-1&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\0&0&0&0&0&0&0&0&0&0&0&0\\\end{matrix}\right]\left[\begin{matrix}y_{acc1}^b\\y_{acc2}^b\\y_{acc3}^b\\y_{acc4}^b\\y_{acc5}^b\\y_{acc6}^b\\y_{acc7}^b\\y_{acc8}^b\\y_{acc9}^b\\y_{acc10}^b\\y_{acc11}^b\\y_{acc12}^b\\\end{matrix}\right]+\frac{1}{2l}\left[\begin{matrix}w_{acc2}^\prime-w_{acc3}^\prime+w_{acc9}^\prime+w_{acc12}^\prime\\-w_{acc1}^\prime+w_{acc3}^\prime-w_{acc6}^\prime+w_{acc10}^\prime\\w_{acc1}^\prime-w_{acc2}^\prime+w_{acc5}^\prime-w_{acc7}^\prime\\\begin{matrix}0\\0\\0\\0\\0\\0\\0\\0\\0\\0\\0\\0\\\end{matrix}\\\end{matrix}\right] \)     (45)

Relations between angular velocities and accelerometer outputs can be written in Eqs. (46-51) from Eqs. (20-25).

\(\begin{aligned} \left(\mathbf{\omega}_{i b}^{b}\right)_{x}\left(\mathbf{\omega}_{i b}^{b}\right)_{y}=& \frac{1}{2 l}\left(y_{a c c s}^{b}-y_{a c 2}^{b}+y_{a c 7}^{b}-y_{a c b}^{b}\right) \\ &+\frac{1}{2 l}\left(b_{s}-b_{2}+b_{7}-b_{1}\right)+\frac{1}{2 l}\left(w_{s}^{\prime}-w_{2}^{\prime}+w_{7}^{\prime}-w_{1}^{\prime}\right) \end{aligned} \)        (46)

\(\begin{aligned} \left(\mathbf{\omega}_{i b}^{b}\right)_{x}\left(\mathbf{\omega}_{i b}^{b}\right)_{z}=& \frac{1}{2 l}\left(y_{a c c 6}^{b}-y_{a c s 3}^{b}+y_{a c c l 0}^{b}-y_{a c c 1}^{b}\right) \\ &+\frac{1}{2 l}\left(b_{6}-b_{3}+b_{10}-b_{1}\right)+\frac{1}{2 l}\left(w_{6}^{\prime}-w_{3}^{\prime}+w_{10}^{\prime}-w_{1}^{\prime}\right) \end{aligned}\)       (47)

\(\begin{aligned} \left(\mathbf{\omega}_{i b}^{b}\right)_{y}\left(\mathbf{\omega}_{i b}^{b}\right)_{z}=& \frac{1}{2 l}\left(y_{a c c 9}^{b}-y_{a c s 3}^{b}+y_{a c a 1}^{b}-y_{a c 2}^{b}\right) \\ &+\frac{1}{2 l}\left(b_{9}-b_{3}+b_{11}-b_{2}\right)+\frac{1}{2 l}\left(w_{9}^{\prime}-w_{3}^{\prime}+w_{11}^{\prime}-w_{2}^{\prime}\right) \end{aligned}\)       (48)

\(\begin{aligned} \left(\left(\boldsymbol{\omega}_{i b}^{b}\right)_{y}\right)^{2}=& \frac{1}{2 l}\left(y_{a c c t}^{b}-y_{a c t}^{b}-y_{a c c 8}^{b}+y_{a c c 2}^{b}-y_{a c c 12}^{b}+y_{a c s}^{b}\right) \\ &+\frac{1}{2 l}\left(b_{4}-b_{1}-b_{8}+b_{2}-b_{12}+b_{3}\right) \\ &+\frac{1}{2 l}\left(w_{4}^{\prime}-w_{1}^{\prime}-w_{8}^{\prime}+w_{2}^{\prime}-w_{12}^{\prime}+w_{3}^{\prime}\right) \end{aligned}\)       (49)

\(\begin{aligned} \left(\left(\boldsymbol{\omega}_{i b}^{b}\right)_{y}\right)^{2}=& \frac{1}{2 l}\left(y_{a c s}^{b}-y_{a c c 2}^{b}-y_{a c c 4}^{b}+y_{a c c 1}^{b}-y_{a c c 12}^{b}+y_{a c c 3}^{b}\right) \\ &+\frac{1}{2 l}\left(b_{g}-b_{2}-b_{4}+b_{1}-b_{12}+b_{3}\right) \\ &+\frac{1}{2 l}\left(w_{s}^{\prime}-w_{2}^{\prime}-w_{4}^{\prime}+w_{1}^{\prime}-w_{12}^{\prime}+w_{3}^{\prime}\right) \end{aligned}\)       (50)

\(\begin{aligned} \left(\left(\omega_{i b}^{b}\right)_{z}\right)^{2}=& \frac{1}{2 l}\left(y_{a c r 12}^{b}-y_{a c c 3}^{b}-y_{a c c}^{b}+y_{a c a}^{b}-y_{a c s}^{b}+y_{a c c 2}^{b}\right) \\ &+\frac{1}{2 l}\left(b_{12}-b_{3}-b_{4}+b_{1}-b_{8}+b_{2}\right) \\ &+\frac{1}{2 l}\left(w_{12}^{\prime}-w_{3}^{\prime}-w_{4}^{\prime}+w_{1}^{\prime}-w_{8}^{\prime}+w_{2}^{\prime}\right) \end{aligned}\)       (51)

The measurement model Eq. (52) is obtained from Eqs. (46-51).

\(\mathbf{z}=h(\mathbf{x})+\mathbf{v}=\left[\begin{array}{c} {x_{1} x_{2}+\frac{1}{2 l}\left(-x_{8}+x_{5}-x_{10}+x_{4}\right)} \\ {x_{1} x_{3}+\frac{1}{2 l}\left(-x_{9}+x_{6}-x_{13}+x_{4}\right)} \\ {x_{2} x_{3}+\frac{1}{2 l}\left(-x_{12}+x_{6}-x_{14}+x_{5}\right)} \\ {x_{1}^{2}+\frac{1}{2 l}\left(-x_{7}+x_{4}+x_{11}-x_{5}+x_{15}-x_{6}\right)} \\ {x_{2}^{2}+\frac{1}{2 l}\left(-x_{11}+x_{5}+x_{7}-x_{4}+x_{15}-x_{6}\right)} \\ {x_{3}^{2}+\frac{1}{2 l}\left(-x_{15}+x_{6}+x_{7}-x_{4}+x_{11}-x_{5}\right)} \end{array}\right]\)       (52)

where v is given in Eq. (53)

\(\mathbf{v}=\left[\begin{array}{c} {w_{\text {accs }}^{\prime}-w_{\text {acc } 2}^{\prime}+w_{\text {acc } 7}^{\prime}-w_{\text {act }}^{\prime}} \\ {w_{\text {accs }}^{\prime}-w_{\text {occa }}^{\prime}+w_{\text {acto }}^{\prime}-w_{\text {accl }}^{\prime}} \\ {w_{\text {ace }}^{\prime}-w_{\text {daca }}^{\prime}+w_{\text {actio }}^{\prime}-w_{\text {acca }}^{\prime}} \\ {w_{\text {accl }}^{\prime}-w_{\text {dacr }}^{\prime}+w_{\text {accli }}^{\prime}-w_{\text {acca }}^{\prime}+w_{\text {acz} 3}^{\prime}} \\ {w_{\text {ace}}^{\prime}-w_{\text {acc} 2}^{\prime}-w_{\text {acc}^{\prime}}^{\prime}+w_{\text {acc}}^{\prime}-w_{\text {accil}}^{\prime}-w_{\text {acc} 3}^{\prime}} \\ {w_{\text {accl} 2}^{\prime}-w_{\text {acc} 3}^{\prime}-w_{\text {acct}}^{\prime}+w_{\text {accl}}^{\prime}-w_{\text {acc}^{\prime}}^{\prime}+w_{\text {acc} 2}^{\prime}} \end{array}\right]\)       (53)

Process model (45) is discretized into (54) (Brown & Hwang 1997).

\(\mathbf{x}_{k+1}=\mathbf{\Phi}_{k} \mathbf{x}_{k}+\mathbf{\Gamma}_{k} \mathbf{u}_{k}+\mathbf{w}_{k}\)       (54)

where \(\mathbf{x}_{k}, \mathbf{\Phi}_{k}, \mathbf{u}_{k}, \mathbf{\Gamma}_{k}\) and w_k are given in Eqs. (55-59), respectively.

\(\begin{aligned} \mathbf{x}_{k} &=\left[\begin{array}{llll} {x_{1, k}} & {x_{2, k}} & {\mathrm{L}} & {x_{15, k}} \end{array}\right]^{\mathrm{T}} \\ &=\left[\begin{array}{ll} {\left(\omega_{i b}^{b}\right)_{x, k}} & {\left(\omega_{i b}^{b}\right)_{y, k}} & {\left(\omega_{i b}^{b}\right)_{z, k}} & b_{1, k} \quad b_{2, k} \quad \mathrm{L} \quad b_{12, k} \end{array}\right. \end{aligned}\)       (55)

\(\mathbf{u}_{k}=\left[\begin{array}{llll} {y_{\text {act }, k}^{b}} & {y_{\text {arc } 2, k}^{b}} & {\text { L }} & {y_{\text {accila. }}^{b}} \end{array}\right]^{\mathbf{T}}\)       (57)

\(\Gamma_{k}=\left[\begin{array}{cccccccccccc} 0 & \frac{\Delta t}{2 l} & -\frac{\Delta t}{2 l} & 0 & 0 & 0 & 0 & 0 & \frac{\Delta t}{2 l} & 0 & \frac{\Delta t}{2 l} & 0 \\ -\frac{\Delta t}{2 l} & 0 & \frac{\Delta t}{2 l} & 0 & 0 & -\frac{\Delta t}{2 l} & 0 & 0 & 0 & \frac{\Delta t}{2 l} & 0 & 0 \\ \frac{\Delta t}{2 l} & -\frac{\Delta t}{2 l} & 0 & 0 & \frac{\Delta t}{2 l} & 0 & -\frac{\Delta t}{2 l} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]\)       (58)

\(\mathbf{w}_{k}=\frac{1}{2 l}\left[\begin{array}{c} {w_{\operatorname{accse}, k}^{\prime}-w_{\operatorname{acc} 3, k}^{\prime}+w_{\operatorname{acos}, k}^{\prime}+w_{\operatorname{arcti} 2, k}^{\prime}} \\ {-w_{\operatorname{arct}, k}^{\prime}+w_{\operatorname{acc} 3, k}^{\prime}-w_{\operatorname{acc} 6, k}^{\prime}+w_{\operatorname{arcti} 0, k}^{\prime}} \\ {w_{\operatorname{arct}, k}^{\prime}-w_{\operatorname{accsin}}^{\prime}+w_{\operatorname{accs}, k}^{\prime}-w_{\operatorname{acc} 7 x}^{\prime}} \\ {0_{1 \times 12}} \end{array}\right]\)       (59)

The measurement can be written in Eq. (60) from Eq. (52).

\(\mathbf{z}_{k}=h\left(\mathbf{x}_{k}\right)+\mathbf{v}_{k}=\left[\begin{array}{c} {x_{1, k} x_{2, k}+\frac{1}{2 l}\left(-x_{s, k}+x_{s, k}-x_{10, k}+x_{4, k}\right)} \\ {x_{1, k} x_{3, k}+\frac{1}{2 l}\left(-x_{s, k}+x_{6, k}-x_{13, k}+x_{4, k}\right)} \\ {x_{2, k} x_{3, k}+\frac{1}{2 l}\left(-x_{12, k}+x_{6, k}-x_{14, k}+x_{s, k}\right)} \\ {x_{1, k}^{2}+\frac{1}{2 l}\left(-x_{7, k}+x_{4, k}+x_{1, k}-x_{5, k}+x_{15, k}-x_{6, k}\right)} \\ {x_{2, k}^{2}+\frac{1}{2 l}\left(-x_{11, k}+x_{5, k}+x_{7, k}-x_{4, k}+x_{4, k}+x_{15, k}-x_{6, k}\right)} \\ {x_{3, k}^{2}+\frac{1}{2 l}\left(-x_{15, k}+x_{6, k}+x_{7, k}-x_{4, k}+x_{1, k}-x_{5, k}\right)} \end{array}\right] +\mathbf{v}_k\)       (60)

where v_k is given in Eq. (61).

\(\mathbf{v}_{k}=\left[\begin{array}{c} {w_{a c c 5, k}^{\prime}-w_{a c c 2, k}^{\prime}+w_{a c \tau, k}^{\prime}-w_{a c c 1, k}^{\prime}} \\ {w_{a c c 6, k}^{\prime}-w_{a c c 3, k}^{\prime}+w_{a c c 7, k}^{\prime}-w_{a c 1, k}^{\prime}} \\ {w_{a c c 9, k}^{\prime}-w_{a c c 3, k}^{\prime}+w_{a c c 10, k}^{\prime}-w_{a c c 1, k}^{\prime}} \\ {w_{a c c 9, k}^{\prime}-w_{a c c 3, k}^{\prime}+w_{a c c 11, k}^{\prime}-w_{a c c 2, k}^{\prime}} \\ {w_{a c c-4, k}^{\prime}-w_{a c c 1, k}^{\prime}-w_{a c c 8, k}^{\prime}+w_{a c c 2, k}^{\prime}-w_{a c c 12, k}^{\prime}+w_{a c c 3, k}^{\prime}} \\ {w_{a c s, k}^{\prime}-w_{a c c 2, k}^{\prime}-w_{a c c-4, k}^{\prime}+w_{a c c 1, k}^{\prime}-w_{a c c 12, k}^{\prime}-w_{a c c 3, k}^{\prime}} \\ {w_{a c 12, k}^{\prime}-w_{a c c 3, k}^{\prime}-w_{a c c 4, k}^{\prime}+w_{a c a, k}^{\prime}-w_{a c c 8, k}^{\prime}+w_{a c c 2, k}^{\prime}} \end{array}\right]\)       (61)

Fig. 3. Software platform for performance evaluation

3. Performance Evaluation

In order to evaluate performance of the proposed GF-INS algorithm, computer simulations were carried out. Angular velocity estimation performance and navigation performance were observed when the vehicle rotated around the X-axis of the body frame under the placement of accelerometers given in Fig. 2 in Section 2.

Fig. 3 shows the software platform for the performance evaluation. The platform is composed of sensor output generator and GF-INS algorithm. The reference trajectory generator in the sensor output generator generates three dimensional reference trajectory. The true data generator generates true acceleration and angular velocity from the reference trajectory. The accelerometer output generator generates outputs of four accelerometer triads that are arranged as shown in Fig. 2 using output of inertial data generator and Eq. (3). The accelerometer errors are added to the accelerometer outputs. The GF-INS algorithm consists of the angular velocity estimator and SDINS algorithm. The angular velocity estimator estimates angular velocity and acceleration of the vehicle from accelerometer output. SDINS algorithm calculates position, velocity, and attitude from the estimated angular velocity and acceleration. Angular velocity output of the angular velocity estimator and reference angular velocity were compared for performance evaluation. Navigation results of the SDINS algorithm were also compared with reference trajectories.

Parameters for generating reference trajectory are shown in Table 1 and generated reference trajectories are given in Figs. 4-6. The vehicle rotates in 3 Hz (1080 deg/s) around the roll axis while it moves parabolically.

Table 1. Parameters for reference trajectory generation.

	Parameters	Value
Initial value	Latitude (deg) Longitude (deg) Altitude (m) Speed (m/s) Roll (deg) Pitch (deg) Heading (deg)	36 127 0 684 0 45 45
Max altitude (m) Roll axis revolution rate (Hz, deg/s) Flight time (s) Sampling rate (Hz)		13983 3, 1080 98.7 1000

 

Fig. 4. Flight path of vehicle.

Fig. 5. Reference trajectory

Fig. 6. Reference roll trajectory for 1 second.

Fig. 7. Angular velocity and acceleration of reference trajectory.

Fig. 7 shows angular velocity and acceleration outputs of the true data generator for the reference trajectory. The X-axis angular velocity is 1080 deg/s. The angular velocities of the Y-axis and the Z-axis vibrates in 3 Hz due to the rotation and the maximum value is 1.17 deg/s where the pitch angle is 0 deg.

Table 2. Specification of accelerometers.

Performance parameter (1 σ)	Automotive grade (IAM-20381, invenSense)	Tactical grade (RBA500, Honeywell)	Navigation grade (QA2000, Honeywell)
Noise density (μg/√Hz) Bias repeatability (mg)	135 50	30 2.5	1.3 0.025

 

Table 2 shows specification of the accelerometers used in the performance evaluation. Noise density and bias repeatability were considered for automotive grade, tactical grade, and navigation grade accelerometers. In the simulations, the scale factor error was not considered.

For the performance evaluation, 25 Monte Carlo simulation runs were carried out for the GF-INS with the accelerometers in Table 2 (Rubinstein 1981). The angular velocity estimation and navigation performance of the integration method, KF without accelerometer bias model, and KF with accelerometer bias model were compared. In the integration method, angular velocity was obtained by integrating Eq. (9).

Fig. 8. Outputs of accelerometer triad A and B of GF-INS using tactical grade accelerometer.

Fig. 9. Outputs of accelerometer triad C and D of GF-INS using tactical grade accelerometer.

Figs 8 and 9 show outputs of the 4 accelerometer triads arranged in Fig. 2 when tactical grade accelerometers are used. The distance between accelerometer triads is l = 0.1 m. It can be observed in Fig. 8 that the outputs of the triad A located in the center of gravity of the vehicle is identical with those in Fig. 7. Since the pitch rate is less than 1.0 deg/s and there is no variation of heading angle by the parabolic motion in Fig. 5, outputs of accelerometers of the GF-INS are gravitational acceleration and centripetal acceleration by the rotation of roll axis. Since the accelerometer triad B is located in the roll axis of the vehicle, the output is similar to that of accelerometer triad A. The accelerometer triads C and D can measure the centripetal acceleration since they are located in the distance l from the rotation axis in Fig. 2. Therefore, the centripetal accelerations in Fig. 9 are measured by the Y axis accelerometer of the accelerometer triad D and the Z axis accelerometer of the accelerometer triad D. In this case, the centripetal acceleration is 35.4 m/s²\(\left(=l \cdot \omega^{2}=0.1 \times(3 \cdot 2 \pi)^{2}\right)\) ) at t = 0 s.

Fig. 10. Angular velocity RMSE of GF-INS using tactical grade accelerometer.

Fig. 11. Estimated angular velocity error from KF without/with accelerometer bias model of GF-INS using tactical grade accelerometer.

Fig. 12. Estimation error of accelerometer bias from KF with accelerometer bias model of GF-INS using tactical grade accelerometer.

Fig. 13. Mean RSSE of GF-INS output using tactical grade accelerometer.

Fig. 14. Attitude RMSE of GF-INS using tactical grade accelerometer

Figs. 10-14 show simulation results for the GF-INS using tactical grade accelerometer. Fig. 10 shows angular velocity Root-Mean-Square Error (RMSE) for three angular velocity estimation methods. The angular velocity error of the integration method increases as time goes due to the accelerometer bias error. Angular velocity errors of the Kalman filters are constants. Kalman filter with accelerometer bias model estimates the angular velocity more accurately than that without the accelerometer bias model.

Table 3. Estimated angular velocity mean error from KF without/with accelerometer bias model of GF-INS using tactical grade accelerometer.

Estimated angular velocity mean error (deg/s)	KF without bias model	KF with bias model
X axis Y axis Z axis	0.816 1.128 1.129	0.876 1.294 1.293

 

Table 4. Angular velocity and navigation error result of GF-INS using automotive grade accelerometer.

Mean error		Automotive grade (IAM-20381, InvenSense)
		Integration	KF without bias model	KF with bias model
Angular velocity (deg/s, RMS)	X axis Y axis Z axis	14134.440 13051.670 14867.250	10.874 18.344 15.321	5.500 6.417 5.535
Position (m, RSS) Velocity (m/s, RSS) Attitude (deg, RSS)		5324.097 125.641 141.311	3851.264 103.484 102.244	3798.359 103.447 100.527

 

Table 5. Angular velocity and navigation error result of GF-INS using tactical grade accelerometer.

Mean error		Tactical grade (RBA500, Honeywell)
		Integration	KF without bias model	KF with bias model
Angular velocity (deg/s, RMS)	X axis Y axis Z axis	843.097 680.883 668.098	0.481 1.143 0.826	0.253 0.748 0.482
Position (m, RSS) Velocity (m/s, RSS) Attitude (deg, RSS)		3787.188 109.696 114.941	702.240 20.992 21.481	353.655 10.646 10.951

 

Table 6. Angular velocity and navigation error result of GF-INS using navigation grade accelerometer.

Mean error		Navigation grade (QA2000, Honeywell)
		Integration	KF without bias model	KF with bias model
Angular velocity (deg/s, RMS)	X axis Y axis Z axis	7.060 6.524 7.432	0.006 0.014 0.011	0.004 0.012 0.010
Position (m, RSS) Velocity (m/s, RSS) Attitude (deg, RSS)		2360.103 72.124 79.853	17.509 0.899 0.672	16.507 0.877 0.647

 

Fig. 11 and Table 3 shows estimated angular velocity errors from the error covariance matrix of the Kalman filter. As the result in Fig. 10, angular velocity estimation performance of the X-axis, which is rotation axis of the vehicle, is better than others. Fig. 12 shows accelerometer bias estimation error of the Kalman filter with accelerometer bias model. The X-axis accelerometer bias estimation performance in the accelerometer triad A and B, which are located at the center of gravity and rotation axis of the vehicle, is better than others. On the other hand, The Z-axis and Y-axis accelerometer bias estimation performance in the accelerometer C and D are better than others. The Z-axis and Y-axis in the triad C and D are orthogonal to the direction of the centripetal acceleration, respectively.

Figs. 13 and 14 show the mean Root-Sum-Square Error (RSSE) of the GF-INS output and attitude RMSE of the GF-INS for the three angular velocity estimation methods. The GF-INS which uses the Kalman filter with accelerometer bias model gives two times more accurate navigation performance than that uses the Kalman filter without accelerometer bias model.

In Tables 4-6, angular velocity errors and navigation errors are shown for the accelerometers in Table 2. It can be seen from Table 4 that the Kalman filter with accelerometer bias model gives two times more accurate angular velocity estimate of the X-axis and three times more accurate angular velocity estimate of the Y-axis and Z-axis than Kalman filter without accelerometer bias model for the automotive grade accelerometer. However, there is no big difference in the navigation performance since the values of the accelerometer errors are large. It can also be seen from Table 5 that the Kalman filter with accelerometer bias model gives two times more accurate navigation performance than Kalman filter without accelerometer bias model for the tactical grade accelerometer as shown in Figs. 10-13. There are no big differences in the angular velocity estimate performances to the Kalman filter models for the navigation grade accelerometer in Table 6 since the bias error is less than noise. The Kalman filter with the bias model gives better navigation performance than others.

4. CONCLUDING REMARKS AND FURTHER STUDIES

In this paper, in order to improve navigation performance of the gyro-free inertial navigation system, an angular velocity estimation algorithm has been proposed based on an extended Kalman filter with an accelerometer random bias model.

The estimated angular velocity and navigation outputs by the proposed algorithm of a vehicle with 3 rev/s rotation rate were compared with those by other methods which are based on the integration method and an extended filter without an accelerometer random bias model. The angular velocity and navigation output estimations were performed for navigation grade, tactical grade, and automotive grade accelerometers. It has been observed that the proposed method gave better results than other methods.

In the future, effect of the scale factor error of the accelerometer, alignment of GF-INS, optimal placement of accelerometers in GF-IMU and Global Positioning System/GF-INS will be studied.