CN103940444B - 一种mimu组网式系统级标定方法 - Google Patents

Abstract

本发明提出一种MIMU组网式系统级标定方法。对量产后同批次MEMS惯性传感器构建的批量MIMU，建立MEMS惯性传感器的误差模型，通过多机动方式对由每个MIMU构建的捷联式惯性导航系统进行内环滤波估计获得内环滤波估计值；然后每个MIMU与其邻近多个MIMU之间进行内环滤波估计值交换，并根据自身内环滤波估计值和邻近多个MIMU的内环滤波估计值进行外环滤波估计，获得每个MIMU中MEMS惯性传感器误差模型参数，实现批量MIMU的标定。本发明克服了采用传统分立式标定方法需要依赖高精度转台以及系统级标定方法只能得到有限参数的缺陷。

Description

一种MIMU组网式系统级标定方法

技术领域

本发明属于惯性技术领域，具体涉及一种MIMU组网式系统级标定方法。

背景技术

在惯性技术领域中，标定技术是一项不可或缺的核心技术。建立精确的惯性器件的数学误差模型，通过软件算法对模型误差项进行补偿可以提高惯性导航系统的精度。常见的数学误差模型的误差项有加速度计的零位偏置、陀螺仪的随机漂移、标度因数误差以及安装误差。现有的标定技术分为分立式标定方法和系统级标定方法。

分立式标定方法是采用专用标定设备（一般是指具有北向和水平基准的转台）提供的角速率信息和重力加速度信息，将安装在转台上的微小型惯性测量单元（MIMU）中加速度计和陀螺仪的输出作为建模信息，依据最小二乘的原理，实现对标定参数的辨识，从而建立数学误差模型。目前分立式标定方法的理论研究较为成熟，进行多位置实验和速率实验即可完成标定。但分立式标定方法的标定精度受到MIMU中传感器的随机噪声影响，需要对同一系统重复多次标定测试并对多次标定参数取均值，才能使得标定模型参数误差的方差与惯性传感器误差的方差趋于一致。因此，传统的分立式标定方法的标定过程复杂，标定周期长，无法满足MIMU工程化的需求，同时，分立式标定无法在一次标定过程中获取同批次惯性传感器的多个样本数据，而从统计学的角度来说，传感器误差样本数量偏少，会导致标定获得的模型与实际使用过程中的真实模型不一致。因此，对于批量MIMU来说，采用分立式标定方法会存在标定时间长、标定模型参数误差不稳定的问题。

系统级标定方法是基于惯性导航系统解算误差的原理进行的，惯性导航系统进入导航状态后，其参数误差经由导航解算会传递到导航结果中去，表现为导航参数误差，若能够获取导航参数误差的全部或者部分信息，就能够实现惯性导航系统标定模型参数的估计。一般而言，系统级标定方法的算法相对复杂，但是能够抑制标定过程中的测量噪声，对转台依赖程度低。目前系统级标定方法主要有两种思路，一种是基于拟合的方法，另外一种是基于参数滤波估计的方法。

基于参数拟合思路的方法中需要建立特定的运动激励从而获得导航系统误差（如位置、速度等误差）与传感器模型参数之间的关系，观测导航误差，拟合估计系统误差参数，常采用最小二乘法进行拟合。这种方法中需要专用转台、北向基准等基准设备提供特定的运动激励，标定周期长，步骤相对繁琐。

基于参数滤波思路的方法中，一般设计滤波器时将惯性器件的模型参数作为滤波器的状态量，通过观测导航误差，滤波估计各标定模型参数项，常采用Kalman滤波器进行状态估计。这种方法通常只能够估计出加速度计的零位偏置和陀螺仪的零位漂移，因此可以获得的标定模型参数有限。

对批量的MIMU进行标定时，若采用分立式标定方法，需要通过专用转台或者其他高精度的基准设备进行多位置实验和速率实验来完成，若采用系统级标定方法，就必须解决标定的模型参数有限的问题。

发明内容

本发明提出一种MIMU组网式系统级标定方法，解决了基于MEMS惯性传感器构建的MIMU批量标定问题，克服了采用传统分立式标定方法需要依赖高精度转台以及系统级标定方法只能得到有限参数的缺陷。

为了解决上述技术问题，本发明提供一种MIMU组网式系统级标定方法对量产后同批次MEMS惯性传感器构建的批量MIMU，建立MEMS惯性传感器的误差模型，通过多机动方式对由每个MIMU构建的捷联式惯性导航系统进行内环滤波估计获得内环滤波估计值；然后每个MIMU与其邻近多个MIMU之间进行内环滤波估计值交换，并根据自身内环滤波估计值和邻近多个MIMU的内环滤波估计值进行外环滤波估计，获得每个MIMU中MEMS惯性传感器误差模型参数，实现批量MIMU的标定。

本发明与现有技术相比，其显著优点在于：（1）本发明中通过对测试平台采用多种机动方式进行激励（加速、减速、转弯、直行等），从捷联式惯性导航方法解算的结果中估计出模型参数，将MIMU的模型参数标定问题转化为模型参数的估计，因此无需专业转台支持，对标定设备要求降低，并缩短了批量MIMU的标定周期；（2）本发明能够实现在一次标定过程中对多个基于同批次MEMS惯性传感器的MIMU进行标定，从统计学的角度讲，可以同时获取同批次多个惯性传感器的输出，满足这一批传感器同一时刻的概率分布特性，有利于MIMU标定模型的稳定性，实现标定后的MIMU输出误差概率与惯性传感器误差概率一致；（3）本发明在对MIMU的模型参数进行估计时采用内环滤波估计环节和外环滤波估计环节，提高了单个MIMU系统级标定时单内环滤波估计环节的估计精度。

附图说明

图1为使用本发明方法进行模型参数标定时多个MIMU在测试平台上的安装示意图。

图2为本发明中各数据流向示意图。

