Angular velocity fusion of the microelectromechanical system inertial measurement unit array based on extended Kalman filter with correlated system noises

Abstract

The paper proposes an angular velocity fusion method of the microelectromechanical system inertial measurement unit array based on the extended Kalman filter with correlated system noises. In the proposed method, an adaptive model of the angular velocity is built according to the motion characteristics of the vehicles and it is regarded as the state equation to estimate the angular velocity. The signal model of gyroscopes and accelerometers in the microelectromechanical system inertial measurement unit array is used as the measurement equation to fuse and estimate the angular velocity. Due to the correlation of the state and measurement noises in the presented fusion model, the traditional extended Kalman filter equations are optimized, so as to accurately and reliably estimate the angular velocity. By simulating angular rates in different motion modes, such as constant and change-in-time angular rates, it is verified that the proposed method can reliably estimate angular rates, even when the angular rate has been out of the microelectromechanical system gyroscope measurement range. And results show that, compared with the traditional angular rate fusion method of microelectromechanical system inertial measurement unit array, it can estimate angular rates more accurately. Moreover, in the kinematic vehicle experiments, the performance advantage of the proposed method is also verified and the angular rate estimation accuracy can be increased by about 1.5 times compared to the traditional method.

Introduction

The angular velocity of the vehicles, such as the multi-rotor unmanned aerial vehicle (UAV), the car or other vehicles,^1–5 is usually measured by microelectromechanical system (MEMS) gyroscopes equipped in the navigation system. Thus, the angular velocity can be measured more accurately by improving the measurement accuracy of MEMS gyroscopes. With respect to the improvement of the measurement accuracy, a MEMS gyroscope array is designed by capitalizing on the decreasing price, size, and power consumption. In such a design, multiple gyroscope measurements are fused through different filter frameworks and signal models,^6–9 and a non-negligible reduction in the measurement errors can be obtained from the redundant measurements, see, for example, the Allan variance analysis. ¹⁰ However, limited by the production process, the measurement accuracy of MEMS gyroscopes cannot be improved and the measurement range cannot be widened simultaneously, even in the array. Since the angular velocity of all points on a rigid object is the same, no additional information, except that from redundant measurements, is obtained from gyroscopes spatially distributed in the array. Nevertheless, if multiple MEMS accelerometers are integrated in an array, the angular rate measurements will be widened effectively. This is because the angular velocity with arbitrary magnitudes and angular acceleration can be obtained from the spatially distributed accelerometer measurements.^11–13 Although the accelerometer array can widen the measurement range of the angular velocity, it cannot accurately estimate the angular velocity for the centrifugal force quadratically depends on the angular speed. With respect to such a problem, a variety of approaches are presented in previous works,^14–18 but these methods inevitably increase the size of such a system and increase the complexity of the hardware design.^19–22 Referring to design ideas of the above sensor array,^23,24 a MEMS inertial measurement unit (IMU) array cannot only improve the measurement accuracy but also widening the range of the angular velocity, because MEMS IMU is consisted of the MEMS gyroscope and accelerometer. MEMS IMU can be shorted as MIMU in the paper. In the paper, we apply the MIMU array to the vehicles and carry out some theoretical researches and simulation analysis.

One of the questions when using the MIMU array is how to fuse the redundant output of gyroscopes and accelerometers. Regarding inertial arrays that consist of multiple IMUs, the measurement fusion problem has mainly been studied in the framework of global navigation satellite system aided inertial navigation systems.^25–29 In the literatures, the proposed information fusion approaches can be broadly grouped into two categories. In the first category, the IMU measurements are fused before they are used in the inertial navigation systems; a weighted average is used and the spatial separation of the sensors is neglected commonly. In the second category, they are fused after being processed by several parallel inertial navigation systems. And a few discussions on the pros and cons of the two approaches, as well as an evaluation of different measurement fusion methods, have been presented in Bancroft and Lachapelle. ²⁶ Based on the MIMU array design, Skog et al. ²⁴ posed the problem of fusing the measurements from an array of accelerometer and gyroscope triads as a parameter estimation problem for the first time. The method derives a maximum likelihood based measurement fusion, outperforming the current state-of-the-art method for measurement fusion as shown in simulation results. Moreover, it allows for smaller and cheaper senor arrays to be constructed. In the paper, such a fusion method is called maximum likelihood estimation (MLE). MLE used in the MIMU array has been demonstrated that it is beneficial to improve performance in terms of pedestrian tracking, but it only gives suboptimal performance. Thus, how to fuse the multiple measurements of the MIMU array for getting more optimal performance is still not clear. Furthermore, there has not been the research applying the MIMU array to the vehicles yet, such as the multi-motor UAV, and another significant issue is whether the current fused method should also be optimized when used in the vehicles. On the basis of the above analysis and being inspired by the fusion method of the MIMU array in Skog et al., ²⁴ this paper proposes an improved angular velocity fusion method of the MIMU array.

The proposed method models the angular velocity according to kinetic characteristics of the vehicles, which refers to the work by Liu et al. ⁹ The model is regarded as the state equation. And it regards the signal model of gyroscopes and accelerometers on the array as the measurement equation. Since the accelerometers’ output-based measurement equation is associated with the angular velocity quadratically, the angular velocity is estimated by the extended Kalman filter (EKF). Moreover, the state and measurement noises are correlated in our established models, thus the traditional EKF coping with the state and measurement uncorrelated noises should be also improved so as to estimate the angular rate accurately. To discuss and demonstrate the performance of our improved fusion method, the paper conducts the simulation and kinematic vehicle experiments. The experiment results demonstrate that the innovation of the paper can be summarized into two main points as follows:

1. The angular velocity can be adaptively estimated according to the angular rates with arbitrary magnitudes, which owes to the signal model of the MIMU array and the state equation with an adaptive model of the angular velocity. Based on the presented fusion model, the angular velocity is estimated more accurately compared with the MLE method;

2. By optimizing the traditional EKF equations, the angular velocity of MIMU array can be estimated even in the filter equations, where the state and measurement noises are correlative.

The remainder of this paper elaborates the research mentality in detail and is organized as follows. Section “Angular velocity fusion method of the MIMU array” derives the angular velocity fusion method of the MIMU array based on EKF with correlated system noises. Section “Experiments and analysis” discusses and analyzes the simulation experimental results. And then conclusions are presented in section “Conclusion.”

Angular velocity fusion method of the MIMU array

This section is divided into three sub-sections to present the angular velocity fusion method of the MIMU array. Section “Accelerometer and gyroscope signal model of the MIMU array” builds the accelerometer and gyroscope signal model of the MIMU array, section “MLE-based fusion method” gives a brief description of the MLE-based method to fuse the angular velocity, and section “Angular velocity estimation based on EKF with correlated system noises” derives our proposed fusion method based on EKF with the correlated state and measurement noises in detail.

The proposed angular velocity fusion process of the MIMU array can be described as shown in Figure 1.

Figure 1.

Block diagram of the proposed angular velocity fusion method of the MIMU array.

Accelerometer and gyroscope signal model of the MIMU array

The derivation of the signal model takes its starting point in the, from classical mechanics obtained, relationship between forces in rotating coordinate frames. The specific force in one point of a rotating coordinate frame should be decomposed into that of another point, a centrifugal term, and an Euler term as illustrated in Figure 2.

Figure 2.

The decomposition of the specific force sensed by an accelerometer in the MIMU array.

The ideal specific force ${\mathit{f}}_i^b$ sensed by the ith accelerometer triad located at position ${\mathit{r}}_i$ in the array coordinate frame can be modeled as

$${\mathit{f}}_i^b={\mathit{f}}^b+\underset{\mathrm{Centrifugal}\mathrm{force}}{\underbrace{\mathit{\omega }\times (\mathit{\omega }\times {\mathit{r}}^i)}}+\underset{\mathrm{Euler}\mathrm{force}}{\underbrace{\stackrel{·}{\mathit{\omega }}\times {\mathit{r}}^i}}$$

(1)

where ${\mathit{f}}^b$ denotes the specific force sensed at the center of the array frame, $\mathit{\omega }$ and $\stackrel{·}{\omega }$ are the array frame’s angular velocity and angular with respect to inertial space, respectively. Then, ${\mathrm{\Omega }}_{\mathit{a}}$ is used to denote the skew-symmetric matrix representations of the vector $\mathit{a}$ , and ${\mathrm{\Omega }}_{\mathit{a}}\mathit{b}$ can represent any two vector cross product $\mathit{a}\times \mathit{b}$ . Thus, equation (1) can be rewritten as

$${\mathit{f}}_i^b={\mathit{f}}^b+{\mathbf{\Omega }}_{\mathit{\omega }}{\mathit{r}}_i-{\mathrm{\Omega }}_{{\mathit{r}}_i}\stackrel{·}{\mathit{\omega }}$$

(2)

Considering the measurement noise of the accelerometer, we can mark the ith accelerometer as ${\mathit{s}}_i$ to replace ${\mathit{f}}_i^b$ in equation (2). The measurement vector ${\mathit{z}}_s=[\begin{array}{ccc}{{\mathit{s}}_1}^{\mathrm{T}}\hfill & \cdots \hfill & {{\mathit{s}}_{\mathrm{N}}}^{\mathrm{T}}\hfill \end{array}{]}^{\mathrm{T}}$ consisting of the measurements from all the accelerometers in the array can be modeled as

$${\mathit{z}}_s={\mathbf{h}}_s\left(\omega \right)+{\mathbf{H}}_s\left(\omega \right)\mathit{\varphi }+{\mathbf{n}}_{\mathbf{s}}$$

(3)

where ${\mathbf{H}}_s=\left[\begin{array}{cc}\mathbf{G}\hfill & 1_N\otimes {\mathbf{I}}_3\hfill \end{array}\right]$ , $\mathbf{G}=\left[\begin{array}{c}-{\mathrm{\Omega }}_{{\mathit{r}}_1}\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{-{\mathrm{\Omega }}_{{\mathit{r}}_N}}\end{array}\right]$ , $\mathit{\varphi }={\left[\begin{array}{cc}{\stackrel{·}{\mathit{\omega }}}^{\mathrm{T}}\hfill & {\left({\mathit{f}}^{\mathbf{b}}\right)}^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$ , ${\mathbf{h}}_s\left(\mathit{\omega }\right)=\left[\begin{array}{c}{\mathrm{\Omega }}_{\mathit{\omega }}{\mathit{r}}_1\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathrm{\Omega }}_{\mathit{\omega }}{\mathit{r}}_N}\end{array}\right]$ , and ${\mathbf{n}}_s$ is the measurement noise of the accelerometers. ⊗ is the Kronecker product of the matrix. Equation (3) represents the accelerometer signal model of the MIMU array.

The gyroscope measurement vector in the array frame is marked as ${\mathit{z}}_{\omega }=[\begin{array}{ccc}{\mathit{\omega }}_1^{\mathrm{T}}\hfill & \cdots \hfill & {\mathit{\omega }}_N^{\mathrm{T}}\hfill \end{array}{]}^{\mathrm{T}}$ and the relationship between ${\mathit{z}}_{\omega }$ and the angular velocity $\mathit{\omega }$ of the array can be given by equation (4), where ${\mathbf{n}}_{\omega }$ is the noise of the gyroscope. Equation (4) is the signal model of the gyroscopes

