Optimization Algorithm for Kalman Filter Exploiting the Numerical Characteristics of SINS/GPS Integrated Navigation Systems

Abstract

Aiming at addressing the problem of high computational cost of the traditional Kalman filter in SINS/GPS, a practical optimization algorithm with offline-derivation and parallel processing methods based on the numerical characteristics of the system is presented in this paper. The algorithm exploits the sparseness and/or symmetry of matrices to simplify the computational procedure. Thus plenty of invalid operations can be avoided by offline derivation using a block matrix technique. For enhanced efficiency, a new parallel computational mechanism is established by subdividing and restructuring calculation processes after analyzing the extracted “useful” data. As a result, the algorithm saves about 90% of the CPU processing time and 66% of the memory usage needed in a classical Kalman filter. Meanwhile, the method as a numerical approach needs no precise-loss transformation/approximation of system modules and the accuracy suffers little in comparison with the filter before computational optimization. Furthermore, since no complicated matrix theories are needed, the algorithm can be easily transplanted into other modified filters as a secondary optimization method to achieve further efficiency.

1. Introduction

In many SINS/GPS integrated navigation systems, common system models cannot conform to the use conditions of classical Kalman filter due to the “colored noise”. To still be able to use the optimal estimation method, it is necessary to expand the system state model [1,2]. However, as the state dimensions increase, filtering computation will rapidly become expensive and unstable, and the so-called “dimension disaster” could even break out. In order to address this problem, scholars have reported various optimization algorithms which can lower the computational costs and/or enhance the numerical robustness. The typical optimization algorithms contain square-root information filtering SRIF [3], U-D decomposition filtering [4], singular value decomposition filtering [5] and their improved versions [6,7,8]. These modified filters, besides greatly reducing the time consumption, theoretically guarantee the positive definiteness of covariance and effectively avoid the numerical divergence, thus they are widely applied in the theoretical design of higher-order systems. Nevertheless, they require the support of relatively complex matrix theory in derivation and may pose certain difficulty in engineering. On the other hand, although these algorithms have decreased computational costs, their complexity is still O(n³). With further expansion of the state dimensions, the above decomposition algorithms, as a kind of general method, are also being challenged on their real-time capability. Recently, researchers have paid much attention to non-commonality methods, which are designed for some specific applications or based on specific models. For instance, aiming at integrated navigation applications, a kind of reduced-order Kalman filter (RDKF) [9,10,11,12] was promoted to ease the computational load. The idea of these filters is to reduce the model dimension by theoretical/engineering methods. As the filter dimension n is vital to computation time, the reduction of the state order will produce a direct benefit in terms of real time. However, the decrease of the state order will also bring partial accuracy damage, therefore, these methods may not be suitable for some high accuracy-demanding applications. Another kind of effective algorithm optimizes the float-point operations mainly by taking advantage of the sparse matrices (e.g., the transition matrix Φ_n in SINS/GPS) during the filtering computation [13,14,15]. In these algorithms, the so-called matrix accumulative method (MAM) or other online methods are used and the algorithm complexity can be simplified to O(s² – u²) (s/u represents the numbers of nonzero/1 elements in Φ_n). As the matrices in the high order model normally contain substantial numbers of zero elements, the computational time can be curtailed to an ideal level by usage of this sparse-matrix-based method. Meanwhile, being different from RDKF, the sparse-matrix-based methods do not change the system model and need little engineering approximation, thus they perform better in the accuracy-control aspect. But these methods also have their own limitations:

(1)

Though they avoid massive unnecessary multiplying-zero operations, the methods still need extra O(n³) times estimation of zero elements in each matrix multiplication;

(2)

Methods based on MAM, a kind of online method, could not seek any deeper optimization online and their efficiency is totally dependent on the number of zeros in the real-time matrix. To enhance the optimization efficiency, the usual way is to set more zero elements by approximation methods, but this comes at the cost of a certain accuracy loss.

In response to the above problems, we present here a new highly-efficient, accuracy-lossless, and engineering-tractable optimization algorithm based on offline derivation and a parallel method exploiting the numerical characteristics of SINS/GPS. In comparison with other algorithms, the proposed method offers numerous advantages: (i) in comparison with the general algorithms, our proposed method requires little complex deduction and is easily understandable; (ii) in comparison with the RDKF optimization method, the proposed method needs little engineering approximation and would be more accurate; and (iii) in comparison with the filter based on MAM, it is free of zero-estimation operations and can deduce some stronger conclusions, thus it is more efficient. Emphasizing on the engineering simplicity, we will introduce our optimization method revolving around the classical Kalman filter and a loosely-coupled model of SINS/GPS. It is apprised here that the chosen model does not imply any constraints on the application scope of our method. Actually, the offline and parallel method, as a pure numerical optimization method, is equally applicable to the extended filter and other complex SINS/GPS models if some similar research on the special model is done. On the other hand, since there is no transformation on any models or equations in our method (the two distinct differences with the normal filtering computations are: (i) unnecessary floating-point operations are directly avoided; and (ii) de-coupled operations are processed in parallel), the precision and robustness of our method would be equal to the classical Kalman filter in theory. Owing to this fact, the efficiency rather than robustness or accuracy is focused on in our discussion later. Section 2 gives a brief account of closed-loop Kalman filtering and a widely-used loosely-coupled model in SINS/GPS. In Section 3, the details of offline derivation and the parallel method based on a block-matrix technique are described. Section 4 and Section 5 deal with the statistics on computational costs and the filter performance on error estimation.

2. Loosely-Coupled SINS/GPS Model Using the Classical Kalman Filter

Depending on the different ways of integration, SINS/GPS are classified into two modes: (i) loose mode; and (ii) tight mode. Loose mode using GPS output to adjust SINS errors is opted for in this research for its distinctive features of high redundancy and easy realization. The emphasis is placed on a common kind of loose model i.e., position-velocity integration with 18 states and six observations.

2.1. State Equation

By using the indirect method, which treats the estimation variables as parametric errors instead of parameters themselves, the state equation is established in Equation (1):

$$\dot{\mathit{x}}\left(t\right)=\mathit{A}\left(t\right)\mathit{x}\left(t\right)+\mathit{G}\left(t\right)\mathit{\omega }\left(t\right)$$

(1)

where, $\mathit{x}\left(t\right)$ represents the state vector, including attitude errors $\mathit{\gamma }$, velocity errors $\delta \mathit{v}$, position errors ${\left(\begin{array}{ccc}\delta L & \delta \lambda & \delta h\end{array}\right)}^{\text{T}}$, and the “colored noise” errors: gyro constant drift ${\mathit{\epsilon }}_c$, Markov process drift ${\mathit{\epsilon }}_r$, and accelerometer drift ${\mathit{\nabla }}_a$; $\mathit{\omega }\left(t\right)$ denotes the collected system noise vector, including gyro drift white noise ${\mathit{\omega }}_g$, Markov driving white noise ${\mathit{\omega }}_r$, and accelerometer drift white noise ${\mathit{\omega }}_a$; $\mathit{A}\left(t\right)$ is state transition matrix and $\mathit{G}\left(t\right)$ is noise coefficient matrix. Combining references [16,17,18,19,20,21,22] with some modification, the variables of the state equation can be described by Equations (2)–(5):

$$\mathit{x}(t)={({\gamma }_x {\gamma }_y {\gamma }_z⋮\delta v_x \delta v_y \delta v_z⋮\delta L \delta \lambda \delta h⋮{\epsilon }_{cx} {\epsilon }_{cy} {\epsilon }_{cz}⋮{\epsilon }_{rx} {\epsilon }_{ry} {\epsilon }_{rz}⋮{\nabla }_{ax} {\nabla }_{ay} {\nabla }_{az})}^{\text{T}}$$

(2)

$$\mathit{\omega }\left(t\right)={({\omega }_{gx} {\omega }_{gy} {\omega }_{gz}⋮{\omega }_{rx} {\omega }_{ry} {\omega }_{rz}⋮{\omega }_{ax} {\omega }_{ay} {\omega }_{az})}^{\text{T}}$$

(3)

$$\mathit{A}(t)={\left[\begin{array}{cccccc}{\mathit{A}}_{11}& {\mathit{A}}_{12}& {\mathit{A}}_{13}& {\mathit{C}}_B^N& {\mathit{C}}_B^N& {\mathit{O}}_{3\times 3}\\ {\mathit{A}}_{21}& {\mathit{A}}_{22}& {\mathit{A}}_{23}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{C}}_B^N\\ {\mathit{O}}_{3\times 3}& {\mathit{A}}_{32}& {\mathit{A}}_{33}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{A}}_{IMU22}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{A}}_{IMU33}\end{array}\right]}_{18\times 18}$$

(4)

$$\mathit{G}\left(t\right)={\left[\begin{array}{ccc}{\mathit{C}}_B^N& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]}_{18\times 9}$$

(5)