图3为本发明中内环滤波估计的计算流程图。

图4为本发明中外环滤波估计的计算流程图。

具体实施方式

本发明为一种MIMU组网式系统级标定方法，对量产后同批次MEMS惯性传感器构建的批量MIMU，建立MEMS惯性传感器的误差模型，通过多机动方式（例如：加速、减速、转弯、直行等）对由每个MIMU构建的捷联式惯性导航系统进行内环滤波估计获得内环滤波估计值，即获得MIMU标定模型参数的初步估计值；然后每个MIMU与其邻近多个MIMU之间进行内环滤波估计值交换，并根据自身内环滤波估计值和邻近多个MIMU的内环滤波估计值进行外环滤波估计，即利用邻近的多个MIMU的内环滤波估计信息和观测信息对其模型参数的初步估计值进行修正，获得每个MIMU中MEMS惯性传感器误差模型的参数的最优估计，从而实现批量MIMU的标定。

本发明方法具体包括以下步骤：

步骤一、如图1所示，将待测MIMU分别安置于同一刚性测试平台内的安装基准上，调整各个待测MIMU的基准面与安装基准吻合；在中心计算机中建立MEMS惯性传感器的误差模型；

步骤二、根据预先设定的采样周期、滤波周期以及测试平台的机动方式（机动方式可以包括加速、减速、转弯、直行运动等）对测试平台进行激励，在测试平台机动过程中采集每个MIMU中MEMS惯性传感器的输出值并将输出值送入中心计算机；

步骤三、中心计算机中对MEMS惯性传感器的输出值采用捷联式惯性导航方法进行导航解算，获得MIMU的速度、位置、姿态；并建立内环滤波估计环节的系统状态方程和系统测量方程；

步骤四、中心计算机进行内环滤波估计获得每个MIMU的内环滤波估计值，内环滤波估计值包括每个MIMU的内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值以及内环滤波估计环节的状态估计值；然后，如图2所示，在每个MIMU与其邻近的多个MIMU之间交换内环滤波估计环节的状态量一步预测值、一步 预测均方误差、滤波增益值；

步骤五、中心计算机根据每个MIMU获得的邻近多个MIMU的内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值，对每个MIMU进行外环滤波估计，获得每个MIMU中MEMS惯性传感器误差模型参数的精确估计值；

步骤六、中心计算机将误差模型参数的精确估计值返回给各个MIMU，使用测试平台的运动激励验证每个MIMU中MEMS惯性传感器误差模型参数精确估计值的准确度，并计算MEMS惯性传感器误差模型参数的误差方差；

步骤七、将步骤六获得的每个MIMU中MEMS惯性传感器误差模型参数的误差方差与预先设定的误差方差阈值进行比较，若误差方差大于误差方差阈值，则返回步骤三；若误差方差不大于误差方差阈值，则把MIMU外环滤波估计获得的模型参数的精确估计值作为每个MIMU中MEMS惯性传感器误差模型的标定结果，完成对批量MIMU的标定。

上述步骤二中，在对测试平台进行激励之前，可以对MIMU通电预热30分钟，设定采样周期为0.01秒。

上述步骤四中进行内环滤波估的滤波器为Kalman滤波器；滤波器的滤波模型包括由MIMU构建的捷联式惯性导航系统的内环滤波估计环节的状态方程和观测方程；

所述内环滤波估计环节的状态方程的状态量包括：捷联式惯性导航方法解算的速度误差、位置误差、姿态误差；待标定的MIMU中的MEMS加速度计的零位偏置、标度因数误差、安装误差以及MEMS陀螺仪的常值漂移、标度因数误差和安装误差。捷联式惯性导航方法解算的速度误差、位置误差、姿态误差是指将MIMU输出的角速率和比力送入捷联式惯性导航方法，解算获得MIMU的速度、位置、姿态与MIMU真实的速度、位置、姿态的差值，是一种待估计量。

所述内环滤波估计环节的观测方程的观测量包括：MIMU的速度误差、位置误差。MIMU的速度、位置误差是指将MIMU输出的角速率和比力送入捷联式惯性导航方法，解算获得MIMU的速度、位置与测试平台所提供的位置、速度激励的差值。

捷联式惯性导航方法的详细计算过程可以参见袁信所著《捷联式惯性导航原理》一书。

上述步骤五中的外环滤波估计的滤波模型包括由MIMU构建的捷联式惯性导航系统外环滤波估计环节的状态方程和观测方程；

所述外环滤波估计环节的状态方程的状态量与内环滤波估计环节的状态方程的状 态量相同；

所述外环滤波估计环节的观测方程的观测量包括：各个MIMU内环滤波估计环节的观测量和邻近的多个MIMU内环滤波估计环节的观测量。

上述过程中，所述中心计算机为嵌入式处理器、可移动终端、笔记本电脑中的一种。

上述过程中，每个MIMU与其邻近的四个MIMU之间交换内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值。

一、MEMS惯性传感器的误差模型：

本发明所述捷联式惯性导航方法中所用的MEMS惯性传感器的误差模型包括加速度计误差模型和陀螺仪误差模型：

(a)加速度计误差模型一般如公式（1）所示：

${\tilde{f}}_b=(I+[{\mathrm{\δK}}_A\left]\right)(I+[\mathrm{\δA}\left]\right)f^b+{\▿}^b---\left(1\right)$

公式（1）中，为MIMU中加速度计的比力输出矢量，I为单位矩阵，δK_A为MIMU中加速度计的标度因数误差矩阵，δA为加速度计的安装误差系数矩阵，f^b为系统的真实比力输入矢量，▽^b为MIMU中加速度计的零位偏置矢量。将公式（1）展开即获得本发明使用的加速度计误差模型，如公式（2）所示，