$${\mathit{z}}_{\omega }={\mathbf{H}}_{\omega }\mathit{\omega }+{\mathbf{n}}_{\omega }$$

(4)

MLE-based fusion method

On the basis of the accelerometer and gyroscope signal model in equations (4) and (5), they can be combined and rewritten as the form shown in equation (5)

$$\mathit{z}=\mathbf{h}\left(\mathit{\omega }\right)+\mathbf{H}\mathit{\varphi }+\mathbf{n}$$

(5)

where $\mathit{z}=\left[\begin{array}{c}{\mathit{z}}_s\hfill \\ \multicolumn{1}{c}{{\mathit{z}}_{\omega }}\end{array}\right]$ , $\mathbf{h}\left(\mathit{\omega }\right)=\left[\begin{array}{c}{\mathbf{h}}_s\left(\mathit{\omega }\right)\hfill \\ \multicolumn{1}{c}{{\mathbf{H}}_{\omega }\mathit{\omega }}\end{array}\right]$ , $\mathbf{H}=\left[\begin{array}{c}{\mathbf{H}}_s\left(\mathit{\omega }\right)\hfill \\ \multicolumn{1}{c}{0_{3N\times 6}}\end{array}\right]$ , $\mathbf{n}=\left[\begin{array}{c}{\mathbf{n}}_s\hfill \\ \multicolumn{1}{c}{{\mathbf{n}}_{\omega }}\end{array}\right]$ .

Seen from equation (5), the relationship between the angular velocity $\mathit{\omega }$ and the measurement vectors of gyroscopes and accelerometers on the MIMU array is mainly embodied in $\mathbf{h}(\mathit{\omega })$ , which includes a non-linear part ${\mathbf{h}}_s(\mathit{\omega })$ . While the relationship between the specific force ${\mathit{f}}^b$ and the measurement vectors of gyroscopes and accelerometers on the MIMU array is the linear part $\mathbf{H}\mathit{\varphi }$ , where $\mathit{\varphi }$ consists of the specific force ${\mathit{f}}^b$ and the angular acceleration $\stackrel{·}{\omega }$ . To obtain the angular velocity $\mathit{\omega }$ of equation (5), an MLE based on the Newton iteration is proposed in Skog et al. ²⁴ Assuming the measurement noise $\mathbf{n}$ to be zero-mean and Gaussian distributed with the known covariance matrix $\mathit{Q}$ , the MLE of $\{\mathit{\omega },\mathit{\varphi }\}$ in equation (5) can be given by equation (6)

$$\{\hat{\mathit{\omega }},\hat{\mathit{\varphi }}\}=\arg \min (\Vert \mathit{z}-\mathbf{h}\left(\mathit{\omega }\right)-\mathbf{H}\mathit{\varphi }{\Vert }_{{\mathit{Q}}^{-1}}^2)$$

(6)

To solve $\hat{\varphi }$ from equation (6), we fix the vector $\mathit{\omega }$ to ${\mathit{\omega }}^{*}$ and then solve $\hat{\varphi }({\mathit{\omega }}^{*})$ according to the weighted least square, which is shown in equation (7)

$$\hat{\mathit{\varphi }}\left({\mathit{\omega }}^{*}\right)={\left({\mathbf{H}}^{\mathrm{T}}{\mathit{Q}}^{-1}\mathbf{H}\right)}^{-1}{\mathbf{H}}^{\mathrm{T}}{\mathit{Q}}^{-1}(\mathit{z}-\mathbf{h}\left({\mathit{\omega }}^{*}\right))$$

(7)

Then, the MLE of $\mathit{\omega }$ can be calculated by substituting equation (7) into equation (6) as shown in equation (8), where the expression of $\mathbf{P}$ is expressed as equation (9)

$$\hat{\mathit{\omega }}=\arg \min (\Vert \mathit{z}-\mathbf{h}\left(\mathit{\omega }\right){\Vert }_{\mathbf{P}}^2)$$

(8)

$$\mathbf{P}={\mathit{Q}}^{-1}-{\mathit{Q}}^{-1}\mathbf{H}{\left({\mathbf{H}}^{\mathrm{T}}{\mathit{Q}}^{-1}\mathbf{H}\right)}^{-1}{\mathbf{H}}^{\mathrm{T}}{\mathit{Q}}^{-1}$$

(9)

For solving $\hat{\omega }$ in equation (8), the Gauss Newton iterative method is applied, and the iterative calculation process is given by equation (10), where ${\mathbf{J}}_h$ is the Jacobian matrix of $\mathbf{h}(\mathit{\omega })$

$$\begin{array}{c}\multicolumn{1}{c}{{\hat{\mathit{\omega }}}_{k+1}={\hat{\mathit{\omega }}}_k+{\left({{\mathbf{J}}_h}^{\mathrm{T}}\mathit{P}{\mathbf{J}}_h\right)}^{-1}{{\mathbf{J}}_h}^{\mathrm{T}}\mathbf{P}(\mathit{z}-\mathbf{h}\left({\hat{\mathit{\omega }}}_k\right))}\\ \multicolumn{1}{c}{{\hat{\mathit{\omega }}}_0={\left((1_N^{\mathrm{T}}\otimes {\mathbf{I}}_3){\mathit{R}}_{\mathit{\omega }}^{-1}(1_N\otimes {\mathbf{I}}_3)\right)}^{-1}(1_N^{\mathrm{T}}\otimes {\mathbf{I}}_3){\mathit{R}}_{\omega }^{-1}{\mathit{z}}_{\omega }}\end{array}$$

(10)