In matrices (4) and (5), all blocks are 3-order square matrices. Blocks ${\mathit{O}}_{3\times 3}$, ${\mathit{I}}_{3\times 3}$, and ${\mathit{C}}_B^N$ stand for the zero matrix, the identity matrix, and the attitude matrix, respectively. The other blocks are described in detail in [16,17,18,19,20,21,22]. In particular, we highlight here two kinds of nonzero blocks ${\mathit{A}}_{i3(i=1,2,3)}$ and ${\mathit{A}}_{IMUii(i=2,3)}$ of which the special single-nonzero-vector and diagonal structure may contribute to deeper optimization:

$${\mathit{A}}_{i3}=\left[\begin{array}{ccc}*& 0& 0\\ *& 0& 0\\ *& 0& 0\end{array}\right],i=1,2,3\hspace{0.17em}{\mathit{A}}_{IMUii}=\left[\begin{array}{ccc}*& 0& 0\\ 0& *& 0\\ 0& 0& *\end{array}\right],i=2,3$$

(6)

In addition, the variance $\mathit{Q}\left(t\right)$ of noise vector $\mathit{\omega }\left(t\right)$ is described as:

$$\mathit{Q}\left(t\right)={\left[\begin{array}{ccc}{\mathit{Q}}_{11}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{22}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{33}\end{array}\right]}_{9\times 9}$$

(7)

where, block ${\mathit{Q}}_{ii(i=1,2,3)}$ is also diagonal.

2.2. Measurement Equation

We adopt a position-velocity integration model and define the filter observation vector $\mathit{z}\left(t\right)$ [16,22]:

$${\mathit{z}}_{obs}\left(t\right)=\left[\begin{array}{c}\left(L_{INS}-L_{GPS}\right)R_n\\ ({\lambda }_{INS}-{\lambda }_{GPS})R_e\cos L\\ h_{INS}-h_{GPS}\\ v_{xINS}-v_{xGPS}\\ v_{yINS}-v_{yGPS}\\ v_{zINS}-v_{zGPS}\end{array}\right]=\left[\begin{array}{c}{R}_n\delta L+N_x\\ R_e\cos L\delta \lambda +N_y\\ \delta h+N_z\\ \delta v_x+M_x\\ \delta v_y+M_y\\ \delta v_z+M_z\end{array}\right]$$

(8)

Then, we define the measurement equation as follows:

$$\mathit{z}\left(t\right)\triangleq \mathit{H}\left(t\right)\mathit{x}\left(t\right)+\mathit{n}\left(t\right)$$

(9)

where $\mathit{n}\left(t\right)$ represents the measurement noise vector; $\mathit{R}\left(t\right)$ denotes the corresponding variance matrix, and $\mathit{H}\left(t\right)$ is the coefficient matrix. $\mathit{n}\left(t\right)$, $\mathit{R}\left(t\right)$ and $\mathit{H}\left(t\right)$ are described as follows according to [16,17,18,19,20,21,22]:

$$\mathit{n}\left(t\right)={\left(N_x,N_y,N_z,M_x,M_y,M_z\right)}^{\text{T}}$$

(10)

$$\mathit{R}\left(t\right)=\text{diag}\left\{{\sigma }_{px}^2,{\sigma }_{py}^2,{\sigma }_{pz}^2,{\sigma }_{vx}^2,{\sigma }_{vy}^2,{\sigma }_{vz}^2\right\}=\left[\begin{array}{cc}{\mathit{R}}_{11}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{R}}_{22}\end{array}\right]$$

(11)

$$\mathit{H}\left(t\right)={\left[\begin{array}{cccccc}{\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{H}}_{13}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\end{array}\right]}_{6\times 18}$$

(12)

where:

$${\mathit{H}}_{13}=diag\left\{R_n ,R_e\cos L,1\right\}$$

(13)

2.3. Closed-Loop Kalman Filtering

In order to fulfill the requirement of closed-loop Kalman filtering, the state equation (Equation (1)) and measurement equation (Equation (9)) are changed to the discrete forms:

$${\mathit{x}}_n={\mathit{\varphi }}_{\hspace{0.17em}n}{\mathit{x}}_{n-1}+{\mathit{u}}_{cn}+{\mathbf{\Gamma }}_n{\mathit{\omega }}_n$$

(14)

$${\mathit{z}}_n={\mathit{H}}_n{\mathit{x}}_n+{\mathit{n}}_n$$

(15)

where, the transition matrix ${\mathit{\varphi }}_{\hspace{0.17em}n}$ is converted from $\mathit{A}\left(t\right)$; the noise driving matrix ${\mathbf{\Gamma }}_n$ is converted from $\mathit{G}\left(t\right)$; the coefficient ${\mathit{H}}_n$ is converted from $\mathit{H}\left(t\right)$; and the control vector ${\mathit{u}}_{cn}={({\mathit{\gamma }}_c^{\mathrm{T}}⋮\mathit{\delta }{\mathit{v}}_c^{\mathrm{T}}⋮\delta L_c \delta {\lambda }_c \delta h_c⋮{\mathit{\epsilon }}_{cc}^{\mathrm{T}}⋮{\mathit{\epsilon }}_{rc}^{\mathrm{T}} ⋮{\nabla }_{ac}^{\mathrm{T}})}_c^{\mathrm{T}}$ is added for real-time calibration on the parameters of SINS.

After discretization, the update of ${\mathit{x}}_n$ is done recursively as follows:

$$\begin{array}{l}{\tilde{\mathit{x}}}_n(-)={\mathit{\varphi }}_{\hspace{0.17em}n}{\tilde{\mathit{x}}}_{n-1}\left({+}_c\right)\\ {\tilde{\mathit{z}}}_n={\mathit{H}}_n{\tilde{\mathit{x}}}_n(-)\\ {\tilde{\mathit{x}}}_n\left({+}_e\right)={\tilde{\mathit{x}}}_n(-)+{\mathit{K}}_n({\mathit{z}}_{obsn}-{\tilde{\mathit{z}}}_n)\\ {\tilde{\mathit{x}}}_n\left({+}_c\right)={\tilde{\mathit{x}}}_n\left({+}_e\right)+{\mathit{u}}_{cn}\end{array}$$

(16)

where, “~” represents the parameter needed estimation; $(-)$ represents the value before estimation; $\left({+}_e\right)$ represents the value after estimation; $\left({+}_c\right)$ represents the value after calibration with the vector ${\mathit{u}}_{cn}$; ${\mathit{z}}_{obsn}$ represents real observation vector; and ${\mathit{K}}_n$ represents Kalman filtering gain.

To minimize the filtering error, state vector ${\tilde{\mathit{x}}}_n\left({+}_c\right)$ is permanently set to zero:

$${\tilde{\mathit{x}}}_n\left({+}_c\right)\equiv \mathbf{0}$$

(17)

Substitution of Equation (17) into Equation (16), yields:

$${\mathit{u}}_{cn}=-{\tilde{\mathit{x}}}_n\left({+}_e\right)=-{\mathit{K}}_n{\mathit{z}}_{obsn}$$

(18)

Apparently, ${\mathit{u}}_{cn}$ becomes the final output of the Kalman filter with control vector. As ${\mathit{z}}_{obsn}$ is almost directly obtained by measurement, ${\mathit{K}}_n$ appears to be the key of ${\mathit{u}}_{cn}$ and even the filter. According to the Kalman filtering theory, ${\mathit{K}}_n$ can be recursively computed as follows:

$${\mathit{P}}_n(-)={\mathit{\varphi }}_{\hspace{0.17em}n}{\mathit{P}}_{n-1}\left({+}_e\right){\mathit{\varphi }}_{\hspace{0.17em}n}^{\mathrm{T}}+{\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$$

(19)

$${\mathit{K}}_n={\mathit{P}}_n(-){\mathit{H}}_n^{\mathrm{T}}{\left({\mathit{H}}_n{\mathit{P}}_n(-){\mathit{H}}_n^{\mathrm{T}}+{\mathit{R}}_n\right)}^{-1}$$

(20)

$${\mathit{P}}_n\left({+}_e\right)=\left(\mathit{I}-{\mathit{K}}_n{\mathit{H}}_n\right){\mathit{P}}_n(-)$$

(21)

where ${\mathit{P}}_n$ is the covariance; ${\mathit{Q}}_n$ and ${\mathit{R}}_n$ are the discretized forms of $\mathit{Q}\left(t\right)$ and $\mathit{R}\left(t\right)$ respectively. In fact, Equations (18)–(21) constitute the main process of the closed-loop Kalman filter. This process is generally subdivided into two processes: time propagation Equation (19) and measurement updating Equations (18), (20) and (21). In summary, the process flow of Kalman filter in SINS/GPS is depicted in Figure 1. Since most matrices involved in Figure 1 or Equations (19)–(21) are either 18 × 18 or 18 × 6 matrices, there will be a large-scale and time-consuming computation in each recursive cycle. Therefore, a certain optimization on the computation would be essential to the real-time properties of SINS/GPS.

Figure 1. Iterative closed-loop Kalman filtering process.

Figure 1. Iterative closed-loop Kalman filtering process.

3. Computational Optimization

Consistent with Figure 1, optimization are sequentially implemented in the updating processes of ${\mathit{\varphi }}_{\hspace{0.17em}n}$, ${\mathbf{\Gamma }}_n{\mathbf{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$, ${\mathit{P}}_n(-)$, ${\mathit{K}}_n$, ${\mathit{P}}_n\left({+}_e\right)$, where the most computational time are consumed.

3.1. Compute ${\mathit{\varphi }}_{\hspace{0.17em}n}$

According to [16,17,18,19,20,21,22], we can calculate ${\mathit{\varphi }}_{\hspace{0.17em}\mathit{n}}$ as follows:

$${\mathit{\varphi }}_{\hspace{0.17em}n}=e^{{\displaystyle {\mathrm{\int }}_{t_{n-1}}^{t_n}\mathit{A}\left(t\right)d}t}\approx \mathit{I}+T_n{\mathit{A}}_n+\frac{T_n^2}{2!}{\mathit{A}}_n^2+\frac{T_n^3}{3!}{\mathit{A}}_n^3$$

(22)

Here, the 3rd order Taloy equation is selected for mainly two reasons:

(1) Equation (22) meets the requirements of most mid-high-accuracy systems;

(2) Equation (22) can deduce the universal optimization conclusion.

It’s not difficult to prove that ${\mathit{\varphi }}_{\hspace{0.17em}n}$ would have a uniform numerical structure independent from which of the 3rd and higher order equation is chosen. On the other hand, lower order equations are obviously a special case of 3rd order equation. Therefore, the conclusion based on 3rd equation is suitable for the model based on the different order of equations too.

As the computation of Equation (22) is centered on the 18 × 18 matrix ${\mathit{A}}_n=\mathit{A}\left(t_n\right)$, it is necessary to inspect the numerical characteristics of ${\mathit{A}}_n$ before computational optimization: matrix ${\mathit{A}}_n$, in which only 64 of the total 324 elements are non-zero, is clearly a sparse matrix. A substantial reduction of useless multiplying-zero operations can be achieved by directly expanding ${\mathit{A}}_n$ and operating every element in deduction (namely “direct derivation”), but because of the high order, direct derivation is quite complicated to handle and is not advisable in practice. Fortunately, the 3-order-block form of ${\mathit{A}}_n$, having 23 zero blocks among the total of 36 blocks (see Equation (4)), is still a sparse matrix structurally. Therefore, we can carry out our offline optimization using an indirect method, in which the blocks instead of the elements are treated as the atomic unit in deduction. In this way, we can largely decrease the derivation complexity and retain most of the efficiency of a direct method at the same time. For these reasons, the indirect method is applied to all the derivation processes in this paper. In addition, the nonzero blocks ${\mathit{A}}_{i3}$ and ${\mathit{A}}_{IMUii}$ (see Equation (6)), which need few operations in multiplication by any 3-order block (no more than 3² float-pointing multiplications and 2 × 3 additions), are also introduced to the derivation for higher efficiency.

It is a fact that manual derivation with indirect method is still a cumbersome job in view of multiple matrix multiplications (e.g., ${\mathit{A}}_n^2$, ${\mathit{A}}_n^3$), so we handle our derivation by a computer program instead. With the powerful symbolic-computation function of some well-known math software, e.g., MATLAB and Maple, the complicated derivation can be easily accomplished. After necessary programming, the final derivation results are shown in Equations (23) and (24):

$${\mathit{A}}_n^2={\left[\begin{array}{cccccc}{\mathit{S}}_{11}& {\mathit{S}}_{12}& {\mathit{S}}_{13}& {\mathit{S}}_{14}& {\mathit{S}}_{15}& {\mathit{S}}_{16}\\ {\mathit{S}}_{21}& {\mathit{S}}_{22}& {\mathit{S}}_{23}& {\mathit{S}}_{24}& {\mathit{S}}_{25}& {\mathit{S}}_{26}\\ {\mathit{S}}_{31}& {\mathit{S}}_{32}& {\mathit{S}}_{33}& {\mathit{O}}_{3\times 1}& {\mathit{O}}_{3\times 3}& {\mathit{S}}_{36}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{S}}_{55}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{S}}_{66}\end{array}\right]}_{18\times 18}$$

(23)

$${\mathit{A}}_n^3={\left[\begin{array}{cccccc}{\mathit{T}}_{11}& {\mathit{T}}_{12}& {\mathit{T}}_{13}& {\mathit{T}}_{14}& {\mathit{T}}_{15}& {\mathit{T}}_{16}\\ {\mathit{T}}_{21}& {\mathit{T}}_{22}& {\mathit{T}}_{23}& {\mathit{T}}_{24}& {\mathit{T}}_{25}& {\mathit{T}}_{26}\\ {\mathit{T}}_{31}& {\mathit{T}}_{32}& {\mathit{T}}_{33}& {\mathit{T}}_{34}& {\mathit{T}}_{35}& {\mathit{T}}_{36}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{T}}_{55}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{T}}_{66}\end{array}\right]}_{18\times 18}$$

(24)

Also, the computation of blocks ${\mathit{S}}_{ij}$ and ${\mathit{T}}_{ij}$ is described in a computerized sequence as follows:

$$\begin{array}{l}{\mathit{S}}_{ij}={\displaystyle \sum _{\mathit{k}=1}^3{\mathit{A}}_{ik}{\mathit{A}}_{kj}},\hspace{0.17em}\hspace{0.17em}i,j\text{ = }1,2,3\\ {\mathit{S}}_{ij}={\mathit{A}}_{i(j/2-1)}{\mathit{C}}_B^N,\hspace{0.17em}\hspace{0.17em}i\text{ = }1,2,3; \text{j = }4,6\\ {\mathit{S}}_{i5}={\mathit{S}}_{i4},\hspace{0.17em}\hspace{0.17em}i,j=1,2\\ {\mathit{S}}_{15}={\mathit{S}}_{15}+ {\left({\mathit{C}}_B^N{\mathit{A}}_{IMU22}\right)}_{Sim}\\ {\mathit{S}}_{26}={\mathit{S}}_{26}+ {\left({\mathit{C}}_B^N{\mathit{A}}_{IMU33}\right)}_{Sim}\\ {\mathit{S}}_{55}={\left({\mathit{A}}_{IMU22}^2\right)}_{Sim}\\ {\mathit{S}}_{66}={\left({\mathit{A}}_{IMU33}^2\right)}_{Sim}\\ {\mathit{T}}_{ij}={\displaystyle \sum _{\mathit{k}=1}^3{\mathit{A}}_{ik}{\mathit{S}}_{kj}},\hspace{0.17em}\hspace{0.17em}i=1,2,3; j=1,2,\dots ,6\\ {\mathit{T}}_{26}={\mathit{T}}_{26}+ {\left({\mathit{C}}_B^N{\mathit{A}}_{IMU33}^2\right)}_{Sim}\\ {\mathit{T}}_{55}={\left({\mathit{A}}_{IMU22}{\mathit{S}}_{55}\right)}_{Sim}\\ {\mathit{T}}_{66}={\left({\mathit{A}}_{IMU33}{\mathit{S}}_{66}\right)}_{Sim}\end{array}$$

(25)

whereas all products involving block ${\mathit{A}}_{31}$, ${\mathit{S}}_{34}$ or ${\mathit{S}}_{35}$ are zero blocks ${\mathit{O}}_{3\times 3}$ and need no real-time computations; products involving the special blocks, e.g., ${\mathit{A}}_{i3}$, ${\mathit{S}}_{i3}$ ( single-nonzero –vector block), ${\mathit{A}}_{IMUii}$, ${\mathit{S}}_{55}$, or ${\mathit{S}}_{66}$ (diagonal blocks), are simplified and are partly indicated in symbol ${\left(\right)}_{Sim}$.

Substituting of ${\mathit{A}}_n$ Equation (4), ${\mathit{A}}_n^2$ Equation (23), and ${\mathit{A}}_n^3$ Equation (24) in ${\mathit{\varphi }}_{\hspace{0.17em}n}$ Equation (22), yields:

$${\mathit{\varphi }}_{\hspace{0.17em}n}={\left[\begin{array}{cccccc}{\mathit{\varphi }}_{\hspace{0.17em}11}& {\mathit{\varphi }}_{\hspace{0.17em}12}& {\mathit{\varphi }}_{\hspace{0.17em}13}& {\mathit{\varphi }}_{\hspace{0.17em}14}& {\mathit{\varphi }}_{\hspace{0.17em}15}& {\mathit{\varphi }}_{\hspace{0.17em}16}\\ {\mathit{\varphi }}_{\hspace{0.17em}21}& {\mathit{\varphi }}_{\hspace{0.17em}22}& {\mathit{\varphi }}_{\hspace{0.17em}23}& {\mathit{\varphi }}_{\hspace{0.17em}24}& {\mathit{\varphi }}_{\hspace{0.17em}25}& {\mathit{\varphi }}_{\hspace{0.17em}26}\\ {\mathit{\varphi }}_{\hspace{0.17em}31}& {\mathit{\varphi }}_{\hspace{0.17em}32}& {\mathit{\varphi }}_{\hspace{0.17em}33}& {\mathit{\varphi }}_{\hspace{0.17em}34}& {\mathit{\varphi }}_{\hspace{0.17em}35}& {\mathit{\varphi }}_{\hspace{0.17em}36}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{\varphi }}_{\hspace{0.17em}55}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{\varphi }}_{\hspace{0.17em}66}\end{array}\right]}_{18\times 18}$$