$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}1+{\mathrm{\δKA}}_x& {\mathrm{\δA}}_z& -{\mathrm{\δA}}_y\\ {-\mathrm{\δA}}_z& 1+{\mathrm{\δKA}}_y& {\mathrm{\δA}}_x\\ {\mathrm{\δA}}_y& {-\mathrm{\δA}}_x& 1+{\mathrm{\δKA}}_z\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{c}{A}_{x0}\\ A_{y0}\\ A_{z0}\end{array}\right]---\left(2\right)$

公式（2）中，A_x、A_y、A_z分别为MIMU中x、y、z三个轴向加速度计的比力输出；δKA_x、δKA_y、δKA_z分别为MIMU中x、y、z三个轴向加速度计的标度因数误差；δA_x、δA_y、δA_z分别为MIMU中x、y、z三个轴向加速度计的安装误差系数；a_x、a_y、a_z分别表示x、y、z三个轴向真实的比力输入；A_x0、A_y0、A_z0分别为MIMU中x、y、z三个轴向加速度计的零位偏置。

(b)陀螺仪误差模型一般如公式（3）所示：

${\widetilde{\mathrm{\Ω}}}_b=(I+[{\mathrm{\δK}}_G\left]\right)(I+[\mathrm{\δG}\left]\right){\mathrm{\Ω}}^b+{\mathrm{\ϵ}}^b---\left(3\right)$

公式（3）中，为MIMU中陀螺仪的角速率输出矢量，I为单位矩阵，δK_G为MIMU中陀螺仪的标度因数误差矩阵，δG为陀螺仪的安装误差系数矩阵，Ω^b为系统的真实角速率输入矢量，ε^b为MIMU中陀螺仪的零位漂移矢量。将公式（3）展开即获得 本发明使用的陀螺仪误差模型，其如公式（4）所示，