The Cramer–Rao bound (CRB) of ${\hat{\mathit{f}}}^b$ and $\hat{\omega }$ is also analyzed in Skog et al. ²⁴ When the gyroscope noise variance ${\sigma }_{\omega }^2$ is equal, the relationship $\mathrm{Cov}(\hat{\mathit{\omega }})\ge {\mathit{R}}_{\omega }^{-1}$ can be calculated, where

$$\begin{array}{c}{\mathit{R}}_{\omega }=\frac{N}{{\sigma }_{\omega }^2}{\mathbf{I}}_3+\frac{d^2(N^2-N)}{6{\sigma }_s^2}\hfill \\ \left[\begin{array}{ccc}2{\omega }_x^2+{\omega }_y^2\hfill & {\omega }_x{\omega }_y\hfill & 2{\omega }_x{\omega }_z\hfill \\ \multicolumn{1}{c}{{\omega }_x{\omega }_y}& 2{\omega }_x^2+{\omega }_y^2\hfill & 2{\omega }_y{\omega }_z\hfill \\ \multicolumn{1}{c}{2{\omega }_x{\omega }_z}& 2{\omega }_y{\omega }_z\hfill & 4{\omega }_z^2\hfill \end{array}\right]\hfill \end{array}$$

and $d$ represents the distance between each MIMU. It can be seen from such a formula that the error variance of the estimated angular velocity is smaller than that of the arithmetic mean of the gyroscope measurement errors, as the accelerometer can also estimate the angular velocity in the array. And when the angular velocity exceeds the measurement range of the gyroscope, the accelerometer can still estimate the angular velocity, thus the range of the angular velocity measured only by the gyroscope can be widened. The measurement reliability of the angular velocity is improved.

The MLE method based on Newton iteration can effectively fuse the redundant output of gyroscopes and accelerometers in the MIMU array, but Skog et al. ³⁰ point out that the above method can only achieve the sub-optimal fusion performance. To further improve the fusion accuracy of the angular velocity of the MIMU array, section “Angular velocity estimation based on EKF with correlated system noises” proposes an optimized EKF-based fusion method.

Angular velocity estimation based on EKF with correlated system noises

In the MIMU array, because the accelerometer signal model has a non-linear relationship with the angular velocity (seen in equation (2)), an EKF-based fusion method to estimate the angular velocity is proposed in this section.

State and measurement equations building for estimating the angular velocity

According to the kinetic characteristics, the relationship between $\mathit{\omega }$ and the angular acceleration $\stackrel{·}{\omega }$ can be given by equation (11). $\stackrel{·}{\omega }$ can be regarded as the Gaussian white noise ⁹ and its variance can be adjusted on the basis of different maneuvering states of the vehicles. When the angular rate changes rapidly, the angular acceleration is adjusted to a larger value, that is, the state noise is increased. When it changes slowly, the angular acceleration is set to a smaller value. In equation (11), $\mathrm{\Delta }\mathrm{T}$ is the output frequency of the MIMU array

$${\mathit{\omega }}_k={\mathit{\omega }}_{k-1}+\mathrm{\Delta }\mathrm{T}{\stackrel{·}{\mathit{\omega }}}_{k-1}$$

(11)

On the basis of equation (11), the state equation for estimating the angular velocity $\mathit{\omega }$ can be described as equation (12), where the state vector ${\mathit{X}}_k$ is the angular velocity. ${\mathit{W}}_{k-1}=\mathrm{\Delta }\mathrm{T}{\stackrel{·}{\omega }}_{k-1}$ and its covariance matrix is $\mathrm{Cov}({\mathit{W}}_k,{\mathit{W}}_j)=E[{\mathit{W}}_k{\mathit{W}}_j^{\mathrm{T}}]=\mathrm{\Delta }{\mathrm{T}}^2{\mathit{Q}}_{\stackrel{·}{\omega }}$

$${\mathit{X}}_k={\mathit{X}}_{k-1}+{\mathit{W}}_{k-1}$$

(12)

The relationship between the specific force ${\mathit{f}}^b$ of the array and the ideal output ${\mathit{f}}_1^b$ of the accelerometer at the position ${\mathit{r}}_1$ can be expressed as the form shown in equation (13) according to equation (2). Similarly, the relationship of ${\mathit{f}}_i^b(i=2,3,\dots ,N)$ and ${\mathit{f}}^b$ can be given by equation (14)

$${\mathit{f}}^b={\mathit{f}}_1^b-{\mathrm{\Omega }}_{\omega }^2{\mathit{r}}_1+{\mathrm{\Omega }}_{{\mathit{r}}_{\mathit{1}}}\stackrel{·}{\mathit{\omega }}$$

(13)

$${\mathit{f}}^b={\mathit{f}}_i^b-{\mathrm{\Omega }}_{\omega }^2{\mathit{r}}_i+{\mathrm{\Omega }}_{{\mathit{r}}_i}\stackrel{·}{\mathit{\omega }}$$

(14)

Equation (15) can be obtained by subtracting equation (14) from equation (13)

$${\mathit{f}}_i^b-{\mathit{f}}_1^b={\mathrm{\Omega }}_{\omega }^2({\mathit{r}}_i-{\mathit{r}}_1)+\stackrel{·}{\mathit{\omega }}({\mathrm{\Omega }}_{{\mathit{r}}_1}-{\mathrm{\Omega }}_{{\mathit{r}}_i})$$

(15)

When measurement errors ${\mathbf{n}}_s$ of the accelerometers are considered, equation (15) can be rewritten as equation (16)

$${\mathit{Z}}_s={\mathbf{h}}_{\mathrm{zs}}\left(\mathit{\omega }\right)+{\mathbf{G}}_s\stackrel{·}{\mathit{\omega }}+{\mathbf{n}}_s$$

(16)

where