(26)

whereas:

$$\begin{array}{l}{\mathit{\varphi }}_{\hspace{0.17em}ij}=T_n{\mathit{A}}_{ij}+\frac{T_n^2}{2!}{\mathit{S}}_{ij}+\frac{T_n^3}{3!}{\mathit{T}}_{ij},i\in \left[1,3\right],j\in \left[1,6\right],i\ne j\\ {\mathit{\varphi }}_{\hspace{0.17em}ii}={\mathit{I}}_{3\times 3}+T_n{\mathit{A}}_{ii}+\frac{T_n^2}{2!}{\mathit{S}}_{ii}+\frac{T_n^3}{3!}{\mathit{T}}_{ii},\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i=1,2,3,5,6\end{array}$$

(27)

Here, ${\mathit{\varphi }}_{\hspace{0.17em}13}$ and ${\mathit{\varphi }}_{\hspace{0.17em}23}$ are single-nonzero-column blocks; ${\mathit{\varphi }}_{\hspace{0.17em}33}$ is the sum of ${\mathit{I}}_{3\times 3}$ and single-nonzero-column block; ${\mathit{\varphi }}_{\hspace{0.17em}55}$ and ${\mathit{\varphi }}_{\hspace{0.17em}66}$ are diagonal blocks. These blocks would be specially used in other processes.

In conclusion, with offline derivation based on 3rd order block method, nearly half amount of blocks ${\mathit{\varphi }}_{\hspace{0.17em}ij}$ is proved to be constant blocks: ${\mathit{O}}_{3\times 3}$ or ${\mathit{I}}_{3\times 3}$ (see Equation (26)), thus, the computations on them are simply eliminated. Additionally, by using the properties of special blocks ${\mathit{A}}_{i3}$, ${\mathit{S}}_{i3}$, ${\mathit{T}}_{i3}$, ${\mathit{A}}_{jj}$, ${\mathit{S}}_{jj}$, ${\mathit{T}}_{jj}$ and introducing the intermediate variables ${\mathit{A}}_n^2$, ${\mathit{A}}_n^3$, real-time computations on the other half can be further reduced.

3.2. Compute ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^T$

According to [16,17,18,19,20,21,22], the 2nd order approximate of ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ are computed as follows:

$${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}=\frac{T_n}{2}\left({\mathit{Q}}_{1n}+{\mathit{\varphi }}_{\hspace{0.17em}n}{\mathit{Q}}_{1n}{\mathit{\varphi }}_{\hspace{0.17em}n}^{\mathrm{T}}\right)$$

(28)

where:

$${\mathit{Q}}_{1n}=\mathit{G}\left(t\right)\mathit{Q}\left(t\right)\mathit{G}{\left(t\right)}^{\mathrm{T}}={\left[\begin{array}{cccc}{\mathit{C}}_B^N{\mathit{Q}}_{11}{\left({\mathit{C}}_B^N\right)}^{\mathrm{T}}& {\mathit{O}}_{3\times 9}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{9\times 3}& {\mathit{O}}_{9\times 9}& {\mathit{O}}_{9\times 3}& {\mathit{O}}_{9\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 9}& {\mathit{Q}}_{22}& {\mathit{O}}_{3\times 3}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 9}& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{33}\end{array}\right]}_{18\times 18}$$

(29)

Obviously, ${\mathit{Q}}_{1n}$ and ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ are both symmetric. Thus, for matrix ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$, only its upper (or lower) triangle elements need to be computed in real time. Substitution of ${\mathit{Q}}_{1n}$ (Equation (29)) in ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ (Equation (28)), yields:

$${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}=\frac{T_n}{2}{\left[\begin{array}{cccccc}{\mathit{Q}}_{n11}& {\mathit{Q}}_{n12}& {\mathit{Q}}_{n13}& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{n15}& {\mathit{Q}}_{n16}\\ {\mathit{Q}}_{n12}^T& {\mathit{Q}}_{n22}& {\mathit{Q}}_{n23}& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{n25}& {\mathit{Q}}_{n26}\\ {\mathit{Q}}_{n13}^T& {\mathit{Q}}_{n23}^T& {\mathit{Q}}_{n33}& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{n35}& {\mathit{Q}}_{n36}\\ {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}\\ {\mathit{Q}}_{n15}^T& {\mathit{Q}}_{n25}^T& {\mathit{Q}}_{n35}^T& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{n55}& {\mathit{O}}_{3\times 3}\\ {\mathit{Q}}_{n16}^T& {\mathit{Q}}_{n26}^T& {\mathit{Q}}_{n36}^T& {\mathit{O}}_{3\times 3}& {\mathit{O}}_{3\times 3}& {\mathit{Q}}_{n66}\end{array}\right]}_{18\times 18}$$

(30)

where:

$$\begin{array}{l}{\mathit{Q}}_{nij}=\frac{T_n}{2}\left[{\mathit{\varphi }}_{\hspace{0.17em}i1}{\mathit{Q}}_{11}^{\prime }{\mathit{\varphi }}_{\hspace{0.17em}j1}^{\mathrm{T}}+{\mathit{\varphi }}_{\hspace{0.17em}i5}{\left({\mathit{Q}}_{22}{\mathit{\varphi }}_{\hspace{0.17em}j5}^{\mathrm{T}}\right)}_{Sim}+{\mathit{\varphi }}_{\hspace{0.17em}i6}{\left({\mathit{Q}}_{33}{\mathit{\varphi }}_{\hspace{0.17em}j6}^{\mathrm{T}}\right)}_{Sim}\right],i,j=1,2,3;i\le j\\ {\mathit{Q}}_{n11}={\mathit{Q}}_{n11}+\frac{T_n}{2}{\mathit{Q}}_{11}^{\prime }\\ {\mathit{Q}}_{nij}=\frac{T_n}{2}{\left({\mathit{\varphi }}_{\hspace{0.17em}ij}{\mathit{Q}}_{j-3,j-3}{\mathit{\varphi }}_{\hspace{0.17em}jj}\right)}_{Sim},i=1,2,3;j=5,6\\ {\mathit{Q}}_{n55}=\frac{T_n}{2}{\left({\mathit{Q}}_{22}+{\mathit{\varphi }}_{\hspace{0.17em}55}{\mathit{Q}}_{22}{\mathit{\varphi }}_{\hspace{0.17em}55}\right)}_{Sim}\\ {\mathit{Q}}_{n66}=\frac{T_n}{2}{\left({\mathit{Q}}_{33}+{\mathit{\varphi }}_{\hspace{0.17em}66}{\mathit{Q}}_{33}{\mathit{\varphi }}_{\hspace{0.17em}66}\right)}_{Sim}\end{array}$$

(31)

In Equation (31), ${\mathit{Q}}_{n55}$ and ${\mathit{Q}}_{n66}$ are apparently diagonal blocks that may be used to simplify other processes. As stated above, we optimize the computation of ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ mainly by applying some special numerical properties to the offline derivation (e.g., the sparsity of $\mathit{G}\left(t\right)$, the simplicity of diagonal blocks ${\mathit{Q}}_{22}$, ${\mathit{Q}}_{33}$, ${\mathit{\varphi }}_{\hspace{0.17em}55}$, ${\mathit{\varphi }}_{\hspace{0.17em}66}$, and the symmetry of ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$) and much of the computation time is saved.

3.3. Compute ${\mathit{P}}_n(-)$

For simplicity, ${\mathit{P}}_n(-)$ and ${\mathit{P}}_n\left({+}_e\right)$ will be written in a unified form ${\mathit{P}}_n$ (this is allowed because of that ${\mathit{P}}_n(-)$ and ${\mathit{P}}_n\left({+}_e\right)$ are actually the same covariance matrices in different state and share the same memories). Similar to ${\mathit{\varphi }}_{\hspace{0.17em}n}$ or ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$, ${\mathit{P}}_n$ is written in 3rd-order-block-matrix form:

$${\mathit{P}}_n={\left[\begin{array}{cccc}{\mathit{P}}_{n,11}& {\mathit{P}}_{n,12}& \cdots & {\mathit{P}}_{n,16}\\ {\mathit{P}}_{n,21}& {\mathit{P}}_{n,22}& \cdots & {\mathit{P}}_{n,26}\\ ⋮& ⋮& \ddots & ⋮\\ {\mathit{P}}_{n,61}& {\mathit{P}}_{n,62}& \cdots & {\mathit{P}}_{n,66}\end{array}\right]}_{18\times 18}$$