$\left[\begin{array}{c}{\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_z\end{array}\right]=\left[\begin{array}{ccc}1+{\mathrm{\δKG}}_x& {\mathrm{\δG}}_z& -{\mathrm{\δG}}_y\\ -{\mathrm{\δG}}_z& 1+{\mathrm{\δKG}}_y& {\mathrm{\δG}}_x\\ {\mathrm{\δG}}_y& -{\mathrm{\δG}}_x& 1+{\mathrm{\δKG}}_z\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_{x0}\\ {\mathrm{\ω}}_{y0}\\ {\mathrm{\ω}}_{z0}\end{array}\right]---\left(4\right)$

公式（4）中，Ω_x、Ω_y、Ω_z分别为MIMU中x、y、z三个轴向陀螺仪的角速率输出；δKG_x、δKG_y、δKG_z分别为MIMU中x、y、z三个轴向陀螺仪的标度因数误差；δG_x、δG_y、δG_z分别为MIMU中x、y、z三个轴向陀螺仪的安装误差系数；ω_x、ω_y、ω_z分别表示x、y、z三个轴向真实的角速率输入；ω_x0、ω_y0、ω_z0分别为MIMU中x、y、z三个轴向陀螺仪的零位漂移。

二、内环滤波估计：

（a）内环滤波模型

本发明内环滤波估的滤波器为Kalman滤波器，内环Kalman滤波模型包括由MIMU构建的捷联式惯性导航系统的内环滤波状态方程和内环滤波观测方程。

（a1）内环滤波估计状态方程如公式（5）所示：

$\stackrel{\·}{X}=\mathrm{FX}+W---\left(5\right)$

公式（5）中，X为由MIMU构建的捷联式惯性导航系统的状态矢量，为系统状态矢量的微分，W为系统激励噪声矢量，F为系统转移矩阵。

系统状态矢量X如公式（6）所示：

X＝[δV δφ δP ▽ ε δKA δKG]^T （6）

公式（6）中，δV为导航坐标系中东北天三个方向的速度误差，且δV＝[δV_e δV_n δV_u]，其中δV_e为东向速度误差，δV_n为北向速度误差，δV_u为天向速度误差；

公式（6）中，δφ为导航坐标系统东北天三个方向的姿态角误差，且δφ＝[δφ_e δφ_n δφ_u]，其中δφ_e为东向失准角误差，δφ_n为北向失准角误差，δφ_u为天向失准角误差；

公式（6）中，δP为导航坐标系统的位置误差，且δP＝[δL δλ δh]，其中δL为纬度误差，δλ为经度误差，δh为高度误差；

公式（6）中，▽为载体坐标系中x、y、z三个轴向加速度计的零位偏置误差，且▽＝[▽_x ▽_y ▽_z]，其中，为x轴向加速度计的零位偏置误差，▽_y为y轴向加速度计的零位偏置误差，▽_z为z轴向加速度计的零位偏置误差；

公式（6）中，ε为载体坐标系中x、y、z三个轴向陀螺仪的常值偏移误差：且ε＝[ε_x ε_yε_z]，ε_x为x轴向陀螺仪的常值漂移误差，ε_y为y轴向陀螺仪的常值漂移误差，ε_z为z轴向陀螺仪的常值漂移误差；

公式（6）中，δKA为加速度计的标度因数误差和安装误差，且δKA＝[δKA_x δKA_y δKA_zδA_x δA_y δA_z]，其中，δKA_x、δKA_y和δKA_z分别为x、y、z各轴向加速度计的标度因数误差，δA_x、δA_y和δA_z分别为x、y、z各轴向加速度计的安装误差；

公式（6）中，δKG为陀螺仪的标度因数误差和安装误差，且δKG＝[δKG_x δKG_y δKG_z δG_x δG_y δG_z]，其中，δKG_x、δKG_y和δKG_z分别为x、y、z各轴向陀螺仪的标度因数误差，δG_x、δG_y和δG_z分别为x、y、z各轴向陀螺仪的安装误差；

公式（6）中，T表示对矩阵进行转置运算；

公式（5）中，W为系统噪声矢量，且

$W={\left\{\begin{array}{cccccccccc}{w}_{V_e}& w_{V_n}& w_{V_u}& w_{{\mathrm{\φ}}_e}& w_{{\mathrm{\φ}}_n}& w_{{\mathrm{\φ}}_u}& w_L& w_{\mathrm{\λ}}& w_h& 0_{1\×18}\end{array}\right\}}^T,$ 并且，系统噪声方差阵Q满足Q＝Cov[W_k,W_j]＝E[W_kW_j ^T]，其中和分别表示东北天三个方向的速度噪声，分别表示东北天三个方向的姿态角噪声，w_L、w_λ和w_h分别表示经度、纬度和高度噪声，W_k为k时刻的系统噪声矢量，W_j为j时刻的系统噪声矢量，Cov表示协方差，E表示数学期望。

公式（5）中，F为状态转移矩阵，且状态转移矩阵如公式（7）所示：

$F=\left[\begin{array}{c}\begin{array}{ccccc}{F}_0& F_1& F_2& F_3& F_4\end{array}\\ \begin{array}{ccccc}{F}_5& F_6& F_7& F_8& F_9\end{array}\\ \begin{array}{cc}{F}_{10}& 0_{3\×18}\end{array}\\ 0_{18\×27}\end{array}\right]---\left(7\right)$

公式（7）中，F₀，F₁，F₂，F₃，F₄，F₅，F₆，F₇，F₈，F₉，F₁₀分别为状态转移矩阵F的子部分。

公式（7）中，子部分F₀如公式（8）所示：

$F_0=\left[\begin{array}{ccc}\frac{V_n\tan L-V_u}{R_N+h}& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\tan L& -({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\\ -({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\tan L)& -\frac{V_u}{R_M+h}& -\frac{V_n}{R_M+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})& \frac{{2V}_n}{R_M+h}& 0\end{array}\right]---\left(8\right)$

公式（8）中，V_e、V_n和V_u分别表示东北天三个方向的速度；L和h分别表示纬度和高度信息。R_M和R_N分别表示沿子午圈和卯酉圈的曲率半径；ω_ie为地球自转角速率。

公式（7）中，子部分F₁如公式（9）所示：

$F_1=\left[\begin{array}{ccc}0& {-f}_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]---\left(9\right)$

公式（9）中，f_e、f_n和f_u分别表示在导航坐标系统中东北天三个方向的比力。

公式（7）中，子部分F₂如公式（10）所示：

$F_2=\left[\begin{array}{ccc}[{2\mathrm{\ω}}_{\mathrm{ie}}(V_u\sin L+V_n\cos L)+\frac{V_e{V}_n}{R_N+h}{\mathrm{Sec}}^{2}L]& 0& \frac{V_e{V}_u-V_e{V}_n\tan L}{{(R_N+h)}^2}\\ -({2V}_e{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{{V_e}^2}{R_N+h}{\sec }^{2}L)& 0& [\frac{V_n{V}_u}{{(R_M+h)}^2}+\frac{{V_e}^2\tan L}{{(R_N+h)}^2}]\\ -{2V}_e{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -[\frac{{V_n}^2}{{(R_M+h)}^2}+\frac{{V_e}^2}{{(R_N+h)}^2}]\end{array}\right]---\left(10\right)$

公式（7）中，子部分F₃如公式（11）所示：

$F_3=\left[\begin{array}{cc}\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}& 0_{3\×3}\end{array}\right]---\left(11\right)$

公式（11）中，T_ij（i＝1～3,j＝1～3）满足 $C_n^b=\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right],$ 为每个滤波周期内的姿态阵。

公式（7）中，子部分F₄如公式（12）所示：

$F_4=\left[\begin{array}{cc}\begin{array}{cccccc}{T}_{11}{f}_{\mathrm{ibx}}^b& T_{11}{f}_{\mathrm{iby}}^b& T_{11}{f}_{\mathrm{ibz}}^b& T_{12}{f}_{\mathrm{ibz}}^b-T_{13}{f}_{\mathrm{iby}}^b& T_{13}{f}_{\mathrm{ibx}}^b-T_{11}{f}_{\mathrm{ibz}}^b& T_{11}{f}_{\mathrm{iby}}^b-T_{12}{f}_{\mathrm{ibx}}^b\\ T_{21}{f}_{\mathrm{ibx}}^b& T_{21}{f}_{\mathrm{iby}}^b& T_{21}{f}_{\mathrm{ibz}}^b& T_{22}{f}_{\mathrm{ibz}}^b-T_{23}{f}_{\mathrm{iby}}^b& T_{23}{f}_{\mathrm{ibx}}^b-T_{21}{f}_{\mathrm{ibz}}^b& T_{21}{f}_{\mathrm{iby}}^b-T_{22}{f}_{\mathrm{ibx}}^b\\ T_{31}{f}_{\mathrm{ibx}}^b& T_{31}{f}_{\mathrm{iby}}^b& T_{31}{f}_{\mathrm{ibz}}^b& T_{32}{f}_{\mathrm{ibz}}^b-T_{33}{f}_{\mathrm{iby}}^b& T_{33}{f}_{\mathrm{ibx}}^b-T_{31}{f}_{\mathrm{ibz}}^b& T_{31}{f}_{\mathrm{iby}}^b-T_{32}{f}_{\mathrm{ibx}}^b\end{array}& 0_{3\×6}\end{array}\right]---\left(12\right)$

公式（12）中，和表示在载体坐标系中x、y、z各轴向的加速度输出的比力。

公式（7）中，子部分F₅如公式（13）所示：

$F_5=\left[\begin{array}{ccc}0& -\frac{V_e}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]---\left(13\right)$

公式（7）中，子部分F₆如公式（14）所示：

$F_6=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_e}{R_N+h}\tan L)& 0& -\frac{V_n}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}& \frac{V_n}{R_M+h}& 0\end{array}\right]---\left(14\right)$

公式（7）中，子部分F₇如公式（15）所示：