$$\begin{array}{c}{\mathit{Z}}_s=\left[\begin{array}{c}{\mathit{s}}_2-{\mathit{s}}_1\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathit{s}}_N-{\mathit{s}}_1}\end{array}\right],{\mathbf{h}}_{\mathrm{zs}}\left(\mathit{\omega }\right)=\left[\begin{array}{c}{\mathrm{\Omega }}_{\omega }^2({\mathit{r}}_2-{\mathit{r}}_1)\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathrm{\Omega }}_{\omega }^2({\mathit{r}}_N-{\mathit{r}}_1)}\end{array}\right],\hfill \\ {\mathbf{G}}_s=\left[\begin{array}{c}{\mathrm{\Omega }}_{{\mathit{r}}_1}-{\mathrm{\Omega }}_{{\mathit{r}}_2}\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathrm{\Omega }}_{{\mathit{r}}_1}-{\mathrm{\Omega }}_{{\mathit{r}}_N}}\end{array}\right],\hfill \end{array}$$

and the variance matrix of ${\mathbf{n}}_s$ is

$${\mathit{R}}_s=\left[\begin{array}{c}{\mathit{Q}}_{{\mathit{S}}_2}+{\mathit{Q}}_{{\mathit{S}}_1}\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathit{Q}}_{{\mathit{S}}_N}+{\mathit{Q}}_{{\mathit{S}}_{\mathit{1}}}}\end{array}\right].$$

${\mathit{Q}}_{{\mathit{S}}_i}(i=1,\dots ,N)$ is the accelerometer noise variance matrix at the position ${\mathit{r}}_i$ , and ${\mathit{R}}_s=2{\sigma }_s^2{\mathbf{I}}_{3N-3}$ when the noise variance on each axis of the accelerometer is equal as ${\sigma }_s^2$ .

Then, by combining equations (4) and (16), the measurement equation of the angular velocity $\mathit{\omega }$ can be given by equation (17), where

$${\mathit{Z}}_k={\left[\begin{array}{cc}{\mathit{Z}}_s^{\mathrm{T}}\hfill & {\mathit{Z}}_{\omega }^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}},\mathbf{h}\left({\mathit{\omega }}_k\right)=\left[\begin{array}{c}{\mathbf{h}}_{\mathrm{zs}}\left({\mathit{\omega }}_k\right)\hfill \\ \multicolumn{1}{c}{{\mathbf{H}}_{\omega }{\mathit{\omega }}_k}\end{array}\right]$$

and the covariance matrix $\mathit{R}$ corresponding to the measurement noise ${\mathit{V}}_k$ is shown in equation (18)

$${\mathit{Z}}_k=\mathbf{h}\left({\mathit{\omega }}_k\right)+{\mathit{V}}_k$$

(17)

$$\mathit{R}=\left[\begin{array}{cc}{\mathit{G}}_s{\mathit{Q}}_{\stackrel{·}{\omega }}{\mathbf{G}}_s^{\mathrm{T}}+{\mathit{R}}_s\hfill & 0_{(3N-3)\times 3N}\hfill \\ \multicolumn{1}{c}{0_{3N\times (3N-3)}}& {\mathit{R}}_{\mathit{\omega }}\hfill \end{array}\right]$$

(18)

It can be seen from equation (18) that ${\mathit{Z}}_s$ has a non-linear relationship with $\mathit{\omega }$ , so the following content will adopt EKF to estimate $\mathit{\omega }$ . The traditional filter equations of EKF are built on the premise that the state and measurement noises are independent to each other. Nevertheless, seen from the above derivation, the state noise $\mathrm{\Delta }{\mathrm{T}}^2{\mathit{Q}}_{\stackrel{·}{\omega }}$ is related to the measurement noise ${\mathbf{G}}_s{\mathit{Q}}_{\stackrel{·}{\omega }}{\mathbf{G}}_s^{\mathrm{T}}+{\mathit{R}}_s$ of equation (18), so it is necessary to optimize the traditional EKF equation for estimating $\mathit{\omega }$ .

Estimation process of the angular velocity based on the optimized EKF

When the state noise is related to the measurement noise, the general form of EKF equation has been derived. ³¹ Thus, on the basis of the state and measurement equation built in equations (12) and (17), we can further derive the optimized EKF equation when the state and measurement noises are correlated. The recursive formula of one-step prediction ${\hat{\mathit{X}}}_k^{-}$ with respect to $\mathit{\omega }$ can be given by equation (19), where $\mathit{M}$ is shown in equation (20)

$${\hat{\mathit{X}}}_k^{-}={\hat{\mathit{X}}}_{k-1}^{+}+{\mathit{M}}_{k-1}({\mathit{Z}}_{k-1}-\mathit{h}\left({\hat{\mathit{\omega }}}_{k-1}^{+}\right))$$

(19)

$$\mathit{M}=\mathit{S}{\mathit{R}}^{-1},\mathit{S}=\left[\begin{array}{cc}\mathrm{\Delta }\mathrm{T}{\mathit{Q}}_{\stackrel{·}{\omega }}{\mathbf{G}}_s^{\mathrm{T}}\hfill & 0_{3\times 3N}\hfill \end{array}\right]$$

(20)

The state estimation ${\hat{\mathit{X}}}_k^{+}$ at the kth iteration and the filter gain ${\mathit{K}}_k$ are calculated in equations (21) and (22), respectively

$${\hat{\mathit{X}}}_k^{+}={\hat{\mathit{X}}}_k^{-}+{\mathit{K}}_k({\mathit{Z}}_k-\mathit{h}\left({\hat{\mathit{\omega }}}_{k-1}^{-}\right))$$

(21)