(32)

According to its definition, ${\mathit{P}}_n$ is a symmetric matrix:

$${\mathit{P}}_{n,ij}={\mathit{P}}_{n,ji}^{\mathrm{T}}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em},\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i,j=1,2\dots 6$$

(33)

Thus, only the lower (or upper) triangle part needs substantive computation. Regardless of ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ at first, Equation (19) is written as:

$${\mathit{P}}_n(-)={\mathit{\varphi }}_{\hspace{0.17em}n}{\mathit{P}}_{n-1}\left({+}_e\right){\mathit{\varphi }}_{\hspace{0.17em}n}^{\mathrm{T}}$$

(34)

Substitution of Equation (26) and (32), yields:

$$\begin{array}{l}{\mathit{P}}_{n,11}={\displaystyle \sum _{i=1}^6\left\{{\mathit{\varphi }}_{\hspace{0.17em}1i}{\left[{\displaystyle \sum _{j=1}^6\left({\mathit{P}}_{n-1,ij}{\mathit{\varphi }}_{\hspace{0.17em}1j}^{\mathrm{T}}\right)}\right]}_{Temp}\right\}}\\ {\mathit{P}}_{n,21}={\displaystyle \sum _{i=1}^6\left\{{\mathit{\varphi }}_{\hspace{0.17em}2i}{\left[{\displaystyle \sum _{j=1}^6\left({\mathit{P}}_{n-1,ij}{\mathit{\varphi }}_{\hspace{0.17em}1j}^{\mathrm{T}}\right)}\right]}_{Temp}\right\}}\\ ⋮\\ {\mathit{P}}_{n,61}={\left[{\mathit{\varphi }}_{\hspace{0.17em}66}{\displaystyle \sum _{j=1}^6{\left({\mathit{P}}_{n-1,6j}{\mathit{\varphi }}_{\hspace{0.17em}1j}^{\mathrm{T}}\right)}_{Rep}}\right]}_{Sim}\\ {\mathit{P}}_{n,22}={\displaystyle \sum _{i=1}^6\left\{{\mathit{\varphi }}_{\hspace{0.17em}2i}{\left[{\displaystyle \sum _{j=1}^6\left({\mathit{P}}_{n-1,ij}{\mathit{\varphi }}_{\hspace{0.17em}2j}^{\mathrm{T}}\right)}\right]}_{Temp}\right\}}\\ {\mathit{P}}_{n,32}={\displaystyle \sum _{i=1}^6\left\{{\mathit{\varphi }}_{\hspace{0.17em}3i}{\left[{\displaystyle \sum _{j=1}^6\left({\mathit{P}}_{n-1,ij}{\mathit{\varphi }}_{\hspace{0.17em}2j}^{\mathrm{T}}\right)}\right]}_{Temp}\right\}}\\ ⋮\\ {\mathit{P}}_{n,62}={\left[{\mathit{\varphi }}_{\hspace{0.17em}66}{\displaystyle \sum _{j=1}^6{\left({\mathit{P}}_{n-1,6j}{\mathit{\varphi }}_{\hspace{0.17em}2j}^{\mathrm{T}}\right)}_{Rep}}\right]}_{Sim}\\ ⋮\\ {\mathit{P}}_{n,66}={\left({\mathit{\varphi }}_{\hspace{0.17em}66}{\mathit{P}}_{n-1,66}{\mathit{\varphi }}_{\hspace{0.17em}66}\right)}_{Sim}\end{array}$$

(35)

where the symbol ${\left(\right)}_{Temp}$ represents intermediate items that would be temporarily stored in memory to avoid repeated computation; the symbol ${\left(\right)}_{Rep}$ represents the repeated item that have been computed before and are available by directly accessing ${\left(\right)}_{Temp}$ and the symbol ${\left(\right)}_{Sim}$ indicates that the special properties of ${\mathit{\varphi }}_{\hspace{0.17em}i3}^{\mathrm{T}}$ are exploited to perform further optimization.

After Equation (35) is done, ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ should be added. Substitution of Equations (30) and (32) in Equation (19), yields:

$${\mathit{P}}_{n,ij}={\mathit{P}}_{n,ij}+{\mathit{Q}}_{n,ij},i,j=1,2,3,5,6;i\ge j$$

(36)

Above all, the computation of ${\mathit{P}}_n(-)$ is divided into two steps: Equations (34) and (36). As the latter has few computations, optimization mainly focuses on the former. Employing the sparsity of ${\mathit{\varphi }}_{\hspace{0.17em}n}$, the symmetry of ${\mathit{P}}_n(-)$ and the repeatability of intermediate items, the real time of calculating Equation (34) is greatly improved.

3.4. Compute ${\mathit{K}}_n$

Substitution of Equations (11), (12) and (32) in the inversion part of Equation (20), yields:

$${\mathit{H}}_n{\mathit{P}}_n(-){\mathit{H}}_n^{\mathrm{T}}+{\mathit{R}}_n={\left[\begin{array}{cc}{\mathit{H}}_{13}{\mathit{P}}_{n,33}{\mathit{H}}_{13}+{\mathit{R}}_{11}& {\mathit{H}}_{13}{\mathit{P}}_{n,23}^{\mathrm{T}}\\ {\mathit{P}}_{n,23}{\mathit{H}}_{13}& {\mathit{P}}_{n,22}+{\mathit{R}}_{22}\end{array}\right]}_{6\times 6}$$

(37)

Since ${\mathit{H}}_n{\mathit{P}}_n(-){\mathit{H}}_n^T+{\mathit{R}}_n$ has a low order, few computations are needed during inversion (the complexity is $\mathit{O}(6^3)$ that can be neglected in comparison with other procedures) and the details of inversion are not discussed accordingly. Here, we use the symbol $\mathit{D}$ to represent the result of inversion directly:

$$\mathit{D}={\left[{\mathit{H}}_n{\mathit{P}}_n(-){\mathit{H}}_n^{\mathrm{T}}+{\mathit{R}}_n\right]}^{-1}$$

(38)

Matrix $\mathit{D}$ is easily proved to be symmetric and can be written in the 3-order-block-matrix form as:

$$\mathit{D}={\left[\begin{array}{cc}{\mathit{D}}_{11}& {\mathit{D}}_{12}\\ {\mathit{D}}_{12}^T& {\mathit{D}}_{22}\end{array}\right]}_{6\times 6}$$

(39)

Then ${\mathit{K}}_n$ can be computed as:

$${\mathit{K}}_n={\left[\begin{array}{cc}{\mathit{P}}_{n,12}{\mathit{D}}_{12}^{\mathrm{T}}+{\mathit{P}}_{n,13}{\mathit{H}}_{13}{\mathit{D}}_{11}& {\mathit{P}}_{n,12}{\mathit{D}}_{22}+{\mathit{P}}_{n,13}{\mathit{H}}_{13}{\mathit{D}}_{12}\\ {\mathit{P}}_{n,22}{\mathit{D}}_{12}^{\mathrm{T}}+{\mathit{P}}_{n,23}{\mathit{H}}_{13}{\mathit{D}}_{11}& {\mathit{P}}_{n,22}{\mathit{D}}_{22}+{\mathit{P}}_{n,23}{\mathit{H}}_{13}{\mathit{D}}_{12}\\ ⋮& ⋮\\ {\mathit{P}}_{n,62}{\mathit{D}}_{12}^{\mathrm{T}}+{\mathit{P}}_{n,63}{\mathit{H}}_{13}{\mathit{D}}_{11}& {\mathit{P}}_{n,62}{\mathit{D}}_{22}+{\mathit{P}}_{n,63}{\mathit{H}}_{13}{\mathit{D}}_{12}\end{array}\right]}_{18\times 6}$$

(40)

Define

$${\mathit{K}}_n$$

as:

$${\mathit{K}}_n={\left[\begin{array}{cc}{\mathit{K}}_{11}& {\mathit{K}}_{12}\\ {\mathit{K}}_{21}& {\mathit{K}}_{22}\\ ⋮& ⋮\\ {\mathit{K}}_{61}& {\mathit{K}}_{62}\end{array}\right]}_{18\times 6}$$

(41)

then:

$${\mathit{K}}_{ij}={\mathit{P}}_{n,i2}{\mathit{D}}_{j2}^{\mathrm{T}}+{\mathit{P}}_{n,i3}{\mathit{H}}_{13}{\mathit{D}}_{1j},i=1,2,\dots ,6;j=1,2$$

(42)

The derivation result shows that, by using the block-matrix optimization technique, only a simple process (Equation (37), (38) and (42)) is needed in ${\mathit{K}}_n$ updating. Compared with the complicated Equation (20) which needs multiple 18-order matrix production, our method seems much more simple and efficient in computing.

3.5. Compute ${\mathit{P}}_n\left({+}_e\right)$

Transform Equation (21) into:

$${\mathit{P}}_n\left({+}_e\right)={\mathit{P}}_n(-)-{\mathit{K}}_n{\mathit{H}}_n{\mathit{P}}_n(-)$$

(43)

Obviously, the pressure in computation mainly comes from ${\mathit{K}}_n{\mathit{H}}_n{\mathit{P}}_n(-)$. Fortunately, ${\mathit{K}}_n{\mathit{H}}_n{\mathit{P}}_n(-)$ is a symmetrical matrix (this is an evident inference according to Equation (43) in the condition that ${\mathit{P}}_n\left({+}_e\right)$, ${\mathit{P}}_n(-)$ are both symmetrical) so that only its upper (or lower) triangular part needs real-time updating. Define ${\mathit{K}}_n{\mathit{H}}_n{\mathit{P}}_n(-)$ as the variance’s increment $\Delta {\mathit{P}}_n$:

$$\Delta {\mathit{P}}_n={\mathit{K}}_n{\mathit{H}}_n{\mathit{P}}_n(-)={\left[\begin{array}{cccc}\Delta {\mathit{P}}_{11}& \Delta {\mathit{P}}_{12}& \cdots & \Delta {\mathit{P}}_{16}\\ \Delta {\mathit{P}}_{21}& \Delta {\mathit{P}}_{22}& \cdots & \Delta {\mathit{P}}_{26}\\ ⋮& ⋮& \ddots & ⋮\\ \Delta {\mathit{P}}_{61}& \Delta {\mathit{P}}_{62}& \cdots & \Delta {\mathit{P}}_{66}\end{array}\right]}_{18\times 18}$$

(44)

Substitution of Equations (12), (32) and (41), yields:

$$\Delta {\mathit{P}}_{ij}={\mathit{K}}_{i1}{\mathit{H}}_{13}{\mathit{P}}_{n,3j}+{\mathit{K}}_{i2}{\mathit{P}}_{n,2j}\hspace{0.17em},i,j=1,2,\dots ,6;i\le j$$

(45)

then:

$${\mathit{P}}_{n,ij}={\mathit{P}}_{n,ij}-\Delta {\mathit{P}}_{ij}\hspace{0.17em},i,j=1,2,\dots ,6;i\le j$$

(46)

In conclusion, taking advantage of the symmetry of ${\mathit{P}}_n\left({+}_e\right)$ and the sparsity of ${\mathit{H}}_n$, we deduct a simple form of computing ${\mathit{P}}_n\left({+}_e\right)$: Equations (45) and (46). Equation (46) is apparently costless as only the matrix subtraction is involved. Equation (45) involves matrix products, but requires no computations on most blocks of ${\mathit{P}}_n$ (except the blocks in 2nd and 3rd rows) and proves to be highly efficient.

3.6. Parallel Computation

The optimization results (see Table 2, Table 3, Table 4, Table 5 and Table 6) reveal that, computation on ${\mathit{P}}_n(-)$ costs nearly half of the processing time and becomes a bottleneck for further efficiency. The low efficiency mainly arises from that ${\mathit{P}}_n(-)$ becomes increasingly dense and can provide few zero blocks after initial filtering cycles. In low-accuracy applications, some researchers may force the some matrix elements to be zero for low computational costs with the engineering approximate method. However, to some accuracy-demanded applications, methods with more accuracy and efficiency are still looked for. Parallel computation is one of the feasible approaches.

Before discussing the parallel method, we perform an inspection of the whole computation processes in order to clarify the dependency of every parameter on ${\mathit{P}}_n(-)$. The numerical relationship between ${\mathit{P}}_n(-)$ and other parameters can be consulted in Table 1.

Except for ${\mathit{P}}_n\left({+}_e\right)$, all parameters are either irrelevant to ${\mathit{P}}_{n,ij}$ or only relevant to 2nd and 3rd block rows (columns) of ${\mathit{P}}_n(-)$. Taking this fact into account, we can call 2nd and 3rd block rows (columns) “useful” data and treat the rest as “useless” (it is emphasized here that “useless” only refers to the requirement of most computation processes, but not that the data is really useless and needs no updates).

Furthermore, because ${\mathit{P}}_n(-)$ is a symmetrical matrix, the “useful” blocks are only 2nd, 3rd block columns in fact. If “ $\times$ ” represents “useless” blocks, ${\mathit{P}}_n(-)$ can be written as:

$${\mathit{P}}_n={\left[\begin{array}{cccccc}\times & {\mathit{P}}_{n,12}& {\mathit{P}}_{n,31}& \times & \times & \times \\ \times & {\mathit{P}}_{n,22}& {\mathit{P}}_{n,32}& \times & \times & \times \\ \times & {\mathit{P}}_{n,32}& {\mathit{P}}_{n,33}& \times & \times & \times \\ \times & {\mathit{P}}_{n,42}& {\mathit{P}}_{n,34}& \times & \times & \times \\ \times & {\mathit{P}}_{n,52}& {\mathit{P}}_{n,35}& \times & \times & \times \\ \times & {\mathit{P}}_{n,62}& {\mathit{P}}_{n,36}& \times & \times & \times \end{array}\right]}_{18\times 18}$$

(47)

Table 1. Dependency of every real-time parameter on ${\mathit{P}}_n(-)$ blocks.

${\mathit{\varphi }}_{\mathit{n}}$, ${\mathit{Q}}_{\mathit{n}}$, ${\mathit{P}}_{\mathit{n}}\mathbf{(}\mathbf{-}\mathbf{)}$	${\mathit{K}}_{\mathit{n}}$ (Equations (40) and (41))	${\mathit{P}}_{\mathit{n}}\mathbf{\left(}{\mathbf{+}}_{\mathit{e}}\mathbf{\right)}$ (Equation (43))	$\mathbf{\Delta }{\mathit{P}}_{\mathit{i}\mathit{j}}$ (Equation (45))
Irrelevant to ${\mathit{P}}_{n,ij}$	Dependent on ${\mathit{P}}_{n,i2}$, ${\mathit{P}}_{n,i3}$ only	Dependent on ${\mathit{P}}_{n,ij}$	Dependent on ${\mathit{P}}_{n,2i}$, ${\mathit{P}}_{n,3i}$ only

Table 1. Dependency of every real-time parameter on ${\mathit{P}}_n(-)$ blocks.

${\mathit{\varphi }}_{\mathit{n}}$, ${\mathit{Q}}_{\mathit{n}}$, ${\mathit{P}}_{\mathit{n}}\mathbf{(}\mathbf{-}\mathbf{)}$	${\mathit{K}}_{\mathit{n}}$ (Equations (40) and (41))	${\mathit{P}}_{\mathit{n}}\mathbf{\left(}{\mathbf{+}}_{\mathit{e}}\mathbf{\right)}$ (Equation (43))	$\mathbf{\Delta }{\mathit{P}}_{\mathit{i}\mathit{j}}$ (Equation (45))
Irrelevant to ${\mathit{P}}_{n,ij}$	Dependent on ${\mathit{P}}_{n,i2}$, ${\mathit{P}}_{n,i3}$ only	Dependent on ${\mathit{P}}_{n,ij}$	Dependent on ${\mathit{P}}_{n,2i}$, ${\mathit{P}}_{n,3i}$ only

In measurement updating (Equations (18), (20) and (21)), all parameters except ${\mathit{P}}_n\left({+}_e\right)$ are decoupled with the “useless” data (Equation (19)) which belong to the time propagation. Moreover, to the coupled ${\mathit{P}}_n\left({+}_e\right)$ its time-consuming part $\Delta {\mathit{P}}_{ij}$ is also independent of the “useless” blocks. In other words, with some appropriate modification of measurement updating and time propagation, we can produce a new decoupling mechanism of the two procedures. The modification contains three steps:

• Step1. Subdivide time propagation.
• Classify the blocks of ${\mathit{P}}_n(-)$ into two kinds: “useful” data and “useless” data;
• Step 2. Subdivide measurement updating.
• Subdivide ${\mathit{P}}_n\left({+}_e\right)$ into two processes: computing $\Delta {\mathit{P}}_{ij}$ (Equation (45)) and adding ${\mathit{P}}_n(-)$ (Equation (46));
• Step 3. Restructure measurement updating.
• Restructure measurement updating process as $\Delta {\mathit{P}}_{ij}$ (instead of ${\mathit{P}}_n\left({+}_e\right)$) (Equations (18) and (21)).

With the above steps, the restructured process and the “useless” data updating process are completely numerically decoupled and can be computed in parallel. In this way, the efficiency bottleneck in ${\mathit{P}}_n(-)$ updating is broken.

Furthermore, in order to achieve the parallel computation on the “useful” data as well, we similarly subdivide the process of updating “useful” data into two steps: computing ${\mathit{\varphi }}_{\hspace{0.17em}n}{\mathit{P}}_{n-1}\left({+}_e\right){\mathit{\varphi }}_{\hspace{0.17em}n}^{\mathrm{T}}$ (Equation (34)) and adding ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ (Equation (36)). Apparently, computing ${\mathit{\varphi }}_{\hspace{0.17em}n}{\mathit{P}}_{n-1}\left({+}_e\right){\mathit{\varphi }}_{\hspace{0.17em}n}^T$ and updating ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ (Equation (31), which belongs to time propagation) can be processed in parallel. In summary, the process of Kalman filtering with parallel computation is described as Figure 2.