$F_7=\left[\begin{array}{ccc}0& 0& \frac{V_n}{{(R_M+h)}^2}\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -\frac{V_e}{{(R_N+h)}^2}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e{\sec }^{2}L}{R_N+h}& 0& \frac{V_e\tan L}{{(R_N+h)}^2}\end{array}\right]---\left(15\right)$

公式（7）中，子部分F₈如公式（16）所示：

$F_8=\left[\begin{array}{cc}{0}_{3\×3}& \begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\end{array}\right]---\left(16\right)$

公式（7）中，子部分F₉如公式（17）所示：

$F_9=\left[\begin{array}{cc}{0}_{3\×6}& \begin{array}{cccccc}-T_{11}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -T_{11}{\mathrm{\ω}}_{\mathrm{iby}}^b& -T_{11}{\mathrm{\ω}}_{\mathrm{ibz}}^b& -T_{12}{\mathrm{\ω}}_{\mathrm{ibz}}^b+T_{13}{\mathrm{\ω}}_{\mathrm{iby}}^b& -T_{13}{\mathrm{\ω}}_{\mathrm{ibx}}^b+T_{11}{\mathrm{\ω}}_{\mathrm{ibz}}^b& -T_{11}{\mathrm{\ω}}_{\mathrm{iby}}^b+T_{12}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ -T_{12}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -T_{21}{\mathrm{\ω}}_{\mathrm{iby}}^b& -T_{21}{\mathrm{\ω}}_{\mathrm{ibz}}^b& -T_{22}{\mathrm{\ω}}_{\mathrm{ibz}}^b+T_{23}{\mathrm{\ω}}_{\mathrm{iby}}^b& -T_{23}{\mathrm{\ω}}_{\mathrm{ibx}}^b+T_{21}{\mathrm{\ω}}_{\mathrm{ibz}}^b& -T_{21}{\mathrm{\ω}}_{\mathrm{iby}}^b+T_{22}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ -T_{31}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -T_{31}{\mathrm{\ω}}_{\mathrm{iby}}^b& -T_{31}{\mathrm{\ω}}_{\mathrm{ibz}}^b& -T_{32}{\mathrm{\ω}}_{\mathrm{ibz}}^b+T_{33}{\mathrm{\ω}}_{\mathrm{iby}}^b& -T_{33}{\mathrm{\ω}}_{\mathrm{ibx}}^b+T_{31}{\mathrm{\ω}}_{\mathrm{ibz}}^b& -T_{31}{\mathrm{\ω}}_{\mathrm{iby}}^b+T_{32}{\mathrm{\ω}}_{\mathrm{ibx}}^b\end{array}\end{array}\right]---\left(17\right)$

公式（17）中，和分别为载体坐标系中x、y、z各轴向的陀螺仪输出的角速度。

公式（7）中，子部分F₁₀如公式（18）所示：