$$\mathit{M}{\mathit{K}}_k={\mathit{P}}_k^{-}{\mathit{H}}_k^{\mathrm{T}}{({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^{\mathrm{T}}+{\mathit{R}}_k)}^{-1}$$

(22)

where ${\mathit{H}}_k$ is obtained by solving the Jacobian matrix of $\mathbf{h}({\mathit{\omega }}_k)$ as shown in equations (23) and (24)

$${\mathit{H}}_k=\left[\begin{array}{cc}\begin{array}{cc}\mathit{A}{({\mathit{\omega }}_k,{\mathit{r}}_2-{\mathit{r}}_1)}^{\mathrm{T}}\hfill & \cdots \hfill \end{array}\hfill & \begin{array}{cc}\mathit{A}{({\mathit{\omega }}_k,{\mathit{r}}_N-{\mathit{r}}_1)}^{\mathrm{T}}\hfill & {\mathbf{H}}_{\omega }^{\mathrm{T}}\hfill \end{array}\hfill \end{array}\right]$$

(23)

$$\mathit{A}({\mathit{\omega }}_k,{\mathit{r}}_i-{\mathit{r}}_1)={\mathrm{\Omega }}_{{\mathit{\omega }}_k}{\mathrm{\Omega }}_{({\mathit{r}}_i-{\mathit{r}}_1)}+{\mathrm{\Omega }}_{{\mathit{\omega }}_{k\times ({\mathit{r}}_i-{\mathit{r}}_1)}}$$

(24)

The calculation formula of one-step prediction ${\mathit{P}}_k^{-}$ of the state covariance is shown in equation (25) and then ${\mathit{P}}_k^{-}$ is calculated in equation (26)

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{P}}_k^{-}=({\mathbf{I}}_3-{\mathit{M}}_{k-1}{\mathit{H}}_{k-1}){\mathit{P}}_k^{+}{({\mathbf{I}}_3-{\mathit{M}}_{k-1}{\mathit{H}}_{k-1})}^{\mathrm{T}}}\\ +\mathrm{\Delta }{\mathrm{T}}^2{\mathit{Q}}_{\omega ,k-1}-{\mathit{M}}_{k-1}{\mathit{S}}_{k-1}^{\mathrm{T}}\hfill \end{array}$$

(25)

$${\mathit{P}}_k^{+}={\mathit{P}}_k^{-}-{\mathit{K}}_k{\mathit{H}}_k{\mathit{P}}_k^{-}$$

(26)

Thus, based on equations (19)–(26), the angular velocity can be estimated through optimized EKF. Seen from the derivation, the condition for deriving MIMU fusion models of the MIMU array is that the MIMU is of the same type, that is, $n_{\omega }$ and $n_s$ are the same for each MIMU, and thus the performance of the MIMU array could be better than that of the single MIMU.

Experiments and analysis

To analyze the performance of our proposed method in section “Angular velocity estimation based on EKF with correlated system noises,” we simulate some typical angular maneuvers to generate the signal measurement of gyroscopes and accelerometers on the MIMU and compare the angular velocity estimation with that of the MLE method. Then, the kinematic vehicle experiments are also designed to verify the performance of the proposed method further.

Simulation experiments and analysis

Simulation experiments

In the experiments, we simulate an MIMU array composed of 16 inertial sensors, and its spatial layout is shown in Figure 3. The principle of selecting the number of MIMU on the array is analyzed in Chapter 5 of Xing. ³¹ When the number of MIMU on the array is not less than 4, the parallel symmetry and skew layout have similar reliability and accuracy. While the more sensors there are, the more advantages of parallel symmetrical layout in terms of volume and design complexity. So the simulation experiments design the array with 16 MIMUs.

Figure 3.

The layout of the simulated MIMU array.

The angular motion patterns can be abstracted into two types: (1) constant angular rate and (2) change-in-time angular velocity with variable frequency and amplitude. Meanwhile, in some specific motions with large angular rates, the gyroscopes would be out of the measurement range due to intense angular motions. Thus, the simulation experiment with change-in-time angular velocity also includes the large angular rate at some points in time. The ideal triaxial angular velocities within different time periods are shown in Table 1.

Table 1.

Ideal triaxial angular velocity within different time periods.

Time periods (s)	$\omega$ (deg/s)
	X	Y	Z
0–27.5	100sin(πt/5)	200	300
27.5–30	0	200	300
30–67	0	200	2000sin(2πt/25)
67–70	0	200	0
70–96.5	0	300sin(2πt/25)	0
96.5–100	0	0	0

In the experiments, the line motion is set to a uniform linear motion.

The distance between each two MIMUs is d = 1 cm in Figure 3. The measurement error’s standard deviations of the gyroscope and accelerometer on the array are shown in Table 2, and the measurement range of gyroscope is set as ±1500 deg/s.

Table 2.

Standard deviation of the MIMU measurement error on the array.

	Standard deviation	Unit
Gyroscope, ${\sigma }_{\omega }$	1	deg/s
Accelerometer, ${\sigma }_s$	10	mg

The simulated triaxial outputs and Z-axis local amplification of gyroscopes on the MIMU array are shown in Figure 4. The numerals 1–16 in the Z-axis local amplification of Figure 4 represent the number of MIMU in Figure 3. It can be seen from Figure 4 that the angular rate cannot be measured accurately when it is larger than the gyroscope measurement range 1500 deg/s.

Figure 4.

Triaxial outputs and Z-axis local amplification of gyroscopes on the MIMU array.

Based on the simulated original data of the gyroscopes and accelerometers in Figures 4 and 5, the angular velocity fused results of our proposed method are analyzed in section “Simulation results analysis.”

Figure 5.

Triaxial outputs and Y-axis local amplification of accelerometers on the MIMU array.