It is pointed out here that the parallel method in this paper has a fundamental difference with some other parallel methods [23,24]. The others owe their decoupling to the so-called “mandatory delay”, which may cause certain accuracy damage. On the contrary, the decoupling in this paper needs neither “mandatory delay” nor engineering approximation and is achieved mainly by introducing the “useful” data and subdividing the computation process. Essentially, it is the numerical characteristic of SINS/GPS that brings up this special decoupling. Therefore, the parallel processing in this paper is an accuracy-lossless method.

In respect of efficiency, the parallel optimization method can reduce much execution time needed in updating both ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ and the “useless” blocks (including 918 multiplications and 603 additions in updating ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$, and 1089 multiplications and 1107 additions in updating “useless” blocks). In comparison with non-parallel methods, efficiency of parallel method increases by about 25%.

Figure 2. Filtering process with parallel computation.

Figure 2. Filtering process with parallel computation.

4. Optimization Efficiency

According to the derivation result, we can figure out the optimization efficiency by counting and comparing the computational counts needed in both the proposed method and the existing algorithm. In detail, the discussion about counts proceeds on each parameter updating procedure, respectively. As matrix multiplication is mainly involved, the calculation will focus on the floating-point operations of multiplication and addition. Table 2, Table 3, Table 4, Table 5 and Table 6 will show the statistical details of each parameter. Then, Table 7 and Table 8 give the global efficiency and the comparison with other general algorithms, respectively. As shown in the tables, the offline-derivation and parallel method saves about 90% of computational time and 66% of memory space, while keeping the required accuracy level. Compared with general decomposition optimization algorithms, the proposed method needs no complicated matrix theory but producing higher efficiency. Under the condition that high hardware precision has guaranteed the numerical robustness, our algorithm appears quite simple and practical.

Table 2. Optimization efficiency of ${\mathit{\varphi }}_{\hspace{0.17em}n}$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$42\text{ × }{3}^3\text{ + }36\times 3^2\text{ + }13\text{ × }3\text{ × }{3}^2=1809$ (I), 10%	$3\text{ × }{18}^3\text{ + }3\times {18}^2\text{ = }18468$ (II)
Addition	$42\text{ × }2\text{ × }{3}^2+9\text{ × }2\text{ × }3+13\text{ × }2\text{ × }{3}^2=1044$ (I), 6%	$3\text{ × }17\text{ × }{18}^2\text{ + }3\text{ × }{18}^2\text{ = }17496$ (II)
Memory	$32\text{ × }{3}^2\text{ × }8\text{ Byte}=2.3\text{ KB}$ (III)	$3\text{ × }{18}^2\text{ × }8\mathrm{Byte}=\text{7.6 KB}$ (III)

Table 2. Optimization efficiency of ${\mathit{\varphi }}_{\hspace{0.17em}n}$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$42\text{ × }{3}^3\text{ + }36\times 3^2\text{ + }13\text{ × }3\text{ × }{3}^2=1809$ (I), 10%	$3\text{ × }{18}^3\text{ + }3\times {18}^2\text{ = }18468$ (II)
Addition	$42\text{ × }2\text{ × }{3}^2+9\text{ × }2\text{ × }3+13\text{ × }2\text{ × }{3}^2=1044$ (I), 6%	$3\text{ × }17\text{ × }{18}^2\text{ + }3\text{ × }{18}^2\text{ = }17496$ (II)
Memory	$32\text{ × }{3}^2\text{ × }8\text{ Byte}=2.3\text{ KB}$ (III)	$3\text{ × }{18}^2\text{ × }8\mathrm{Byte}=\text{7.6 KB}$ (III)

In (I), 42 × 3³ (42 × 2 × 3²), 36 × 3² (9 × 2 × 3) and 13 × 3 × 3² (13 × 2 × 3²) represent the counts on multiplication (addition) of items in Equation (25) without special blocks, of items in Equation (25) with special block and of items in Equation (27) respectively, and only 3² & 3³ items are counted; In (II), the first item represents the counts on operations of 18-order-matrix multiplication, the second item represents that of multiplication by scalars or matrix addition; In (III), data are assumed to be double-precision (8-byte word length); the percentages represent the efficiency in comparison with the traditional method.

Table 3. Optimization efficiency of ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$24\times 3^3+18\times 3^2+12\times 3^2=918$, 6%	$2\times {18}^3+{18}^2+18\times 6^2+6\times {18}^2=14580$
Addition	$24\times 2\times 3^2+9\times 2\times 3+13\times 3^2=603$, 4%	$2\times 17\times {18}^2+{18}^2+5\times 6\times 18+17\times 18\times 6=13716$
Memory	$13\times 3^2\times 8\text{ Byte}=0.9\text{ KB}$	$3 \times {18}^2\text{ × }8\text{ Byte}=7.6\text{ KB}$

Table 3. Optimization efficiency of ${\mathbf{\Gamma }}_n{\mathit{Q}}_n{\mathbf{\Gamma }}_n^{\mathrm{T}}$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$24\times 3^3+18\times 3^2+12\times 3^2=918$, 6%	$2\times {18}^3+{18}^2+18\times 6^2+6\times {18}^2=14580$
Addition	$24\times 2\times 3^2+9\times 2\times 3+13\times 3^2=603$, 4%	$2\times 17\times {18}^2+{18}^2+5\times 6\times 18+17\times 18\times 6=13716$
Memory	$13\times 3^2\times 8\text{ Byte}=0.9\text{ KB}$	$3 \times {18}^2\text{ × }8\text{ Byte}=7.6\text{ KB}$

Table 4. Optimization efficiency of ${\mathit{P}}_n(-)$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$120\times 3^3+37\times 3^2=3573$, 31%	$2\times {18}^3=11664$
Addition	$120\times 2\times 3^2+18\times 2\times 3+134\times 3^2=3474$, 31%	$2\times 17\times {18}^2+{18}^2=11340$
Memory	$60\times 3^2\times 8\text{ Byte}=4.2 \text{KB}$	$3\times {18}^2\times 8 \text{Byte}=7.6 \text{KB}$

Table 4. Optimization efficiency of ${\mathit{P}}_n(-)$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$120\times 3^3+37\times 3^2=3573$, 31%	$2\times {18}^3=11664$
Addition	$120\times 2\times 3^2+18\times 2\times 3+134\times 3^2=3474$, 31%	$2\times 17\times {18}^2+{18}^2=11340$
Memory	$60\times 3^2\times 8\text{ Byte}=4.2 \text{KB}$	$3\times {18}^2\times 8 \text{Byte}=7.6 \text{KB}$

Table 5. Optimization efficiency of ${\mathit{K}}_n$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$24\times 3^3+6\times 3^2=702$, 11%	$18\text{ × }18\text{ × }6\text{ + }18\text{ × 6 × }6\text{ + }6\text{ × }18 \times 18\text{ + }6\text{ × }18\text{ × }6\text{ = }5184$
Addition	$24\times 2\times 3^2+12\times 3^2=540$, 11%	$17\text{ × }18\text{ × }6\text{ + }17\text{ × }6\text{ × }6\text{ + }5\text{ × }18\text{ × }6\text{ + }17\text{ × }18\text{ × }6=4824$
Memory	$16 \times 3^2 \times 8 \text{Byte}= 1.1 \text{KB}$	$（3 \times 6 \times 18+6 \times 6）\times 8 \text{Byte} = 2.8 \text{KB}$

Table 5. Optimization efficiency of ${\mathit{K}}_n$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$24\times 3^3+6\times 3^2=702$, 11%	$18\text{ × }18\text{ × }6\text{ + }18\text{ × 6 × }6\text{ + }6\text{ × }18 \times 18\text{ + }6\text{ × }18\text{ × }6\text{ = }5184$
Addition	$24\times 2\times 3^2+12\times 3^2=540$, 11%	$17\text{ × }18\text{ × }6\text{ + }17\text{ × }6\text{ × }6\text{ + }5\text{ × }18\text{ × }6\text{ + }17\text{ × }18\text{ × }6=4824$
Memory	$16 \times 3^2 \times 8 \text{Byte}= 1.1 \text{KB}$	$（3 \times 6 \times 18+6 \times 6）\times 8 \text{Byte} = 2.8 \text{KB}$

Table 6. Optimization efficiency of ${\mathit{P}}_n\left({+}_e\right)$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$42\times 3^3+6\times 3^2=1188$, 15%	$6\times 18\times 18+{18}^3=7776$
Addition	$42\times 2\times 3^2+42\times 3^2=1134$, 15%	$5\times {18}^2+17\times {18}^2+{18}^2=7452$
Memory	$16\times 3^2\times 8 \text{Byte}=1.1 \text{KB}$	$（3\times 6\times 18+6\times 6）\times 8 \text{Byte}=2.8 \text{KB}$