$F_{10}=\left[\begin{array}{ccccccccc}0& \frac{1}{R_M+h}& 0& 0& 0& 0& 0& 0& -\frac{V_N}{{(R_M+h)}^2}\\ \frac{\sec L}{R_N+h}& 0& 0& 0& 0& 0& \frac{V_e}{R_N+h}\tan \mathrm{LSecL}& 0& -\frac{V_E\sec L}{{(R_N+h)}^2}\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(18\right)$

（a2）内环滤波估计环节的系统观测方程如公式（19）所示：

Z＝HX+V （19）

公式（19）中，Z为内环滤波估计环节的系统观测量矢量，且Z＝[δV_e δV_n δV_u δL δλδh]^T；

公式（19）中，H为内环滤波估计环节的系统观测矩阵，且

$H=\left[\begin{array}{c}{H}_0\\ H_1\\ H_2\\ H_3\\ H_4\\ H_5\end{array}\right]---\left(20\right)$

公式（20）中，H₀、H₁、H₂、H₃、H₄、H₅分别为内环滤波估计环节的系统观测矩阵的子部分。

公式（20）中，子部分H₀＝[1 0_1×26]，子部分H₁＝[0 1 0_1×25]，子部分H₂＝[0 0 10_1×24]，子部分H₃＝[0_1×6 1 0_1×20]，子部分H₄＝[0_1×7 1 0_1×19]，子部分H₅＝[0_1×8 1 0_1×18]。

公式（19）中，V为内环滤波估计环节的系统测量噪声序列，且V＝[v₀ v₁ v₂ v₃ v₄v₅]^T为白噪声，v₀、v₁、v₂、v₃、v₄、v₅分别表示东向速度、北向速度、天向速度、纬度、经度以及高度零均值的白噪声，并满足系统测量噪声序列 的方差阵R＝Cov[V_k,V_j]＝E[V_kV_j ^T]，V_k为k时刻的零均值白噪声，V_j为j时刻的零均值白噪声。

（b）内环滤波估计的计算过程

利用MIMU的输出进行捷联式惯性导航方法解算并建立内环滤波估计环节的系统状态方程和系统测量方程，利用Kalman滤波算法进行估计，获得内环滤波估计环节的状态量估计值。其中，Kalman滤波算法的流程图如图3所示，Kalman滤波算法的步骤如下：

步骤1：根据前一时刻的内环滤波估计环节的状态量以及一步状态转移矩阵Φ_k,k-1计算获得内环滤波估计环节的状态量一步预测值计算方式如公式（21）所示：

${\stackrel{\‾}{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(21\right)$

步骤2：根据前一时刻的一步预测均方误差P_k-1、一步状态转移矩阵Φ_k,k-1、系统噪声方差阵Q_k-1以及系统噪声驱动阵Γ_k-1计算获得内环滤波估计环节的一步预测均方误差P_k/k-1，计算方式如公式（22）所示：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(22\right)$

步骤3：根据系统的一步预测均方误差P_k/k-1、滤波系统的观测阵H_k以及系统的测量噪声序列的方差阵R_k计算滤波增益值K_k，计算方式如公式（23）所示：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(23\right)$

步骤4：根据内环滤波估计环节的一步预测值观测量Z_k、滤波系统的观测阵H_k以及滤波增益值K_k计算内环滤波估计环节的状态估值计算方式如公式（24）所示：

${\hat{X}}_k={\stackrel{\‾}{X}}_{k/k-1}+K_k(Z_k-H_k{\stackrel{\‾}{X}}_{k/k-1})---\left(24\right)$

步骤5：根据内环滤波估计环节的滤波增益值K_k、滤波系统的观测阵H_k、一步预测均方误差P_k/k-1以及测量噪声序列的方差阵R_k计算内环滤波估计环节的估计均方差P_k，计算方式如公式（25）所示：

$P_k=(1-K_k{H}_k)P_{k/k-1}{(1-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(25\right)$

三、外环滤波估计：

（a）外环滤波模型

第i个MIMU（假设总共N各MIMU）进行外环滤波估计的外环滤波器模型包括外环滤波估计环节的系统状态方程和观测方程。

（a1）外环滤波估计环节的系统状态方程

外环滤波系统状态方程与内环滤波系统状态方程相同，外环滤波系统状态量与内环滤波系统状态量相同。

（a2）外环滤波估计环节的系统观测方程

外环滤波估计环节的系统观测方程如公式（26）所示：

$Z_t^i=H^i{x}_t+u_t^i---\left(26\right)$

公式（26）中，为外环滤波估计环节的系统观测量如公式（27）所示：

$Z_t^i={\left[\begin{array}{ccccccc}{y}_t^i& y_t^{i1}& \·\·\·& y_t^{{\mathrm{iN}}_i}& z_t^{i1}& \·\·\·& z_t^{{\mathrm{iN}}_i}\end{array}\right]}^T---\left(27\right)$

公式（27）中， $y_t^i(j=0~N(j\≠i))$ 为第i个MIMU的观测值， $z_t^{\mathrm{ij}}(j=1~N(j\≠i))$ 为邻近MIMU的各自内环滤波估计环节的观测值；

公式（26）中，Hⁱ为外环滤波估计环节的系统观测矩阵，且 $H^i={\left[\begin{array}{ccccccc}{H}_0^i& H_1^i& \·\·\·& H_5^i& I& \·\·\·& I\end{array}\right]}^T,H_m^i(m=0~5)$ 为第i个MIMU内环滤波估计环节的系统观测矩阵的子部分；xt为外环滤波估计环节的系统状态矢量；为外环滤波估计环节的系统测量噪声序列，且 $u_t^i={\left[\begin{array}{ccccccc}{v}_0^i& v_1^i& \·\·\·& v_5^i& u_t^{i1}& \·\·\·& u_t^{\mathrm{iN}}\end{array}\right]}^T,\left[\begin{array}{cccc}{v}_0^i& v_1^i& \·\·\·& v_5^i\end{array}\right]$ 为第i个MIMU内环滤波估计环节的系统测量噪声序列，为邻近MIMU内环滤波估计环节的系统测量噪声序列。

（b）外环滤波估计的计算过程

根据MIMU之间交换的信息，设计外环滤波估计环节的滤波算法，获得外环滤波估计环节状态量的最优估计值。外环滤波估计环节的算法流程图如图4所示，外环滤波估计环节的计算过程为：

步骤1：根据第i个MIMU前一时刻内环滤波估计环节的状态量以及一步状态转移矩阵计算获得外环滤波估计环节的状态一步预测值计算方式如公式（28）所示：

${\stackrel{\‾}{X}}_k^i={\mathrm{\Φ}}_{k,k-1}^i{\hat{X}}_{k-1}^i---\left(28\right)$

步骤2：根据第i个MIMU前一时刻的一步预测均方误差一步状态转移矩阵系统噪声方差阵以及系统噪声驱动阵计算获得外环滤波估计环节的系统一步预测均方误差计算方式如公式（29）所示：

$P_{k/k-1}^i={{\mathrm{\Φ}}_{k,k-1}^i{P}_{k-1}^i{\mathrm{\Φ}}_{k,k-1}^i}^T+{{\mathrm{\Γ}}_{k-1}^i{Q}_{k-1}^i{\mathrm{\Γ}}_{k-1}^i}^T---\left(29\right)$

步骤3：根据第i个MIMU外环滤波估计环节的系统一步预测均方误差以及与其领近的其他多个MIMU外环滤波估计环节的观测矩阵H^j和测量噪声序列的方差阵R^j计算获得第i个MIMU外环滤波估计环节的误差估计协方差计算方式如公式（30）所示：

$M_k^i={[{\left(P_{k/k-1}^i\right)}^{-1}+\underset{j\∈N_i\∪i}{\mathrm{\Σ}}{\left(H^j\right)}^T{\left(R^j\right)}^{-1}{H}^j]}^{-1}---\left(30\right)$

步骤4：根据第i个MIMU外环滤波估计环节计算的状态一步预测值外环滤波估计环节的误差估计协方差阵以及与其领近的其他多个MIMU的外环滤波估计环节的观测矩阵H^j、测量噪声序列的方差阵R^j和状态一步预测值计算获得状态估值 计算方式如公式（31）所示：

${\hat{X}}_k^i={\stackrel{\‾}{X}}_k^i+M_k^i\underset{{j\∈N}_i\∪i}{\mathrm{\Σ}}{\left(H^j\right)}^T{\left(R^j\right)}^{-1}(Z_k^j-H^j{\stackrel{\‾}{X}}_k^i)+{\mathrm{\ϵM}}_k^i\underset{{j\∈N}_i}{\mathrm{\Σ}}({\stackrel{\‾}{X}}_k^j-{\stackrel{\‾}{X}}_k^i)---\left(31\right)$

公式中ε为可调参数，N_i表示1～N中不包含i。

Claims (9)

1.一种MIMU组网式系统级标定方法，其特征在于，包括以下步骤：

步骤一、将待测MIMU分别安置于同一刚性测试平台内的安装基准上，调整各个待测MIMU的基准面与安装基准吻合；在中心计算机中建立MEMS惯性传感器的误差模型；

步骤二、根据预先设定的采样周期、滤波周期以及测试平台的机动方式对测试平台进行激励，在测试平台机动过程中采集每个MIMU中MEMS惯性传感器的输出值并将输出值送入中心计算机；

步骤三、中心计算机中对MEMS惯性传感器的输出值采用捷联式惯性导航方法进行导航解算，获得MIMU的速度、位置、姿态；并建立内环滤波估计环节的系统状态方程和系统测量方程；

步骤四、中心计算机进行内环滤波估计获得每个MIMU的内环滤波估计值，内环滤波估计值包括每个MIMU的内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值以及内环滤波估计环节的状态估计值；然后在每个MIMU与其邻近的多个MIMU之间交换内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值；

步骤五、中心计算机根据每个MIMU获得的邻近多个MIMU的内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值，对每个MIMU进行外环滤波估计，获得每个MIMU中MEMS惯性传感器误差模型参数的精确估计值；

步骤六、中心计算机将误差模型参数的精确估计值返回给各个MIMU，使用测试平台的运动激励验证每个MIMU中MEMS惯性传感器误差模型参数精确估计值的准确度，并计算MEMS惯性传感器误差模型参数的误差方差；

步骤七、将步骤六获得的每个MIMU中MEMS惯性传感器误差模型参数的误差方差与预先设定的误差方差阈值进行比较，若误差方差大于误差方差阈值，则返回步骤三；若误差方差不大于误差方差阈值，则把MIMU外环滤波估计获得的模型参数的精确估计值作为每个MIMU中MEMS惯性传感器误差模型的标定结果，完成对批量MIMU的标定。

2.如权利要求1所述的MIMU组网式系统级标定方法，其特征在于，所述MEMS惯性传感器的误差模型包括加速度计误差模型和陀螺仪误差模型，

所述加速度计误差模型如公式(1)所示，

$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}1+{\mathrm{\δKA}}_x& {\mathrm{\δA}}_z& -{\mathrm{\δA}}_y\\ -{\mathrm{\δA}}_z& 1+{\mathrm{\δKA}}_y& {\mathrm{\δA}}_x\\ {\mathrm{\δA}}_y& -{\mathrm{\δA}}_x& 1+{\mathrm{\δKA}}_z\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{c}{A}_{x0}\\ A_{y0}\\ A_{z0}\end{array}\right]---\left(1\right)$

公式(1)中，A_x、A_y、A_z分别为MIMU中x、y、z三个轴向加速度计的比力输出；δKA_x、δKA_y、δKA_z分别为MIMU中x、y、z三个轴向加速度计的标度因数误差；δA_x、δA_y、δA_z分别为MIMU中x、y、z三个轴向加速度计的安装误差系数；a_x、a_y、a_z分别为x、y、z三个轴向真实的比力输入；A_x0、A_y0、A_z0分别为MIMU中x、y、z三个轴向加速度计的零位偏置；

所述陀螺仪误差模型如公式(2)所示，

$\left[\begin{array}{c}{\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_z\end{array}\right]=\left[\begin{array}{ccc}1+{\mathrm{\δKG}}_x& {\mathrm{\δG}}_z& -{\mathrm{\δG}}_y\\ -{\mathrm{\δG}}_z& 1+{\mathrm{\δKG}}_y& {\mathrm{\δG}}_x\\ {\mathrm{\δG}}_y& -{\mathrm{\δG}}_x& 1+{\mathrm{\δKG}}_z\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_{x0}\\ {\mathrm{\ω}}_{y0}\\ {\mathrm{\ω}}_{z0}\end{array}\right]---\left(2\right)$

公式(2)中，Ω_x、Ω_y、Ω_z分别为MIMU中x、y、z三个轴向陀螺仪的角速率输出；δKG_x、δKG_y、δKG_z分别为MIMU中x、y、z三个轴向陀螺仪的标度因数误差；δG_x、δG_y、δG_z分别为MIMU中x、y、z三个轴向陀螺仪的安装误差系数；ω_x、ω_y、ω_z分别表示x、y、z三个轴向真实的角速率输入；ω_x0、ω_y0、ω_z0分别为MIMU中x、y、z三个轴向陀螺仪的零位漂移。

3.如权利要求1所述的MIMU组网式系统级标定方法，其特征在于，所述步骤二中，在对测试平台进行激励之前，对MIMU通电预热30分钟，设定采样周期为0.01秒。

4.如权利要求1所述的MIMU组网式系统级标定方法，其特征在于，所述中心计算机为嵌入式处理器、可移动终端、笔记本电脑中的一种。

5.如权利要求1所述的MIMU组网式系统级标定方法，其特征在于，每个MIMU与其邻近的四个MIMU之间交换内环滤波估计环节的状态量一步预测值、一步预测均方误差、滤波增益值。

6.如权利要求1所述的MIMU组网式系统级标定方法，其特征在于，所述步骤四中进行内环滤波估的滤波器为Kalman滤波器；滤波模型包括由MIMU构建的捷联式惯性导航系统的内环滤波估计环节的状态方程和观测方程；

所述内环滤波估计环节的状态方程如公式(3)所示：

$\stackrel{\·}{X}=FX+W---\left(3\right)$

公式(3)中，X为由MIMU构建的捷联式惯性导航系统的状态矢量，为系统状态矢量的微分，W为系统激励噪声矢量，F为系统转移矩阵；

所述内环滤波估计环节的状态方程的状态量包括：捷联式惯性导航方法解算的速度误差、位置误差、姿态误差；待标定的MIMU中的MEMS加速度计的零位偏置、标度因数误差、安装误差以及MEMS陀螺仪的常值漂移、标度因数误差和安装误差；

所述内环滤波估计环节的观测方程如公式(4)所示：

Z＝HX+V (4)

公式(4)中，Z为内环滤波估计环节由MIMU构建的捷联式惯性导航系统的观测量矢量，H为内环滤波估计环节的系统观测矩阵，X为内环滤波估计环节的系统状态矢量，V为内环滤波估计环节的系统测量噪声序列；

所述内环滤波估计环节的观测方程的观测量包括：MIMU的速度误差、位置误差。

7.如权利要求6所述的MIMU组网式系统级标定方法，其特征在于，使用Kalman滤波器进行内环滤波估计的计算过程为：

步骤7.1：根据前一时刻的内环滤波估计环节的状态量以及一步状态转移矩阵Φ_k,k-1计算获得内环滤波估计环节的状态量一步预测值计算方式如公式(5)所示：

${\stackrel{\‾}{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(5\right)$

步骤7.2：根据前一时刻的一步预测均方误差P_k-1、一步状态转移矩阵Φ_k,k-1、系统噪声方差阵Q_k-1以及系统噪声驱动阵Γ_k-1计算获得内环滤波估计环节的一步预测均方误差P_k/k-1，计算方式如公式(6)所示：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(6\right)$

步骤7.3：根据系统的一步预测均方误差P_k/k-1、滤波系统的观测阵H_k以及系统的测量噪声序列的方差阵R_k计算滤波增益值K_k，计算方式如公式(7)所示：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(7\right)$

步骤7.4：根据内环滤波估计环节的一步预测值观测量Z_k、滤波系统的观测阵H_k以及滤波增益值K_k计算内环滤波估计环节的状态估值计算方式如公式(8)所示：

${\hat{X}}_k={\stackrel{\‾}{X}}_{k/k-1}+K_k(Z_k-H_k{\stackrel{\‾}{X}}_{k/k-1})---\left(8\right)$

步骤7.5：根据内环滤波估计环节的滤波增益值K_k、滤波系统的观测阵H_k、一步预测均方误差P_k/k-1以及测量噪声序列的方差阵R_k计算内环滤波估计环节的估计均方差P_k，计算方式如公式(9)所示：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(9\right)$

上述计算过程中，T表示对矩阵进行转置的运算符，I为单位矩阵。

8.如权利要求1所述的MIMU组网式系统级标定方法，其特征在于，所述步骤五中进行外环滤波估计的滤波模型包括外环滤波估计环节的状态方程和观测方程；

所述外环滤波估计环节的状态方程和状态量与内环滤波估计环节的状态方程和状态量相同；

所述外环滤波估计环节的观测方程如公式(10)所示：

$Z_t^i=H^i{x}_t+u_t^i---\left(10\right)$

公式(10)中，为外环滤波估计环节由MIMU构建的捷联式惯性导航系统的观测量，Hⁱ为外环滤波估计环节的系统观测矩阵，为外环滤波估计环节的系统测量噪声序列，x_t为外环滤波估计环节的系统状态矢量；

所述外环滤波估计环节的观测方程的观测量包括：每个MIMU各自内环滤波估计环节的观测量和邻近的多个MIMU内环滤波估计环节的观测量。

9.如权利要求8所述的MIMU组网式系统级标定方法，其特征在于，使用外环滤波估计的滤波模型进行外环滤波估计的计算过程为：

步骤9.1：根据第i个MIMU前一时刻内环滤波估计环节的状态量以及一步状态转移矩阵计算获得外环滤波估计环节的状态一步预测值计算方式如公式(11)所示：

${\stackrel{\‾}{X}}_k^i={\mathrm{\Φ}}_{k,k-1}^i{\hat{X}}_{k-1}^i---\left(11\right)$

步骤9.2：根据第i个MIMU前一时刻的一步预测均方误差一步状态转移矩阵系统噪声方差阵以及系统噪声驱动阵计算获得外环滤波估计环节的系统一步预测均方误差计算方式如公式(12)所示：

$P_{k/k-1}^i={\mathrm{\Φ}}_{k,k-1}^i{P}_{k-1}^i{{\mathrm{\Φ}}_{k,k-1}^i}^T+{\mathrm{\Γ}}_{k-1}^i{Q}_{k-1}^i{{\mathrm{\Γ}}_{k-1}^i}^T---\left(12\right)$

步骤9.3：根据第i个MIMU外环滤波估计环节的系统一步预测均方误差以及与其领近的其他多个MIMU外环滤波估计环节的观测矩阵H^j和测量噪声序列的方差阵R^j计算获得第i个MIMU外环滤波估计环节的误差估计协方差计算方式如公式(13)所示：

$M_k^i={\[{\left(P_{k/k-1}^i\right)}^{-1}+\underset{j\∈N_i\∪i}{\Σ}{\left(H^j\right)}^T{\left(R^j\right)}^{-1}{H}^j\]}^{-1}---\left(13\right)$

步骤9.4：根据第i个MIMU外环滤波估计环节计算的状态一步预测值外环滤波估计环节的误差估计协方差阵以及与其领近的其他多个MIMU的外环滤波估计环节的观测矩阵H^j、测量噪声序列的方差阵R^j和状态一步预测值计算获得状态估值计算方式如公式(14)所示：

${\hat{X}}_k^i={\stackrel{\‾}{X}}_k^i+M_k^i\underset{j\∈N_i\∪i}{\Σ}{\left(H^j\right)}^T{\left(R^j\right)}^{-1}(Z_k^j-H^j{\stackrel{\‾}{X}}_k^i)+{\mathrm{\ϵM}}_k^i\underset{j\∈N_i}{\Σ}({\stackrel{\‾}{X}}_k^j-{\stackrel{\‾}{X}}_k^i)---\left(14\right)$

公式中ε为可调参数，N_i表示1～N中不包含i；

上述计算过程中，T表示对矩阵进行转置的运算符。