The simulated triaxial outputs and Y-axis local amplification of accelerometers on the MIMU array are shown in Figure 5. Although the speed is set to be unchanged in the experiments, that is to say, the ideal specific force at the center of the array should be [0, 0, −g], the outputs of accelerometers on the same axis change with time. For instance, the outputs of accelerometers on the same axis vary between 30 and 70 s in Figure 4. The reason is that the measurement of each accelerometer on the array is comprehensively affected by the angular velocity and angular acceleration as shown in equation (1) of section “Angular velocity fusion method of the MIMU array.”

Simulation results analysis

In this section, the traditional MLE and the proposed method based on the optimized EKF are, respectively, applied to estimate the angular velocity . The comparisons of the estimated angular rates with different methods are shown in Figures 6–8.

Figure 6.

Comparison of the X-axis angular rate.

Figure 7.

Comparison of the Y-axis angular rate.

Figure 8.

Comparison of the Z-axis angular rate.

It can be seen from Figures 6–8 that the triaxial angular rate can be estimated regardless of how high the frequency is and how large the amplitude is. This because the established estimation model of the angular velocity in equation (11) can be adjusted adaptively. The frequency of ${\omega }_y$ is higher, thus ${\stackrel{·}{\omega }}_y$ in the model is adjusted to a larger value. The frequency of ${\omega }_z$ is lower, and then ${\stackrel{·}{\omega }}_z$ is adjusted to a smaller value. Moreover, even when the Z-axis angular rate is out of the gyroscope measurement range seen from Figures 4 and 8, applying the two methods can also estimate the angular rate accurately, which is consistent with the conclusion of section “MLE-based fusion method.” To analyze and compare the performance differences between MLE and our proposed optimization EKF, the triaxial angular rate estimation errors under the conditions of constant and change-in-time angular rates are calculated, respectively, as shown in Figures 9 and 10, that is, corresponding to Figures 6–8.

Figure 9.

Angular velocity estimation errors under the condition of constant angular rates.

Figure 10.

Angular velocity estimation errors under the condition of change-in-time angular rates.

In addition, the root mean square errors (RMSE) of the estimated angular rates are calculated from the Monte Carlo simulation experiments. ²⁴ Table 3 shows the RMSE comparison of MLE and optimized EKF under the condition of constant angular rates.

Table 3.

RMSE of the angular velocity under the condition of constant angular rates.

Axis	${\sigma }_{\omega }$ (deg/s)	RMSE (deg/s)
		MLE	Optimized EKF
X	1	0.236	0.153
Y	1	0.212	0.139
Z	1	0.239	0.148

MLE: maximum likelihood estimation; EKF: extended Kalman filter.

Table 4 shows the RMSE comparison of the two methods under the condition of change-in-time angular rates.

Table 4.

RMSE of the angular velocity under the condition of change-in-time angular rates.

Axis	${\sigma }_{\omega }$ (deg/s)	RMSE (deg/s)
		MLE	Optimized EKF
X	1	0.254	0.173
Y	1	0.244	0.161
Z	1	0.341	0.251

MLE: maximum likelihood estimation; EKF: extended Kalman filter; RMSE: root mean square error.

In section “MLE-based fusion method,” it is analyzed that after fusing the measurements of gyroscopes and accelerometers on the MIMU array, the CRB of the estimated angular meets the formula as shown in equation (27), that is to say, the minimum RMSE of the estimated angular rate can be smaller than ${\sigma }_{\omega }/\sqrt{N}$ . In the simulation experiments, as $N$ is 16, the estimated accuracy of the angular rate in the array is at least four times higher than the measurement of one gyroscope, and the statistical results given in Tables 3 and 4 are consistent with the theoretical analysis

$$\begin{array}{c}\mathrm{Cov}\left(\hat{\omega }\right)\ge {\mathit{R}}_{\omega }^{-1},{\mathit{R}}_{\omega }=\frac{N}{{\sigma }_{\omega }^2}{\mathbf{I}}_3+\frac{d^2(N^2-N)}{6{\sigma }_s^2}\hfill \\ \left[\begin{array}{ccc}2{\omega }_x^2+{\omega }_y^2\hfill & {\omega }_x{\omega }_y\hfill & 2{\omega }_x{\omega }_z\hfill \\ \multicolumn{1}{c}{{\omega }_x{\omega }_y}& 2{\omega }_x^2+{\omega }_y^2\hfill & 2{\omega }_y{\omega }_z\hfill \\ \multicolumn{1}{c}{2{\omega }_x{\omega }_z}& 2{\omega }_y{\omega }_z\hfill & 4{\omega }_z^2\hfill \end{array}\right]\hfill \end{array}$$

(27)

Moreover, it can be seen from Tables 3 and 4 that the optimized EKF-based estimation method proposed in the paper can adaptively adjust the angular velocity model according to the dynamic characteristics, so its angular rate estimation accuracy can be increased by about 1.5 times compared to the MLE method.

Kinematic vehicle experiments and results

In this section, the kinematic vehicle experiment was designed based on an in-house designed MIMU array and the fiber optic strapdown inertial navigation system (SPAN-CPT). The gyroscope outputs of SPAN-CPT were collected as the comparison of that of the MIMU array, so as to evaluate the proposed angular velocity fusion method of the MIMU array. The in-house designed MIMU array is shown in Figure 11.

Figure 11.

In-house designed MIMU array.

Five different types of MIMUs are integrated on the MIMU array to conduct the extended research and validation of the data fusion of the MIMU array. Due to the angular velocity fusion method proposed in this paper which is based on the redundant output of the same type of MIMU, we chose one type of MIMU for analysis in the kinematic vehicle experiments. Before the kinematic vehicle experiment, the temperature experiment, vibration experiment, and zero bias repeatability experiment were carried out for the five different types of MIMUs. Then through the above performance test experiments, it is concluded that the performance of BMI055 is the best. Therefore, the outputs of BMI055 on the array were used to estimate the angular velocity fusion in the experiment. The layout of BMI055 on the MIMU array is shown in Figure 11, and the performance index parameters of its gyroscopes and accelerometers are shown in Table 5. In Figure 11, the data of BMI055 on the MIMU array were collected by the integrated advanced RISC machine (ARM) processor and stored in real time in the TF card on the back of the module. Through the designed verified software, the data stored in the TF card are read to verify the angular velocity fusion method.