Table 6. Optimization efficiency of ${\mathit{P}}_n\left({+}_e\right)$ updating process.

	Offline-Derivation Method	Traditional Method
Multiplication	$42\times 3^3+6\times 3^2=1188$, 15%	$6\times 18\times 18+{18}^3=7776$
Addition	$42\times 2\times 3^2+42\times 3^2=1134$, 15%	$5\times {18}^2+17\times {18}^2+{18}^2=7452$
Memory	$16\times 3^2\times 8 \text{Byte}=1.1 \text{KB}$	$（3\times 6\times 18+6\times 6）\times 8 \text{Byte}=2.8 \text{KB}$

Table 7. Optimization efficiency of offline-derivation and parallel method.

	Offline-Derivation & Parallel Method	Traditional Method
Multiplication	$1809+2484+702+1188=6183$ (IV), 10.7%	$18468+14580+11664+5184+7776=57672$
Addition	$1044+2475+540+1134=5193$ (IV), 9.5%	$17496+13716+11340+4824+7452=54828$
Memory	$2.3+0.9+4.2+1.1+1.1=9.6 \text{KB}$	$7.6+7.6+7.6+2.8+2.8=28.4 \text{KB}$

In (IV), 2484 (2475) is the operation times of “useful” blocks updating.

Table 7. Optimization efficiency of offline-derivation and parallel method.

	Offline-Derivation & Parallel Method	Traditional Method
Multiplication	$1809+2484+702+1188=6183$ (IV), 10.7%	$18468+14580+11664+5184+7776=57672$
Addition	$1044+2475+540+1134=5193$ (IV), 9.5%	$17496+13716+11340+4824+7452=54828$
Memory	$2.3+0.9+4.2+1.1+1.1=9.6 \text{KB}$	$7.6+7.6+7.6+2.8+2.8=28.4 \text{KB}$

In (IV), 2484 (2475) is the operation times of “useful” blocks updating.

Table 8. Comparison of offline-derivation & parallel method and general optimization filters.

	Multiplication	Addition
SRIF filtering	$\frac{7}{6}\times {18}^3+36\times {18}^2=18468$ (V)	$\frac{7}{6}\times {18}^3+36\times {18}^2=18468$ (V)
U-D decomposing filtering	$\frac{1}{2}\times {18}^3+\frac{1}{2}\times {18}^2+10\times {18}^2+8\times 18=6462$ (VI)	$\frac{1}{2}\times {18}^3+\frac{1}{2}\times {18}^2+9\times {18}^2+9\times 18=6156$ (VI)
SVD filtering	$26\times {18}^3+78\times {18}^2=176904$ (VII)	$26\times {18}^3+78\times {18}^2=176904$ (VII)
offline-derivation and parallel method	$2484+702+1188=4374$ (VIII)	$2475+540+1134=4149$ (VIII)

Table 8. Comparison of offline-derivation & parallel method and general optimization filters.

	Multiplication	Addition
SRIF filtering	$\frac{7}{6}\times {18}^3+36\times {18}^2=18468$ (V)	$\frac{7}{6}\times {18}^3+36\times {18}^2=18468$ (V)
U-D decomposing filtering	$\frac{1}{2}\times {18}^3+\frac{1}{2}\times {18}^2+10\times {18}^2+8\times 18=6462$ (VI)	$\frac{1}{2}\times {18}^3+\frac{1}{2}\times {18}^2+9\times {18}^2+9\times 18=6156$ (VI)
SVD filtering	$26\times {18}^3+78\times {18}^2=176904$ (VII)	$26\times {18}^3+78\times {18}^2=176904$ (VII)
offline-derivation and parallel method	$2484+702+1188=4374$ (VIII)	$2475+540+1134=4149$ (VIII)

(V) is explained in Reference [3], while (VI) in Reference [25], Reference [26] and (VII) in Reference [5]; Point out here that, (VI) relies on the precondition that ${\mathit{\varphi }}_{\hspace{0.17em}n}$ is already an upper-triangle matrix in 9-order-block form (what is the conclusion of offline derivation), otherwise, 9702 multiplications and 9396 additions are actually needed; concerning (VIII), the procedure of computing ${\mathit{\varphi }}_{\hspace{0.17em}n}$ is no longer counted as no optimizations are against ${\mathit{\varphi }}_{\hspace{0.17em}n}$ in the general algorithms.

5. Simulation

To evaluate the method validity, we design a practical loosely-coupled SINS/GPS program in CCS v5.5 (a well-known programming tool on DSP platform). The program comprises three modules:

(1)

Sensor data sampling module.

(2)

SINS algorithm module. This module is designed to implement the calculation of attitude, velocity and position independently (only the inertial sensors data is needed).

(3)

Kalman filter module. This part is for data fusion. The system module is designed according to Section 2, and the computation process is programmed using the offline and parallel method described in Section 3. Besides, for accuracy evaluation, the classical KF is also designed as a control group.

With the developed program, we can simulate filtering process and evaluate the performance of our method in CCS. The simulation parameters and conditions are set as follows:

(1)

DSP TMS320C6722, a famous float-point CPU, is chosen in CCS as the computing device.

(2)

The carrier of the navigation system is assumed to be static. With this assumption, the gyro/accelerometer would detect no valid rotate/velocity rate aside from devices noise. Therefore, noise is considered the sampling data driving the SINS/GPS program. As the carrier is static, velocity/position should stay at the zero/initial value in theory, thus, this assumption can largely facilitate the evaluation of program results.

(3)

Velocity/position measured by GPS is constantly set:

$$v_{GPS}\equiv {\left(0,0,0\right)}^T,\left(L\mathsf{\lambda }h \right){ }_{GPS}^T\equiv {\left(\begin{array}{ccc}0.6977686& 2.0306152& 90.0\end{array}\right)}^T$$

Velocity/position calculated by SINS is initialized:

$$v_{SINS}={\left(0,0,0\right)}^T,\left(L\lambda h \right){ }_{SINS}^T={\left(\begin{array}{ccc}0.6977688370723981& 2.030615370778482& 91.0\end{array}\right)}^T$$

(4)

IMU data rate (noise frequency): 100 Hz; SINS velocity/position updating rate: 50/20 Hz; filtering rate: 10 Hz.

With the above setting, we run the program in CCS for 100 s and record the navigation outputs (e.g., the X-axis velocity and latitude). The simulation results are illustrated in Figure 3 and Figure 4.

Figure 3. Latitude calculation by different methods.

Figure 3. Latitude calculation by different methods.

Figure 4. Velocity calculation by different methods.

Figure 4. Velocity calculation by different methods.

The results show that the proposed method effectively restrains the unbounded error accumulation in SINS and keep the estimated variables around their mean values. On the aspect of estimation performance, our method is nearly at par to the classical KF (the proposed-method curve does not completely overlap the classical-KF one because of less calculation and less truncation errors). This conclusion agrees with the theoretical analysis: our numerical method, needing no modification of the system module and no engineering approximation, causes little damage to the estimation accuracy, but from the point of view of real-time computation, our method is much more efficient than the classical KF. In the DSP program, the proposed method needs 64,820 system clocks in each filtering cycle while classical KF needs 390,480 clocks. If a clock frequency of 200 MHz, the typical frequency of a DSP TMS320C6722 is chosen, then our method will cost only 324 μs in each filtering operation. Such a level of real-time efficiency is highly valuable to the many accurate navigation applications.

6. Conclusions

Starting from a common classical-Kalman-fitler-based SINS/GPS model (position-speed integration with 18 states and six measurements), we present an optimization algorithm based on the system’s numerical characteristics. The algorithm employs a block-matrix technique in offline derivation, where special blocks (e.g., zero blocks, diagonal blocks, etc.) are used to simplify the calculation. In this way, plenty of invalid multiplying by zero and repeated operations are avoided offline. Furthermore, aiming at the time-consuming update of ${\mathit{P}}_n(-)$, a novel parallel method is implemented by defining “useful” data and subdividing computational process, and the entire filtering procedure is greatly accelerated. The statistical analysis and simulation results reveal that the offline algorithm, coupled with the parallel method. can reduce the CPU processing time by 90% and the memory usage by 66%. Compared with several general decomposition-optimization algorithms, the proposed method requires no complex matrix theory rather more efficiency. The distinguished feature of the algorithm is that no modification of the system module and no engineering approximation are needed, thus it causes little harm to the base filter. It is pointed out that although the derivation is based on a specific integration model, the algorithm as a complete numerical optimization approach can be transplanted to other advanced models of SINS/GPS or other extended filters. With the powerful symbolic-operation function of the MATLAB program, researchers can manipulate formulas involving high order matrices in an easy way. Consequently, the proposed method is an engineering-tractable approach with high efficiency and high precision for SINS/GPS integrated navigation systems.