Table 5.

Performance of BMI055.

	Gyroscope	Acceleration
Bias	$1/\mathrm{s}$	$10\mathrm{mg}$
Angle random walk	$0.014/\mathrm{s}/\sqrt{\mathrm{Hz}}$	–
Speed random walk	–	$150\mathrm{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$

To compare and analyze the performance of the algorithm, the output of the fiber-optic gyroscope in Novatel’s SPAN-CPT integrated navigation system was selected as the comparison benchmark in the experiment. The kinematic vehicle experimental system based on the independently designed MIMU array and the SPAN-CPT integrated navigation system is shown in Figure 12. The SPAN-CPT system could directly use the +13-V DC power provided by the vehicle system.

Figure 12.

Kinematic vehicle experiments systems.

Kinematic vehicle experiments

In total, 350 s of data were totally collected in the sports car experiment. The driving track and the vehicle status measured by the SPAN-CPT integrated navigation system in the experiment are shown in Figure 13. It can be seen from the figure that the sports car’s driving track is several rounds around a triangular area. In the attitude curve, in addition to the maneuvering changes in course, the horizontal rolling and pitching directions also have a small fluctuation in a certain range due to the sway of the car body and the road fluctuating.

Figure 13.

Trajectory and attitude in the kinematic vehicle experiment.

The definition of the coordinates of the MIMU array is shown in Figure 14. The coordinates of BMI055 marked as A in the figure are $r_A=[\begin{array}{ccc}1.25\hfill & 0.85\hfill & 0\hfill \end{array}]$ , while the coordinates marked as B are $r_B=[\begin{array}{ccc}2.65\hfill & 0.85\hfill & 0\hfill \end{array}]$ , and the coordinates marked as C are $r_C=[\begin{array}{ccc}1.25\hfill & -1.75\hfill & 0\hfill \end{array}]$ , as well as the coordinates marked as D are $r_D=[\begin{array}{ccc}2.65\hfill & -1.75\hfill & 0\hfill \end{array}]$ . Coordinates are in centimeters (cm).

Figure 14.

Coordinate frame of the MIMU array.

The outputs of the gyroscopes and accelerometers of the four BMI055s in Figure 14 and their partial enlarged diagrams are shown in Figures 15 and 16.

Figure 15.

Gyroscope outputs of BMI055 on the array.

Figure 16.

Accelerometer outputs of BMI055 on the array.

After deducting the bias of the gyroscopes and accelerometers in the BMI055 array shown in Figures 15 and 16, the angular velocity fusion method of the MIMU array proposed in the paper and the traditional MLE method are, respectively, used to estimate the angular velocity measured by BMI055, and the results are analyzed in section “Results analysis.”

Results analysis

The fused angular velocity result is compared with the outputs of the gyroscopes, in the SPAN-CPT, with the bias deducted, which acts as the data comparison benchmark when comparing the RMSE of the fused angular velocity by two fusion methods. The result is shown in Table 6. After analyzing the data shown in Table 6, it can be seen from the result that the measurement accuracy of the fused angular velocity of MIMU array is about two times higher than that of a single gyroscope, while it is consistent with the theoretical analysis and simulation result. Compared with the traditional MLE method, the angular velocity fusion method proposed in this paper has improved its accuracy by about 1.5 times. As is shown in the above analysis results, the sports car experiment validated that the angular velocity fusion method of MIMU array proposed in this paper can further improve the angular velocity estimation accuracy.

Table 6.

RMSE of the fused angular velocity by different fusion methods of the MIMU array.

RMSE (deg/s)	Axis	Single gyroscope	MLE	Optimized EKF
	X	5.06	2.55	1.86
	Y	4.82	3.11	2.57
	Z	0.52	0.26	0.18

RMSE: root mean square error; MLE: maximum likelihood estimation; EKF: extended Kalman filter; MIMU: microelectromechanical system inertial measurement unit.

Conclusion

In this study, the MIMU array is applied to provide the angular velocity for the vehicles, such as the multi-motor UAV and the car. In the MIMU array, the angular velocity can be measured not only by redundant MEMS gyroscopes but also by the accelerometers spatially distributed on the array, thus the accuracy can be improved and the range can be broadened simultaneously. To get more optimal estimation performance of the angular velocity, an optimized EKF with correlated system noises is proposed and demonstrated by the simulation experiments and kinematic vehicle experiments that the accuracy can be increased by about 1.5 times compared to the traditional MLE method.

This contribution has the following innovations: (1) According to the kinetic characteristics of the vehicles, an adaptive estimation model of the angular velocity is built as the state equation, and the signal models of the MIMU array are considered as the measurement equation, thus the angular rates with arbitrary magnitudes can be estimated; (2) By optimizing the traditional EKF equations, the angular velocity of MIMU array can be estimated even in the filter equations, where the state and measurement noises are correlated.

There are some further researches in the future work: (1) This method has to be demonstrated more adequately in the actual flight experiments of the UAV. Due to the limitation of experimental conditions and considering the reliability and safety of flight experiments on multi-rotor UAV, there are no conditions for flight experiments at present. The algorithm will be verified by more reliable experiments on the ground before flight experiments. (2) In the actual flight experiments, how to adjust the angular acceleration in the state equation according to the complicated maneuvers is a problem worth further analysis.