CN102252677A

CN102252677A - Time series analysis-based variable proportion self-adaptive federal filtering method

Info

Publication number: CN102252677A
Application number: CN2011100958407A
Authority: CN
Inventors: 李宁; 袁克非; 袁赣南; 刘利强; 张勇刚
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2011-04-18
Filing date: 2011-04-18
Publication date: 2011-11-23

Abstract

The invention discloses a variable-scale self-adaptive federated filtering method based on time series analysis, which is applied to an underwater multi-sensor integrated navigation system. Firstly, the system state equation and measurement equation are established according to the error equation of each navigation sensor system, and the obtained equation is discretized, and the discrete state space model corresponding to each navigation sensor system is established, and then according to the historical data of the navigation sensor system, through autoregressive The model obtains the information weight of the navigation sensor system, obtains the information distribution ratio according to the information weight and the law of information conservation, and finally obtains the global optimal estimate, and uses the global optimal estimate to reset the filter value and estimation error variance matrix. The invention improves system navigation precision, system stability and fault tolerance, and can meet the requirements of high precision and high reliability of underwater vehicles.

Description

A Variable Scale Adaptive Federated Filtering Method Based on Time Series Analysis

技术领域 technical field

本发明属于多传感器组合导航系统技术领域，涉及一种多传感器组合导航系统滤波方法，具体是一种基于时间序列分析的变比例自适应联邦滤波方法。The invention belongs to the technical field of multi-sensor integrated navigation systems, and relates to a filtering method for a multi-sensor integrated navigation system, in particular to a variable-scale adaptive federated filtering method based on time series analysis.

背景技术 Background technique

随着导航技术的进步，任何单一导航设备的性能和应用范围都表现出明显的局限性，无法满足运载器日益增长的精度要求，也无法适应复杂的应用环境，无法完全满足系统可靠性的要求。Horst Ahlers说：“未来属于多传感器。”多传感器组合导航系统成为一种必然的趋势。随着硬件条件的改善，融合方法成为制约组合系统性能的重要因素。由N.A.Carlson提出而发展起来的联邦卡尔曼滤波方法，是成功运用于工程实践的融合方法，它采用噪声方差上界技术消除子滤波相关性，达到全局最优。With the advancement of navigation technology, the performance and application range of any single navigation device show obvious limitations, and cannot meet the increasing accuracy requirements of the carrier, nor can it adapt to complex application environments, and cannot fully meet the requirements of system reliability. . Horst Ahlers said: "The future belongs to multi-sensors." Multi-sensor integrated navigation systems have become an inevitable trend. With the improvement of hardware conditions, the fusion method has become an important factor restricting the performance of the combined system. The federated Kalman filter method developed by N.A.Carlson is a fusion method successfully applied in engineering practice. It uses the noise variance upper bound technology to eliminate the sub-filter correlation and achieve the global optimum.

但在已有的联邦滤波结构中，信息分配比例是随着结构的确定而固定的，它不能反映导航传感器工作状态和数据状态的变化，不能准确反映导航传感器信息权重分配，不能准确真实的反映系统的情况，不利于对提高系统精度、可靠性和容错性的研究。However, in the existing federated filtering structure, the information distribution ratio is fixed with the determination of the structure, which cannot reflect the changes in the working status and data status of the navigation sensor, cannot accurately reflect the information weight distribution of the navigation sensor, and cannot accurately and truly reflect The situation of the system is not conducive to the research on improving the accuracy, reliability and fault tolerance of the system.

发明内容 Contents of the invention

本发明针对现有联邦滤波方法中信息分配比例随着结构的确定，不能准确反映导航传感器信息权重分配的问题，提供一种基于时间序列分析的变比例自适应联邦滤波方法，可以使信息分配比例与导航传感器工作情况和载体运行环境情况相适应，达到提高系统精度、可靠性和容错性的目的。Aiming at the problem that the information distribution ratio in the existing federated filtering method cannot accurately reflect the distribution of navigation sensor information weights with the determination of the structure, the present invention provides a variable ratio adaptive federated filtering method based on time series analysis, which can make the information distribution ratio Adapt to the working conditions of the navigation sensor and the operating environment of the carrier to achieve the purpose of improving system accuracy, reliability and fault tolerance.

一种基于时间序列分析的变比例自适应联邦滤波方法，具体包括以下步骤：A variable-scale adaptive federated filtering method based on time series analysis, specifically comprising the following steps:

步骤一、根据组合导航系统的各导航传感器系统的误差方程建立系统状态方程和量测方程；Step 1, establishing a system state equation and a measurement equation according to the error equations of each navigation sensor system of the integrated navigation system;

步骤二、将得到的系统状态方程和量测方程离散化，建立各导航传感器系统对应的离散状态空间模型；Step 2, discretize the obtained system state equation and measurement equation, and establish a discrete state space model corresponding to each navigation sensor system;

步骤三、根据导航传感器系统的历史数据，通过自回归模型获取该导航传感器系统的信息权值；Step 3, according to the historical data of the navigation sensor system, obtain the information weight of the navigation sensor system through the autoregressive model;

步骤四、根据信息权值和信息守恒定律获取信息分配比例；Step 4. Obtain the information distribution ratio according to the information weight and the law of information conservation;

步骤五、根据信息分配比例进行联邦滤波，得到全局最优估计，最后用全局最优估计重置滤波值和估计误差方差阵。Step 5: Perform federated filtering according to the information distribution ratio to obtain the global optimal estimate, and finally use the global optimal estimate to reset the filter value and the estimated error variance matrix.

所述步骤一中的组合导航系统，根据某水下航行器的导航传感器配置情况，该组合导航系统由惯导系统INS，静电陀螺监控器ESGM，卫星定位导航系统GPS和多普勒测速系统DVL组成。The integrated navigation system in the described step 1, according to the configuration of the navigation sensor of a certain underwater vehicle, the integrated navigation system is composed of an inertial navigation system INS, an electrostatic gyro monitor ESGM, a satellite positioning navigation system GPS and a Doppler speed measuring system DVL composition.

根据各系统误差方程分别建立INS状态方程、INS/ESGM量测方程、INS/GPS量测方程和INS/DVL系统状态方程和量测方程：According to the error equations of each system, the INS state equation, INS/ESGM measurement equation, INS/GPS measurement equation, INS/DVL system state equation and measurement equation are respectively established:

所述的INS状态方程为：

The INS state equation is:

其中，状态量

δL、δλ、δh分别表示纬度L、经度λ、高度h的变化，δV_E、δV_N、δV_U分别表示东向速度V_E、北向速度V_N、天向速度V_U的变化，φ_E、φ_N、φ_U分别表示纵摇角、横摇角、偏航角，ε_bx、ε_by、ε_bz为陀螺漂移随机常数误差在三轴上的分量，ε_rx、ε_ry、ε_rz为陀螺漂移马尔可夫过程误差在三轴上的分量，

为加速度计误差在三轴上的分量；过程噪声阵W＝[w_bx w_by w_bz w_gx w_gy w_gz w_ax w_ay w_az]^T，w_bx、w_by、w_bz表示陀螺漂移随机常数的白噪声在三轴上的分量，w_gx、w_gy、w_gz表示陀螺漂移马尔可夫过程的白噪声在三轴上的分量，w_ax、w_ay、w_az表示加速度计误差的白噪声在三轴上的分量；状态转移阵其中

f_E、f_N、f_U分别为东向加速度、北向加速度、天向加速度，ω_ie为地球自转角速度；噪声驱动阵

其中

是方向余弦矩阵。Among them, the state quantity

δL, δλ, and δh represent changes in latitude L, longitude λ, and height h, respectively; _δVE , δV _N , and δV _U represent changes in eastward velocity V _E , northward velocity V _N , and skyward velocity V _U respectively; φ _E , φ _N , φ _U represent the pitch angle, roll angle and yaw angle respectively; ε _bx , ε _by , ε _bz are the components of the gyro drift random constant error on the three axes; ε _rx , ε _ry , ε _rz are the gyro The components of the drift Markov process error on the three axes,

is the component of the accelerometer error on the three axes; the process noise matrix W=[w _bx w _by w _bz w _gx w _gy w _gz w _ax _way w _az ] ^T , w _bx , w _by , w _bz represent the random drift of the gyroscope The components of the constant white noise on the three axes, w _gx , w _gy , w _gz represent the components of the white noise of the gyro drift Markov process on the three axes, w _ax , _way , w _az represent the white noise of the accelerometer error Components of noise on three axes; state transition matrix in

f _E , f _N , and f _U are eastward acceleration, northward acceleration, and celestial acceleration respectively; ω _ie is the earth's rotation angular velocity; the noise-driven array

in

is the direction cosine matrix.

所述的INS/ESGM量测方程为：Z₁(t)＝H₁(t)X₁(t)+V₁(t)，其中，Z₁为ESGM和INS位置、航向信息的差值为观测量，V₁(t)＝[m₁，m₂，m₃，m₄]^T是由ESGM的位置误差及方位误差产生的零均值的高斯白噪声；H₁(t)是量测阵，INS/ESGM状态方程X₁(t)＝X(t)。The INS/ESGM measurement equation is: Z ₁ (t)=H ₁ (t)X ₁ (t)+V ₁ (t), wherein, Z ₁ is the difference between ESGM and INS position and heading information Observational quantity, V ₁ (t)=[m ₁ , m ₂ , m ₃ , m ₄ ] ^T is Gaussian white noise with zero mean value generated by ESGM position error and orientation error; H ₁ (t) is the measurement array , INS/ESGM state equation X ₁ (t)=X(t).

所述的INS/GPS量测方程为：

其中，位置量测量

速度量测量 v₂(t)表示白噪声，INS/GPS状态方程X₂(t)＝X(t)；The INS/GPS measurement equation is:

Among them, the position measurement

Velocity measurement v ₂ (t) represents white noise, INS/GPS state equation X ₂ (t)=X(t);

所述的INS/DVL系统状态方程为： The state equation of the INS/DVL system is:

所述的INS/DVL量测方程为：

The described INS/DVL measurement equation is:

其中，多普勒测速系统的噪声驱动阵G_d(t)＝diag[1 1 0]，F_d(t)是多普勒测速系统的增广状态转移阵，W_d(t)是多普勒测速系统的增广过程噪声阵，X_d(t)是多普勒测速系统增广状态量，X_d＝[δV_d δΔ δC]^T，δV_d为多普勒测速系统中的速度偏移误差，δΔ为多普勒测速系统中的偏流角误差，δC为多普勒测速系统中的刻度系数误差；

V₃(t)＝[m_VE m_VN]^T为观测白噪声，V_dE、V_dN分别表示多普勒测速系统中的东向速度、北向速度，K′是船的航向角。Among them, the noise driving matrix of the Doppler velocity measurement system G _d (t) = diag[1 1 0], F _d (t) is the augmented state transition matrix of the Doppler velocity measurement system, W _d (t) is the Doppler The augmented process noise array of the Doppler velocimetry system, X _d (t) is the augmented state quantity of the Doppler velocimetry system, X _d = [δV _d δΔ δC] ^T , δV _d is the velocity offset in the Doppler velocimetry system Error, δΔ is the bias angle error in the Doppler velocity measurement system, and δC is the scale coefficient error in the Doppler velocity measurement system;

V ₃ (t)=[m _VE m _VN ] ^T is the observation white noise, V _dE and V _dN respectively represent the eastward speed and northward speed in the Doppler speed measurement system, and K′ is the heading angle of the ship.

所述步骤二中，各导航传感器系统对应的离散状态空间模型为：In the second step, the discrete state space model corresponding to each navigation sensor system is:

$\{\begin{matrix} {X x}_{k k} = = {Φ Φ}_{k k / / k k - - 11} {X x}_{k k - - 11} + + {Γ Γ}_{k k / / k k - - 11} {W W}_{k k - - 11} \\ {Z Z}_{k k} = = {H h}_{k k} {X x}_{k k} + + {V V}_{k k} \end{matrix}$

其中，k表示离散时间状态的索引值，X_k表示k时刻的状态量，Z_k表示k时刻的观测量，H_k表示k时刻的量测阵，Γ_k/k-1是噪声驱动阵，W_k-1为k-1时刻的系统状态噪声，且过程噪声协方差阵Q_k＝E[W_kW_k ^T]，V_k为k时刻的系统量测噪声，且量测噪声协方差阵R_k＝E[V_kV_k ^T]，状态转移阵Φ_k/k-1采用分步累积离散法得到：Among them, k represents the index value of the discrete-time state, X _k represents the state quantity at time k, Z _k represents the observed quantity at time k, H _k represents the measurement array at time k, Γ _k/k-1 is the noise-driven array, W _k-1 is the system state noise at time k-1, and the process noise covariance matrix Q _k ＝E[W _k W _k ^T ], V _k is the system measurement noise at time k, and the measurement noise covariance matrix R _k ＝E[V _k V _k ^T ], the state transition matrix Φ _k/k-1 is obtained by step-by-step cumulative discrete method:

${Φ Φ}_{k k / / k k - - 11} = = {Π Π}_{i i - - 11}^{J J} {Φ Φ}_{(({t t}_{k k - - 11} + + is is)) / / (({t t}_{k k - - 11} + + ((i i - - 11)) s the s))} \approx \approx {Π Π}_{i i - - 11}^{J J} ((I I + + s the s {F f}_{i i - - 11})) \approx \approx I I + + s the s {Σ Σ}_{i i = = 11}^{J J} {F f}_{i i - - 11}$

其中J＝T/s，T为采样时间，s为采样时间内划分的步长，I是单位阵，F_i-1是i-1时刻的状态转移阵。Where J=T/s, T is the sampling time, s is the step size divided in the sampling time, I is the unit matrix, and F _i-1 is the state transition matrix at time i-1.

所述步骤三中，将每个导航传感器系统的预测误差作为信息权值Θ_i：In the third step, the prediction error of each navigation sensor system is used as the information weight Θ _i :

${Θ Θ}_{i i} = = {y the y}_{i i} ((τ τ)) - - {\overset{^^}{y the y}}_{i i} ((τ τ)) ((i i = = 0,1,2,3 0,1,2,3))$

其中，y_i(τ)为第i个导航传感器的输出序列，为第i个导航传感器的输出序列的预测值，i＝0，1，2，3分别对应惯导系统、静电陀螺监控器、卫星定位导航系统和多普勒测速系统。Among them, y _i (τ) is the output sequence of the i-th navigation sensor, is the predicted value of the output sequence of the i-th navigation sensor, and i=0, 1, 2, 3 respectively correspond to the inertial navigation system, electrostatic gyro monitor, satellite positioning navigation system and Doppler velocity measurement system.

步骤四所述的信息分配比例，和信息权值成正比，且根据信息守恒定律，各导航传感器系统的权重和为

其中β₀为主系统权重，β_i为各子系统权重，n为所有子系统的个数，所述的主系统是惯导系统，子系统为静电陀螺监控器、卫星定位导航系统和多普勒测速系统，最后得到各导航传感器系统的权重为：

The information distribution ratio described in Step 4 is proportional to the information weight, and according to the law of information conservation, the sum of the weights of each navigation sensor system is

Wherein β ₀ is the weight of the main system, β _i is the weight of each subsystem, and n is the number of all subsystems. The main system is the inertial navigation system, and the subsystems are the electrostatic gyro monitor, the satellite positioning navigation system and the Doppler system. Le speed measuring system, and finally the weight of each navigation sensor system is obtained as:

步骤五所述的全局最优估计为：

其中，

为全局估计值，P_g为估计均方误差，P_i，k表示将第i个导航传感器系统的估计误差方差阵P_k通过比例β_i分配重置后的估计误差方差阵，

表示预测的第i个导航传感器系统的状态量。The global optimal estimate described in step five is:

in,

is the global estimated value, P _g is the estimated mean square error, P _i,k represents the estimated error variance matrix P _k of the i-th navigation sensor system after distribution and reset through the proportion β _i ,

Indicates the predicted state of the i-th navigation sensor system.

用全局最优解重置滤波值和估计误差方差阵。Reset the filtered values and estimated error variance matrix with the global optimal solution.

由于在分配系统噪声时使用方差上界技术，各子系统互不相关，增广系统也不耦合。虽然子系统的滤波结果是次优的，但融合时被重新合成，所以全局估计最优。Since the variance upper bound technique is used when assigning system noise, the subsystems are not correlated with each other, and the augmented system is not coupled. Although the filtering results of the subsystems are sub-optimal, they are re-synthesized during fusion, so the global estimation is optimal.

本发明方法的优点和积极效果在于：Advantage and positive effect of the inventive method are:

(1)信息权的选择基于各导航传感器系统的源信息，避免了以滤波估计值评价各导航传感器系统所带来的误差，使组合导航系统结构更加适应系统真实情况；同时权值的获取还使用了测量前向数据，使得导航传感器系统的历史数据得到了充分的运用，提高了信息的利用率。(1) The selection of information rights is based on the source information of each navigation sensor system, which avoids the error caused by evaluating each navigation sensor system with the estimated value of the filter, and makes the structure of the integrated navigation system more adaptable to the real situation of the system; The use of measurement forward data makes full use of the historical data of the navigation sensor system and improves the utilization rate of information.

(2)本发明方法使得组合导航系统的导航精度保持在相当的水平，误差相对于INS误差小且不发散，克服了INS误差积累，尤其是位置误差的弱点。(2) The method of the present invention keeps the navigation accuracy of the integrated navigation system at a considerable level, and the error is smaller than the INS error and does not diverge, and overcomes the weakness of the INS error accumulation, especially the position error.

(3)使用本发明方法可以使在不同导航传感器配置情况下，组合导航系统性能保持平稳，使得组合导航系统具有良好的滤波精度，具有良好的定位能力和误差纠正能力，具有较强的抗干扰能力和噪声抑制能力，提高了组合导航系统的可靠性和可用性。(3) Using the method of the present invention can make the performance of the integrated navigation system remain stable under different navigation sensor configurations, so that the integrated navigation system has good filtering accuracy, good positioning capability and error correction capability, and strong anti-interference capability and noise suppression capability, improving the reliability and usability of the integrated navigation system.

附图说明 Description of drawings

图1是本发明联邦滤波方法应用在多传感器组合导航系统中的示意图；Fig. 1 is the schematic diagram that the federal filter method of the present invention is applied in the multi-sensor integrated navigation system;

图2是本发明联邦滤波方法的具体步骤流程图；Fig. 2 is a flow chart of specific steps of the federal filtering method of the present invention;

图3是应用本发明联邦滤波方法的联邦滤波结构示意图；Fig. 3 is a schematic diagram of a federated filtering structure applying the federated filtering method of the present invention;

图4是信息分配比例变化曲线示意图；Fig. 4 is a schematic diagram of information distribution ratio change curve;

图5是融合系统和INS系统位置误差对比曲线示意图；Figure 5 is a schematic diagram of the position error comparison curve between the fusion system and the INS system;

图6是融合系统和INS系统速度误差对比曲线示意图；Figure 6 is a schematic diagram of the speed error comparison curve of the fusion system and the INS system;

图7是应用本发明方法的融合系统和应用经典联邦滤波方法的融合系统的位置误差对比曲线示意图。Fig. 7 is a schematic diagram of a position error comparison curve between the fusion system applying the method of the present invention and the fusion system applying the classic federated filtering method.

具体实施方式 Detailed ways

下面将结合附图和实施例对本发明作进一步的详细说明。The present invention will be further described in detail with reference to the accompanying drawings and embodiments.

本发明实施例中所应用的多传感器组合导航系统为水下多传感器组合导航系统，该组合导航系统由惯导系统(Inertial Navigation System，简称INS)，静电陀螺监控器(Electrostatic Supported Gyroscope Monitor，简称ESGM)，卫星定位导航系统(Global Positioning System，简称GPS)和多普勒测速系统(Doppler Velocity Log，简称DVL)组成。The multi-sensor integrated navigation system applied in the embodiment of the present invention is an underwater multi-sensor integrated navigation system. ESGM), Global Positioning System (Global Positioning System, GPS for short) and Doppler Velocity Log (DVL for short).

INS由陀螺仪和加速度仪组成，是一种不依赖外界信息的自主导航系统，它可以提供包括速度、位置和姿态等多种导航参数，具有抗干扰，全天候的优点，因此适合作为水下潜器的基准导航系统。INS的精度主要取决于惯性器件，导航精度误差会随着时间的积累而不断增加，需要考虑其长时间航行的误差稳定问题。ESGM是由两个双自由度静电陀螺仪和一个间接稳定平台构成的“哑铃式”组合体，它作为高精度和长期稳定性的信息源监测、补偿惯性导航系统。ESGM利用星体导航原理实现对INS参数的监控，而INS则为ESGM提供间接稳定平台和解算参数。这种组合方式可以大大抑制导航误差的发散，延长系统的重调周期，保证水下航行器长时间高精度巡航。GPS的显著优点是实时导航，定位误差与时间无关，有较高的定位和测速精度。当天线浮出水面或探测浮标投出时，获取当前点准确位置，将之校准导航误差，并修正系统时间。DVL基于多普勒效应，多采用固定发射方向的四波束多普勒声纳，它不但可以补偿偏流角测量中载体的姿态误差，而且可测量载体的对地速度，从而给出载体的空间绝对速度矢量。INS is composed of a gyroscope and an accelerometer. It is an autonomous navigation system that does not rely on external information. It can provide various navigation parameters including speed, position and attitude. It has the advantages of anti-interference and all-weather, so it is suitable for underwater diving. base navigation system for the device. The accuracy of INS mainly depends on the inertial device, and the navigation accuracy error will increase with the accumulation of time, so it is necessary to consider the error stability of its long-term voyage. ESGM is a "dumbbell" combination composed of two dual-degree-of-freedom electrostatic gyroscopes and an indirect stable platform. It serves as a high-precision and long-term stability information source to monitor and compensate the inertial navigation system. ESGM uses the principle of star navigation to monitor INS parameters, and INS provides ESGM with an indirect stable platform and calculation parameters. This combination can greatly suppress the divergence of navigation errors, prolong the readjustment period of the system, and ensure long-term high-precision cruise of underwater vehicles. The significant advantage of GPS is real-time navigation, positioning error has nothing to do with time, and has high positioning and speed measurement accuracy. When the antenna surfaced or the detection buoy was launched, the accurate position of the current point was obtained, the navigation error was calibrated, and the system time was corrected. DVL is based on the Doppler effect and mostly uses a four-beam Doppler sonar with a fixed emission direction. It can not only compensate the attitude error of the carrier in the measurement of the drift angle, but also measure the carrier’s ground velocity, thus giving the spatial absoluteness of the carrier. velocity vector.

如图1所示，组合导航系统所测量的导航信息经过预处理，主要是通过距离函数法对数据进行剔野值处理等，之后对采集到的导航数据进行时间、空间校准，然后经过变比例自适应联邦滤波方法对校准后的有效数据进行融合，得到最优解，最后输出导航数据。As shown in Figure 1, the navigation information measured by the integrated navigation system has been preprocessed, mainly through the distance function method to remove the wild value of the data, etc., and then the time and space calibration of the collected navigation data, and then through the scaling The adaptive federated filtering method fuses the calibrated effective data to obtain the optimal solution, and finally outputs the navigation data.

本发明的基于时间序列分析的变比例自适应联邦滤波方法，如图2所示，包括以下步骤。The variable-scale adaptive federated filtering method based on time series analysis of the present invention, as shown in FIG. 2 , includes the following steps.

步骤一、根据组合导航系统的各导航传感器误差方程建立系统状态方程和量测方程。本实施例中需要建立INS状态方程、INS/ESGM量测方程、INS/GPS量测方程和INS/DVL系统状态方程和量测方程。Step 1: Establish system state equations and measurement equations according to the error equations of each navigation sensor of the integrated navigation system. In this embodiment, the INS state equation, the INS/ESGM measurement equation, the INS/GPS measurement equation, the INS/DVL system state equation and the measurement equation need to be established.

步骤A、建立INS状态方程。Step A, establishing the INS state equation.

惯导系统INS的误差包括三通道位置、速度、姿态误差和惯性器件误差。三通道位置误差由纬度L、经度λ与高度h表征，三通道速度的误差由东向速度V_E、北向速度V_N与天向速度V_U表征，三通道的姿态误差由纵摇角、横摇角和偏航角表征，各误差方程分别为：The error of the inertial navigation system INS includes three-channel position, velocity, attitude error and inertial device error. The three-channel position error is characterized by latitude L, longitude λ, and height h; the three-channel velocity error is characterized by eastward velocity V _E , northward velocity V _N and skyward velocity V _U ; the three-channel attitude error is represented by pitch angle, lateral velocity The roll angle and yaw angle are characterized, and the error equations are:

$δ δ \overset{\cdot \cdot}{L L} = = \frac{δ δ {V V}_{N N}}{{R R}_{M m} + + h h}$

$δ δ \overset{\cdot \cdot}{λ λ} = = \frac{δ δ {V V}_{E E.}}{{R R}_{N N} + + h h} sec sec L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} sec sec LtgLδL LtgLδL$

$δ δ \overset{\cdot \cdot}{h h} = = δ δ {V V}_{U u}$

$δ δ {\overset{\cdot &Center Dot;}{V V}}_{E E.} = = {f f}_{N N} {φ φ}_{U u} - - {f f}_{U u} {φ φ}_{N N} + + ((\frac{{V V}_{E E.}}{{R R}_{M m} + + h h} tgL wxya - - \frac{{V V}_{U u}}{{R R}_{M m} + + h h})) {δV δV}_{E E.} + + (({22 ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) {δV δV}_{N N}$

$+ + ((22 {ω ω}_{ie ie} cos cos L L \cdot \cdot {V V}_{N N} + + \frac{{V V}_{N N} {V V}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L + + 22 {ω ω}_{ie ie} sin sin L L \cdot \cdot {V V}_{U u})) δL δ L - - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h})) δ δ {V V}_{U u} + + {&dtri; &dtri;}_{E E.}$

$δ δ {\overset{\cdot \cdot}{V V}}_{N N} = = {f f}_{U u} {φ φ}_{E E.} - - {f f}_{E E.} {φ φ}_{U u} - - 22 (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) {δV δV}_{E E.} - - \frac{{V V}_{U u}}{{R R}_{M m} + + h h} {δV δV}_{N N} - - \frac{{V V}_{N N}}{{R R}_{M m} + + h h} δ δ {V V}_{U u}$

$- - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L)) {V V}_{E E.} δL δ L + + {&dtri; &dtri;}_{N N}$

$δ δ {\overset{\cdot \cdot}{V V}}_{U u} = = {f f}_{E E.} {φ φ}_{N N} - - {f f}_{N N} {φ φ}_{E E.} + + 22 (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) δ δ {V V}_{E E.} + + 22 \frac{{V V}_{N N}}{{R R}_{M m} + + h h} δ δ {V V}_{N N} - - 22 {ω ω}_{ie ie} cos cos L L \cdot &Center Dot; {V V}_{E E.} δL δ L + + {&dtri; &dtri;}_{U u}$

${\overset{\cdot &Center Dot;}{φ φ}}_{E E.} = = - - \frac{δ δ {V V}_{N N}}{{R R}_{M m} + + h h} + + (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) {φ φ}_{N N} - - (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) {φ φ}_{U u} + + {ϵ ϵ}_{E E.}$

${\overset{\cdot \cdot}{φ φ}}_{N N} = = \frac{δ δ {V V}_{E E.}}{{R R}_{N N} + + h h} - - {ω ω}_{ie ie} sin sin LδL LδL - - (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) {φ φ}_{E E.} - - \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} {φ φ}_{U u} + + {ϵ ϵ}_{N N}$

${\overset{\cdot &Center Dot;}{φ φ}}_{U u} = = \frac{δ δ {V V}_{E E.}}{{R R}_{N N} + + h h} tgL wxya + + (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L)) δL δ L + + (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h})) {φ φ}_{E E.} + + \frac{{V V}_{N N}}{{R R}_{N N} + + h h} {φ φ}_{N N} + + {ϵ ϵ}_{U u}$

其中，

分别为纬度L、经度λ、高度h、东向速度V_E、北向速度V_N、天向速度V_U、纵摇角、横摇角、偏航角的变化率；δL、δλ、δh分别表示纬度L、经度λ、高度h的变化，δV_E、δV_N、δV_U分别表示东向速度V_E、北向速度V_N、天向速度V_U的变化；f_E、f_N、f_U分别为东向加速度、北向加速度、天向加速度，ω_ie为地球自转角速度，R_M为子午圈曲率半径，R_N为卯酉圈曲率半径。in,

are the change rates of latitude L, longitude λ, height h, eastward velocity V _E , northward velocity V _N , skyward velocity V _U , pitch angle, roll angle, and yaw angle; δL, δλ, and δh represent The changes of latitude L, longitude λ, and height h, δV _E , δV _N , and δV _U represent the changes of eastward velocity V _E , northward velocity V _N , and skyward velocity V _U respectively; f _E , f _N , and f _U are respectively Eastward acceleration, northward acceleration, and celestial acceleration, ω _ie is the angular velocity of the earth's rotation, R _M is the radius of curvature of the meridian circle, and R _N is the radius of curvature of the meridian circle.

惯性器件误差包括安装误差、刻度系数误差和随机误差等，安装误差和刻度系数误差因器件种类、型号、环境、工艺等条件各不相同，不具备统一性，因此本发明中只考虑随机误差。随机误差需要考虑陀螺漂移与加速度计误差。Inertial device error includes installation error, scale coefficient error and random error, etc. The installation error and scale coefficient error are different due to device type, model, environment, process and other conditions, and do not have unity. Therefore, only random error is considered in the present invention. Random errors need to account for gyro drift and accelerometer errors.

陀螺漂移为：ε＝ε_b+ε_r+ε_g；其中，ε_b表示随机常数，ε_r表示一阶马尔柯夫过程，ε_g为陀螺漂移的白噪声。三轴陀螺漂移方程为：Gyro drift is: ε＝ε _b +ε _r +ε _g ; among them, ε _b represents a random constant, ε _r represents a first-order Markov process, and ε _g is white noise of gyro drift. The three-axis gyro drift equation is:

${\overset{\cdot &Center Dot;}{ϵ ϵ}}_{b b} = = 00$

${\overset{\cdot &Center Dot;}{ϵ ϵ}}_{r r} = = - - \frac{11}{{T T}_{g g}} {ϵ ϵ}_{r r} + + {w w}_{g g}$

其中T_g表示三轴陀螺漂移的相关时间，w_g表示陀螺漂移马尔可夫过程的白噪声。where T _g represents the correlation time of the three-axis gyro drift, and w _g represents the white noise of the Markov process of the gyro drift.

加速度计误差可考虑为一阶马尔柯夫过程，加速度计的误差方程为：The accelerometer error can be considered as a first-order Markov process, and the error equation of the accelerometer is:

${\overset{\cdot &Center Dot;}{&dtri; &dtri;}}_{a a} = = - - \frac{11}{{T T}_{a a}} {&dtri; &dtri;}_{a a} + + {w w}_{a a}$

其中T_a表示加速度计误差的相关时间，w_a表示加速度计误差的白噪声。Where T _a represents the correlation time of the accelerometer error, and w _a represents the white noise of the accelerometer error.

由惯导系统INS的误差方程，可确定INS状态方程为：From the error equation of the inertial navigation system INS, the state equation of the INS can be determined as:

$\overset{\cdot &Center Dot;}{X x} ((t t)) = = F f ((t t)) X x ((t t)) + + G G ((t t)) W W ((t t))$

其中，状态量

包括了三通道位置、速度和姿态及陀螺误差和加速度计误差，ε_bx、ε_by、ε_bz为陀螺漂移随机常数误差在三轴上的分量，ε_rx、ε_ry、ε_rz为陀螺漂移马尔可夫过程误差在三轴上的分量，

为加速度计误差在三轴上的分量，所述的三轴指x轴、y轴和z轴。Among them, the state quantity

Including the three-channel position, velocity and attitude, gyro error and accelerometer error, ε _bx , ε _by , ε _bz are the components of the gyro drift random constant error on the three axes, ε _rx , ε _ry , ε _rz are the gyro drift Marr The components of the Cove process error on the three axes,

is the component of the accelerometer error on the three axes, and the three axes refer to the x-axis, y-axis and z-axis.

过程噪声阵W＝[w_bx w_by w_bz w_gx w_gy w_gz w_ax w_ay w_az]^T，w_bx、w_by、w_bz分别表示陀螺漂移随机常数的白噪声在三轴上的分量，w_gx、w_gy、w_gz分别表示陀螺漂移马尔可夫过程的白噪声在三轴上的分量，w_ax、w_ay、w_az分别表示加速度计误差的白噪声在三轴上的分量。Process noise matrix W＝[w _bx w _by w _bz w _gx w _gy w _gz w _ax w _ay w _az ] ^T , w _bx , w _by , w _bz represent the components of the white noise of the gyro drift random constant on the three axes respectively , w _gx , w _gy , w _gz represent the three-axis components of the white noise of the gyro drift Markov process, respectively, and w _ax , _way , w _az represent the three-axis components of the white noise of the accelerometer error.

噪声驱动阵

其中

是方向余弦矩阵，b指的是船体坐标系，固连在船体上，n指的是导航坐标系，根据导航系统工作状态选取。noise driven array

in

is the direction cosine matrix, b refers to the hull coordinate system, which is fixed on the hull, and n refers to the navigation coordinate system, which is selected according to the working state of the navigation system.

状态转移阵

其中，

其中T_gx、T_gy、T_gz分别表示陀螺漂移的相关时间在三轴上的分量，T_ax、T_ay、T_az分别表示加速度计误差的相关时间在三轴上的分量；state transition matrix

in,

Among them, T _gx , T _gy , and T _gz respectively represent the components of the relative time of the gyro drift on the three axes, and T _ax , T _ay , and T _az respectively represent the components of the relative time of the accelerometer error on the three axes;

步骤B、建立INS/ESGM量测方程。Step B, establishing the INS/ESGM measurement equation.

静电陀螺监控器ESGM监测、补偿惯导系统INS，但它只能提供位置及航向信息。INS/ESGM系统状态方程使用INS状态方程，使X₁＝X。ESGM和INS位置、航向信息的差值为观测量Z₁：The electrostatic gyro monitor ESGM monitors and compensates the inertial navigation system INS, but it can only provide position and heading information. The INS/ESGM system state equation uses the INS state equation, so that X ₁ =X. The difference between ESGM and INS position and heading information is observation Z ₁ :

${Z Z}_{11} = = [\begin{matrix} {L L}_{E E.} - - L L \\ {λ λ}_{E E.} - - λ λ \\ {h h}_{E E.} - - h h \\ {K K}_{E E.} - - K K \end{matrix}] + + [\begin{matrix} {m m}_{11} \\ {m m}_{22} \\ {m m}_{33} \\ {m m}_{44} \end{matrix}]$

其中，L_E、L分别表示ESGM和INS的纬度，λ_E、λ分别表示ESGM和INS的经度，h_E、h分别表示ESGM和INS的高度；K_E、K分别表示ESGM和INS的航向角。[m₁，m₂，m₃，m₄]^T是由ESGM的位置误差及方位误差产生的零均值的高斯白噪声。Among them, L _E , L represent the latitude of ESGM and INS respectively, λ _E , λ represent the longitude of ESGM and INS respectively, h _E , h represent the altitude of ESGM and INS respectively; K _E , K represent the heading angles of ESGM and INS respectively . [m ₁ , m ₂ , m ₃ , m ₄ ] ^T is Gaussian white noise with zero mean value generated by ESGM position error and azimuth error.

相应的INS/ESGM量测方程为：The corresponding INS/ESGM measurement equation is:

Z₁(t)＝H₁(t)X₁(t)+V₁(t)Z ₁ (t) = H ₁ (t) X ₁ (t) + V ₁ (t)

其中，V₁(t)＝[m₁，m₂，m₃，m₄]^T是由ESGM的位置误差及方位误差产生的零均值的高斯白噪声。H₁(t)是量测阵，INS/ESGM状态方程X₁(t)＝X(t)。Among them, V ₁ (t)=[m ₁ , m ₂ , m ₃ , m ₄ ] ^T is Gaussian white noise with zero mean value generated by ESGM position error and orientation error. H ₁ (t) is the measurement array, and the INS/ESGM state equation X ₁ (t)=X(t).

步骤C、确定INS/GPS量测方程。Step C, determining the INS/GPS measurement equation.

GPS误差包括：SA误差，电离层传输误差，对流层传输误差，星历误差，以及时钟同步误差等。在位置、速度组合模式下，将GPS上述误差视为量测噪声，不进行状态扩充，取X₂(t)＝X(t)。GPS errors include: SA error, ionospheric transmission error, tropospheric transmission error, ephemeris error, and clock synchronization error. In the combination mode of position and speed, the above-mentioned errors of GPS are regarded as measurement noise, and state expansion is not performed, and X ₂ (t)=X(t) is taken.

观测量Z₂为GPS与INS位置差值和速度差值。INS/GPS系统状态方程使用INS状态方程：X₂(t)＝X(t)，INS/GPS量测方程为：Observation Z ₂ is the position difference and speed difference between GPS and INS. The INS/GPS system state equation uses the INS state equation: X ₂ (t)=X(t), and the INS/GPS measurement equation is:

${Z Z}_{22} = = [\begin{matrix} {Z Z}_{p p} ((t t)) \\ {Z Z}_{V V} ((t t)) \end{matrix}] = = {H h}_{22} ((t t)) {X x}_{22} ((t t)) + + {v v}_{22} ((t t))$

其中，位置量测量

速度量测量

量测阵

v₂(t)表示由GPS的位置误差及方位误差产生的零均值的高斯白噪声阵。L_G表示GPS的纬度，λ_G表示GPS的经度，h_G表示GPS的高度，V_GE表示GPS中的东向速度，V_GN表示GPS中的北向速度，V_GU表示GPS中的天向速度。Among them, the position measurement

Velocity measurement

Measurement array

v ₂ (t) represents the zero-mean Gaussian white noise array generated by GPS position error and azimuth error. L _G represents the latitude of GPS, λ _G represents the longitude of GPS, h _G represents the altitude of GPS, V _GE represents the eastward velocity in GPS, V _GN represents the northward velocity in GPS, and V _GU represents the skyward velocity in GPS.

步骤D、确定INS/DVL系统方程。Step D, determining the INS/DVL system equation.

多普勒计程仪可提供对地速度和偏流角，它的测量误差主要有速度偏移误差δV_d，刻度系数误差δC和偏流角误差δΔ，测速误差和偏流角测量误差可用一阶马尔可夫过程描述，刻度系数误差为随机常数，误差方程如下：The Doppler log can provide ground velocity and drift angle. Its measurement errors mainly include velocity offset error δV _d , scale coefficient error δC and drift angle error δΔ. The process description of the husband, the scale coefficient error is a random constant, and the error equation is as follows:

$\{\begin{matrix} δ δ {\overset{\cdot \cdot}{V V}}_{d d} = = - - {β β}_{d d} δ δ {V V}_{d d} + + {w w}_{d d} \\ δ δ \overset{\cdot &Center Dot;}{Δ Δ} = = - - {β β}_{Δ Δ} δΔ δΔ + + {w w}_{Δ Δ} \\ δ δ \overset{\cdot \cdot}{C C} = = 00 \end{matrix}$

$[\begin{matrix} δ δ {\overset{\cdot &Center Dot;}{V V}}_{d d} \\ δ δ \overset{\cdot \cdot}{Δ Δ} \\ δ δ \overset{\cdot \cdot}{C C} \end{matrix}] = = [\begin{matrix} - - {β β}_{d d} \\ - - {β β}_{Δ Δ} \\ 00 \end{matrix}] [\begin{matrix} {δV δV}_{d d} \\ δΔ δΔ \\ δC δC \end{matrix}] + + [\begin{matrix} {w w}_{d d} \\ {w w}_{Δ Δ} \\ 00 \end{matrix}]$

其中，δV_d、δΔ、δC分别表示速度误差、偏流角误差、刻度系数误差的变化率，β_d、β_Δ分别为速度误差马尔可夫过程、偏流角误差马尔可夫过程的相关时间常数的倒数，w_d、w_Δ表示速度误差、偏流角误差的白噪声。于是将X_d＝[δV_d δΔ δC]^T作为扩充系统状态阵，与INS误差方程组成INS/GPS的系统状态方程：Among them, δV _d , δΔ, and δC represent the rate of change of velocity error, bias angle error, and scale coefficient error, respectively, and β _d , β _Δ are the relative time constants of the velocity error Markov process and bias angle error Markov process, respectively. Reciprocal, w _d , w _Δ represent the white noise of speed error and bias angle error. Therefore, X _d = [δV _d δΔ δC] ^T is used as the extended system state matrix, and the INS error equation forms the INS/GPS system state equation:

${\overset{\cdot \cdot}{X x}}_{33} = = [\begin{matrix} \overset{\cdot \cdot}{X x} \\ {\overset{\cdot \cdot}{X x}}_{d d} \end{matrix}] = = [\begin{matrix} F f \\ {F f}_{d d} \end{matrix}] [\begin{matrix} X x \\ {X x}_{d d} \end{matrix}] + + [\begin{matrix} G G \\ {G G}_{d d} \end{matrix}] [\begin{matrix} W W \\ {W W}_{d d} \end{matrix}]$

其中矩阵G_d＝diag[1 1 0]。F_d是多普勒测速系统的增广状态转移阵，W_d是多普勒测速子系统的增广过程噪声阵。Wherein the matrix G _d =diag[1 1 0]. F _d is the augmented state transition matrix of the Doppler velocity measurement system, and W _d is the augmented process noise matrix of the Doppler velocity measurement subsystem.

观测量Z₃(t)为DVL与INS的速度差值，DVL/INS的观测方程为：The observed value Z ₃ (t) is the speed difference between DVL and INS, and the observation equation of DVL/INS is:

${Z Z}_{33} ((t t)) = = [\begin{matrix} {V V}_{E E.} - - {V V}_{dE E} \\ {V V}_{N N} - - {V V}_{dN dN} \end{matrix}] = = {H h}_{33} ((t t)) {X x}_{33} ((t t)) + + {V V}_{33} ((t t))$

其中，量测阵

V₃(t)＝[m_VE m_VN]^T为观测白噪声，K′是船的航向角，V_dE、V_dN表示多普勒测量的东向速度、北向速度。Among them, the measurement array

V ₃ (t)=[m _VE m _VN ] ^T is the observation white noise, K′ is the heading angle of the ship, V _dE and V _dN represent the eastward velocity and northward velocity measured by Doppler.

步骤二、将状态方程和量测方程离散化，建立离散状态空间模型。Step 2, discretize the state equation and the measurement equation, and establish a discrete state space model.

本发明的联邦滤波方法通过调整变比例权重，使主滤波器和子滤波器的作用权重与导航设备的工作状况和导航数据的有效状况相适应，主滤波为子滤波提供反馈信息，具有良好的性能。应用本发明方法的变比例自适应联邦滤波结构如图3所示。惯导系统INS将状态量提供给主滤波器，静电陀螺监控器ESGM和INS产生观测量Z₁向第一子滤波器发送信号，卫星定位导航系统GPS和INS产生观测量Z₂向第二子滤波器发送信号，多普勒测速系统DVL和INS产生观测量Z3向第三子滤波器发送信号，经三个子滤波器滤波后的信号发送给主滤波器。根据惯导系统INS、静电陀螺监控器ESGM、卫星定位导航系统GPS以及多普勒测速系统DVL的历史数据通过自回归(AutoRegressive，简称AR)模型进行比例权值计算得出信息权值Θ_i，然后提供给主滤波器作为信息分配比例计算的依据，完成一个滤波周期后主滤波器向INS以及三个子滤波器提供全局估计值

估计均方误差P_g和信息分配比例β_i以重置。经过不断时间更新和量测更新，得出最优估计。The federated filtering method of the present invention adapts the weights of the main filter and the sub-filters to the working conditions of the navigation equipment and the effective status of the navigation data by adjusting the variable ratio weights, and the main filter provides feedback information for the sub-filters, which has good performance . The variable scale adaptive federated filtering structure applying the method of the present invention is shown in FIG. 3 . The inertial navigation system INS provides the state quantity to the main filter, the electrostatic gyro monitor ESGM and INS generate observation Z ₁ to send a signal to the first sub-filter, and the satellite positioning and navigation system GPS and INS generate observation Z ₂ to the second sub-filter The filter sends a signal, and the Doppler speed measurement system DVL and INS generate an observation Z3 to send a signal to the third sub-filter, and the signal filtered by the three sub-filters is sent to the main filter. According to the historical data of inertial navigation system INS, electrostatic gyro monitor ESGM, satellite positioning and navigation system GPS and Doppler speed measurement system DVL, the information weight Θ _i is obtained by calculating the proportional weight value through the AutoRegressive (AR) model, Then it is provided to the main filter as the basis for the calculation of the information distribution ratio. After completing a filtering cycle, the main filter provides the global estimated value to the INS and the three sub-filters

Estimate the mean square error P _g and information distribution ratio β _i to reset. After continuous time update and measurement update, the optimal estimate is obtained.

将主滤波器和各子系统状态方程和量测方程都离散化，本发明实施例中对INS状态方程、INS/ESGM量测方程、INS/GPS量测方程以及INS/DVL状态方程和量测方程都进行离散化。对应状态方程和量测方程离散化后建立的离散状态空间模型为：The main filter and each subsystem state equation and measurement equation are all discretized. In the embodiment of the present invention, the INS state equation, INS/ESGM measurement equation, INS/GPS measurement equation and INS/DVL state equation and measurement All equations are discretized. The discrete state space model established after discretization of the corresponding state equation and measurement equation is:

其中，k表示离散时间状态的索引值，X_k表示k时刻的状态量，Z_k表示k时刻的观测量，H_k表示k时刻的量测阵，Γ_k/k-1是噪声驱动阵，W_k-1为系统状态噪声，且过程噪声协方差阵Q_k＝E[W_kW_k ^T]，V_k为系统量测噪声，且量测噪声协方差阵R_k＝E[V_kV_k ^T]。状态转移阵Φ_k/k-1采用分步累积离散法得到：Among them, k represents the index value of the discrete-time state, X _k represents the state quantity at time k, Z _k represents the observed quantity at time k, H _k represents the measurement array at time k, Γ _k/k-1 is the noise-driven array, W _k-1 is the system state noise, and the process noise covariance matrix Q _k ＝E[W _k W _k ^T ], V _k is the system measurement noise, and the measurement noise covariance matrix R _k ＝E[V _k V _kT ^] . The state transition matrix Φ _k/k-1 is obtained by step-by-step cumulative discrete method:

步骤三、根据各导航传感器系统之前测得的历史数据，通过自回归模型得出信息权值。Step 3, according to the historical data previously measured by each navigation sensor system, an information weight is obtained through an autoregressive model.

导航传感器系统信息权值的计算基于导航传感器系统之前测得的若干历史数据，通过自回归模型(AR模型)分析得出。下面以某一个导航传感器系统为例来说明。The calculation of the information weight of the navigation sensor system is based on some historical data measured before by the navigation sensor system, and is obtained through the analysis of the autoregressive model (AR model). Let's take a certain navigation sensor system as an example to illustrate.

设y(τ)为该导航传感器系统的输出序列，y(τ)的AR模型表示为：Let y(τ) be the output sequence of the navigation sensor system, and the AR model of y(τ) is expressed as:

其中，τ表示序列y(τ)的第τ个点，N表示AR模型的阶数，a_k为均值为零、方差为σ²的高斯白噪声，a_k表示模型误差，

表示N阶AR模型系数，可以得到y(τ)的功率谱密度S_y(ω)为：Among them, τ represents the τth point of the sequence y(τ), N represents the order of the AR model, a _k is Gaussian white noise with zero mean and variance ^σ2 , a _k represents the model error,

Representing the Nth-order AR model coefficients, the power spectral density S _y (ω) of y(τ) can be obtained as:

表示τ阶AR模型系数，e^-jωτ表示系统的频率响应，ω表示角频率，j表示虚部单位。y(τ)的自相关函数阵R_N+1为：

Indicates the τ order AR model coefficient, e ^-jωτ indicates the frequency response of the system, ω indicates the angular frequency, and j indicates the imaginary part unit. The autocorrelation function matrix R _N+1 of y(τ) is:

${R R}_{N N + + 11} = = [\begin{matrix} {\overset{^^}{r r}}_{y the y} ((00)) & {\overset{^^}{r r}}_{y the y} ((11)) & {\overset{^^}{r r}}_{y the y} ((22)) & . . . . . . & {\overset{^^}{r r}}_{y the y} ((N N)) \\ {\overset{^^}{r r}}_{y the y} ((11)) & {\overset{^^}{r r}}_{y the y} ((00)) & {\overset{^^}{r r}}_{y the y} ((22)) & . . . . . . & {\overset{^^}{r r}}_{y the y} ((N N - - 11)) \\ {\overset{^^}{r r}}_{y the y} ((22)) & {\overset{^^}{r r}}_{y the y} ((11)) & {\overset{^^}{r r}}_{y the y} ((00)) & . . . . . . & {\overset{^^}{r r}}_{y the y} ((N N - - 22)) \\ . . & . . & . . & . . & . . \\ . . & . . & . . & . . & . . \\ . . & . . & . . & . . & . . \\ {\overset{^^}{r r}}_{y the y} ((N N)) & {\overset{^^}{r r}}_{y the y} ((N N - - 11)) & {\overset{^^}{r r}}_{y the y} ((N N - - 22)) & . . . . . . & {\overset{^^}{r r}}_{y the y} ((00)) \end{matrix}]$

根据Yule-Waker方程，已知y(τ)的自相关函数R_N+1可以求出AR模型的模型系数

假设

为AR模型系数

的估计值，i＝1，2，…N，

为N阶AR模型的误差功率，

为预测的最小误差功率，

根据Toeplita矩阵的性质，通过Levinson-Durbin递推算法确定模型系数：According to the Yule-Waker equation, the autocorrelation function R _N+1 of y(τ) is known and the model coefficient of the AR model can be obtained

suppose

is the AR model coefficient

Estimated value of , i=1, 2, ... N,

is the error power of the Nth-order AR model,

is the predicted minimum error power,

According to the properties of the Toeplita matrix, the model coefficients are determined by the Levinson-Durbin recursive algorithm:

通过最终预测误差(FPE)法则得到阶数：The order is obtained by the Final Prediction Error (FPE) law:

$FPE FPE ((N N)) = = {\overset{^^}{ρ ρ}}_{N N} \frac{Ξ ξ + + ((N N + + 11))}{Ξ ξ - - ((N N + + 11))}$

其中，Ξ为测量数据的长度。Wherein, Ξ is the length of the measurement data.

从而得到传感器输出的预测值：The predicted value of the sensor output is thus obtained:

由于AR模型是建立在平稳解空间上的全极点模型，所以预测误差可以反映传感器输出的平滑性。将每个传感器的预测误差作为信息权值Θ_i，信息权越小，预测误差越小，导航传感器输出越平滑。所述的信息权值为：Since the AR model is an all-pole model built on a stationary solution space, the prediction error can reflect the smoothness of the sensor output. The prediction error of each sensor is taken as the information weight Θ _i , the smaller the information weight, the smaller the prediction error, and the smoother the output of the navigation sensor. The information weights described are:

y_i(τ)为第i个导航传感器的输出序列，

为第i个导航传感器的输出序列的预测值，i＝0，1，2，3分别对应惯导系统、静电陀螺监控器、卫星定位导航系统和多普勒测速系统。y _i (τ) is the output sequence of the i-th navigation sensor,

is the predicted value of the output sequence of the i-th navigation sensor, and i=0, 1, 2, 3 respectively correspond to the inertial navigation system, electrostatic gyro monitor, satellite positioning navigation system and Doppler velocity measurement system.

步骤四、根据信息权值和信息守恒定律计算信息分配比例。Step 4: Calculate the information distribution ratio according to the information weight and the law of information conservation.

信息分配比例选取原则是：子系统精度越高，输出越可靠，越要减小主系统估计值的作用，信息分配比越小。所述的主系统是指基准系统，本发明实施例中为惯导系统，子系统为静电陀螺监控器、卫星定位导航系统和多普勒测速系统为自系统。因此取信息分配比例和信息权值成正比。同时，根据信息守恒：其中β₀为主系统权重，β_i为各子系统权重，n为所有子系统的个数。因此最后得到各导航传感器系统的权重为：The selection principle of information distribution ratio is: the higher the accuracy of the subsystem, the more reliable the output, the more the role of the estimated value of the main system should be reduced, the smaller the information distribution ratio. The main system refers to the reference system, which is the inertial navigation system in the embodiment of the present invention, the subsystem is the electrostatic gyro monitor, the satellite positioning and navigation system and the Doppler speed measurement system are self-systems. Therefore, the information distribution ratio is proportional to the information weight. At the same time, according to information conservation: Among them, β ₀ is the weight of the main system, β _i is the weight of each subsystem, and n is the number of all subsystems. Therefore, the final weight of each navigation sensor system is:

${β β}_{i i} = = \frac{{Θ Θ}_{i i}}{{Σ Σ}_{i i = = 00}^{n no} {Θ Θ}_{i i}}$

步骤五、根据信息分配比例进行联邦滤波，得到全局最优估计。Step 5: Perform federated filtering according to the information distribution ratio to obtain the global optimal estimate.

首先得到信息分配：First get the information assignment:

${P P}_{i i,, k k - - 11} = = {β β}_{i i}^{- - 11} {P P}_{k k - - 11}$

${Q Q}_{i i,, k k - - 11} = = {β β}_{i i}^{- - 11} {Q Q}_{k k - - 11}$

X_i，k-1＝X_k-1 X _{i, k-1} = X _k-1

将第i个导航传感器系统的估计误差方差阵P_k-1通过比例β_i分配为重置后的估计误差方差阵P_i，k-1，又将第i个导航传感器系统的过程噪声协方差阵Q_k-1通过比例β分配为重置后的过程噪声协方差阵Q_i，k-1。X_i，k-1表示将第i个导航传感器系统的状态量，k表示离散时间状态的索引值。The estimated error variance matrix P _k-1 of the i-th navigation sensor system is assigned to the reset estimated error variance matrix P _i,k-1 through the ratio β _i , and the process noise covariance of the i-th navigation sensor system is The matrix Q _k-1 is assigned by the ratio β as the reset process noise covariance matrix Q _i,k-1 . X _{i, k-1} represents the state quantity of the i-th navigation sensor system, and k represents the index value of the discrete time state.

其次进行时间更新：Then update the time:

${\overset{^^}{X x}}_{i i,, k k / / k k - - 11} = = {Φ Φ}_{i i,, k k / / k k - - 11} {\overset{^^}{X x}}_{i i,, k k - - 11}$

${P P}_{i i,, k k / / k k - - 11} = = {Φ Φ}_{i i,, k k / / k k - - 11} {P P}_{i i,, k k - - 11} {Φ Φ}_{i i,, k k / / k k - - 11}^{T T} + + {Γ Γ}_{i i,, k k / / k k - - 11} {Q Q}_{i i,, k k - - 11} {Γ Γ}_{i i,, k k / / k k - - 11}^{T T}$

为第i个导航传感器系统的状态一步预测阵，P_i，k/k-1为第i个导航传感器系统的一步预测均方误差阵，Φ_i，k/k-1为第i个导航传感器系统的状态转移阵，Γ_i，k/k-1为第i个导航传感器系统的噪声驱动阵，

为状态量X_i，k-1的预测值。

is the one-step prediction matrix of the i-th navigation sensor system, P _{i, k/k-1} is the one-step prediction mean square error matrix of the i-th navigation sensor system, Φ _{i, k/k-1} is the i-th navigation sensor The state transition matrix of the system, Γ _{i, k/k-1} is the noise driving matrix of the i-th navigation sensor system,

is the predicted value of state quantity Xi _,k-1 .

然后进行观测更新：Then do an observation update:

${K K}_{i i,, k k} = = {P P}_{i i,, k k / / k k - - 11} {H h}_{i i,, k k}^{T T} {(({H h}_{i i,, k k} {P P}_{i i,, k k / / k k - - 11} {H h}_{i i,, k k}^{T T} + + {R R}_{i i,, k k}))}^{- - 11}$

${\overset{^^}{X x}}_{i i,, k k} = = {\overset{^^}{X x}}_{i i,, k k / / k k - - 11} + + {K K}_{i i,, k k} (({Z Z}_{i i,, k k} - - {H h}_{i i,, k k} {\overset{^^}{X x}}_{i i,, k k / / k k - - 11}))$

${P P}_{i i,, k k} = = ((I I - - {K K}_{i i,, k k} {H h}_{i i,, k k})) {P P}_{i i,, k k / / k k - - 11} {((I I - - {K K}_{i i,, k k} {H h}_{i i,, k k}))}^{T T} + + {K K}_{i i,, k k} {R R}_{i i,, k k} {K K}_{i i,, k k}^{T T}$

K_i，k为第i个导航传感器系统的滤波增益，H_i，k为第i个导航传感器系统的量测阵，R_i，k是由第i个导航传感器系统的系统量测噪声协方差阵，Z_i，k为第i个导航传感器系统的观测量。k表示离散时间状态的索引值。K _{i, k} is the filter gain of the i-th navigation sensor system, H _{i, k} is the measurement array of the i-th navigation sensor system, R _{i, k} is the system measurement noise covariance of the i-th navigation sensor system array, Z _i,k is the observation of the i-th navigation sensor system. k represents the index value of the discrete-time state.

最后得到全局优化值：Finally, the global optimization value is obtained:

${\overset{^^}{X x}}_{g g} = = {P P}_{g g} {Σ Σ}_{i i = = 11}^{m m} {P P}_{i i,, k k}^{- - 11} {\overset{^^}{X x}}_{i i,, k k}$

${P P}_{g g} = = {(({Σ Σ}_{i i = = 11}^{m m} {P P}_{i i,, k k}^{- - 11}))}^{- - 11}$

为全局估计值，P_g为估计均方误差，m是子滤波器个数，本发明实施例m为3。

is the global estimated value, P _g is the estimated mean square error, m is the number of sub-filters, and m is 3 in the embodiment of the present invention.

用全局最优解重置滤波值和估计误差方差阵。由于在分配系统噪声时使用方差上界技术，各子系统互不相关，增广系统也不耦合。虽然子系统的滤波结果是次优的，但融合时被重新合成，所以全局估计最优。Resets the filtered values and estimated error variance matrix with the global optimal solution. Since the variance upper bound technique is used when assigning system noise, the subsystems are not correlated with each other, and the augmented system is not coupled. Although the filtering results of the subsystems are suboptimal, they are resynthesized during fusion, so the global estimation is optimal.

下面通过数学分析软件matlab环境下的组合导航系统进行仿真实验。仿真实验为8小时。设置如下参数，本发明提供的变比例联邦滤波方法与INS的误差结果进行比较。INS常值漂移0.1°/h，漂移马尔可夫过程时间常数T_g为300s，加表零偏为0.1mg，加表马尔可夫过程时间常数T_a为1000s。ESGM白噪声0.04°/h。GPS输出加入随机分布的位置、速度误差。DVL误差0.4m/s。The simulation experiment is carried out through the integrated navigation system under the environment of the mathematical analysis software matlab. The simulation experiment is 8 hours. The following parameters are set, and the variable-scale federated filtering method provided by the present invention is compared with the error result of the INS. The INS constant value drifts 0.1°/h, the drift Markov process time constant T _g is 300s, the zero offset of the added meter is 0.1mg, and the added meter Markov process time constant T _a is 1000s. ESGM white noise 0.04°/h. The GPS output adds randomly distributed position and velocity errors. DVL error 0.4m/s.

比例权重变化曲线如图4。从图4中可以反映传感器不同工作条件下的比例权值变化情况，当INS发挥主要作用时，INS的权重β₀要低于其他比例值，当GPS或DVL进行校正时，其比例权值要高于其他系统，突出了校正系统的作用。得到系统误差曲线如图5与图6。其中红色实线代表本发明实施例的水下多传感器组合导航系统的误差，黑色虚线代表INS系统误差。如图5和图6所示，组合系统位置误差峰值的绝对值为20m，速度误差峰值的绝对值为0.8m/s，且不发散。而INS位置误差为500m，速度误差5m/s，且不断增加。可以看出，本发明提供的联邦滤波方法得到的导航信息精度优于单传感器系统，而且系统输出在一定幅度内保持平稳而不发散，克服了惯导系统误差积累(尤其位置误差)的弱点，提高了系统的可靠性与稳定性。在导航传感器信息权不断变化，尤其是GPS和DVL校正切换的较大跳变的情况下计算得到的位置误差方差为8.92，速度误差方差0.065，可见使用本发明方法的组合导航系统性能比较平稳。如图7所示，使用本发明变比例自适应联邦滤波方法的的组合导航系统的动态性能和位置误差幅度，与使用常规联邦滤波方法的组合导航系统相比，使用本发明方法的组合导航系统的位置误差幅度变化小，具有明显优势，能够更加真实地反映组合导航系统的情况。The proportional weight change curve is shown in Figure 4. From Fig. 4, it can reflect the change of the proportional weight of the sensor under different working conditions. When the INS plays the main role, the weight _β0 of the INS is lower than other proportional values. When the GPS or DVL is corrected, the proportional weight should be Higher than other systems, highlighting the role of the correction system. The system error curves are obtained as shown in Figure 5 and Figure 6. The red solid line represents the error of the underwater multi-sensor integrated navigation system according to the embodiment of the present invention, and the black dotted line represents the error of the INS system. As shown in Figure 5 and Figure 6, the absolute value of the peak value of the position error of the combined system is 20m, the absolute value of the peak value of the speed error is 0.8m/s, and it does not diverge. The INS position error is 500m, and the speed error is 5m/s, and it keeps increasing. It can be seen that the accuracy of navigation information obtained by the federated filtering method provided by the present invention is better than that of a single sensor system, and the system output remains stable within a certain range without divergence, which overcomes the weakness of inertial navigation system error accumulation (especially position error), Improve the reliability and stability of the system. When the information weight of the navigation sensor is constantly changing, especially when the GPS and DVL correction switching is greatly changed, the calculated position error variance is 8.92, and the speed error variance is 0.065. It can be seen that the performance of the integrated navigation system using the method of the present invention is relatively stable. As shown in Figure 7, the dynamic performance and position error range of the integrated navigation system using the variable scale adaptive federated filtering method of the present invention, compared with the integrated navigation system using the conventional federated filtering method, the integrated navigation system using the method of the present invention The variation of position error range is small, which has obvious advantages, and can more truly reflect the situation of the integrated navigation system.

本发明通过分析导航传感器前向的若干数据，采用时间序列分析得到信息权值，再计算滤波器权值，以此改变信息分配的比例，达到提高系统精度、可靠性和容错性的目的。相比于其它自适应联邦滤波，本系统信息权的确定是根据导航传感器的源信息，而非经卡尔曼滤波处理后的估计值，这样使权值的确定更加接近导航传感器的真实情况，从而更加真实地反映系统的情况。The present invention analyzes several forward data of navigation sensors, adopts time series analysis to obtain information weights, and then calculates filter weights, thereby changing the ratio of information distribution and achieving the purpose of improving system accuracy, reliability and fault tolerance. Compared with other adaptive federated filters, the determination of the information weight of this system is based on the source information of the navigation sensor, rather than the estimated value processed by the Kalman filter, so that the determination of the weight is closer to the real situation of the navigation sensor, thus More realistically reflect the situation of the system.

Claims

1. A time series analysis-based variable proportion self-adaptive federal filtering method is characterized by comprising the following steps:

firstly, establishing a system state equation and a measurement equation according to an error equation of each navigation sensor system of the integrated navigation system;

discretizing the obtained system state equation and measurement equation, and establishing a discrete state space model corresponding to each navigation sensor system;

thirdly, acquiring an information weight of the navigation sensor system through an autoregressive model according to historical data of the navigation sensor system;

step four, obtaining an information distribution proportion according to the information weight and an information conservation law;

and fifthly, carrying out federal filtering according to the information distribution proportion to obtain global optimal estimation, and finally resetting the filtering value and the estimation error variance matrix by using the global optimal estimation.

2. The variable-proportion self-adaptive federal filtering method based on time series analysis according to claim 1, wherein the integrated navigation system is an underwater multi-sensor integrated navigation system, and is composed of four navigation sensor systems: inertial navigation system INS, static gyro monitor ESGM, satellite positioning navigation system GPS and Doppler velocity measurement system DVL.

3. The time series analysis-based variable-scale adaptive federated filtering method according to claim 1 or 2, wherein the system state equation and the measurement equation of step one include: an INS state equation, an INS/ESGM measurement equation, an INS/GPS measurement equation, an INS/DVL system state equation and a measurement equation;

the INS state equation is as follows:

wherein the state quantity

δ L, δ λ, δ h represent changes in latitude L, longitude λ, altitude h, respectively, and δ V_E、δV_N、δV_URespectively represent east velocity V_EVelocity V in the north direction_NVelocity in the direction of the sky V_UChange of (phi)_E、φ_N、φ_URespectively representing pitch angle, roll angle, yaw angle, epsilon_bx、ε_by、ε_bzIs the component of gyro drift random constant error on three axes, epsilon_rx、ε_ry、ε_rzFor the components of the gyro drift markov process error in the three axes,

is the component of the accelerometer error in three axes; process noise array W ═ W_bx w_by w_bz w_gx w_gy w_gz w_ax w_ay w_az]^T，w_bx、w_by、w_bzComponent on three axes of white noise, w, representing the gyro drift random constant_gx、w_gy、w_gzComponent of white noise on three axes, w, representing a gyro-drift Markov process_ax、w_ay、w_azA component of white noise on three axes representing accelerometer error; the three axes are an x axis, a y axis and a z axis; noise driving array

Wherein

Is a directional cosine matrix; state transition array

Wherein,

T_gx、T_gy、T_gzrespectively representing the components of the gyro drift with respect to time on three axes, T_ax、T_ay、T_azRespectively representing the components of the accelerometer error with respect to time in three axes,

wherein f is_EFor acceleration in east directionDegree f_NIs the north acceleration, f_UAcceleration in the direction of the sky, ω_ieIs the rotational angular velocity of the earth, R_MRadius of curvature of meridian, R_NThe curvature radius of the prime circle is;

the INS/ESGM measurement equation is as follows: z₁(t)＝H₁(t)X₁(t)+V₁(t)；

Wherein, V₁(t)＝[m₁，m₂，m₃，m₄]^TIs zero-mean white Gaussian noise, H, generated by the ESGM position and orientation errors₁(t) is a measurement array, the observed quantity Z₁(t) is the difference value of the ESGM, the INS and the course information; INS/ESGM State equation X₁(t)＝X(t)；

The INS/GPS measurement equation is as follows:

wherein the position quantity is measured

Measurement of speedMeasuring array

v₂(t) the observed quantity Z is a white Gaussian noise array of zero mean value generated by the position error and the azimuth error of the GPS₂(t) is the difference between the GPS and INS positions and velocities; l is_GIndicating the latitude, λ, of the GPS_GIndicates the longitude, h, of the GPS_GIndicating the height, V, of the GPS_GEIndicating east velocity, V, in GPS_GNIndicating north velocity, V, in GPS_GURepresenting the speed of the sky in GPS; INS/GPS equation of state X₂(t)＝X(t)；

The INS/DVL system state equation is as follows:

wherein, the noise of the Doppler velocity measurement system drives the array G_d(t)＝diag[1 1 0]，F_d(t) is the augmented state transition array of the Doppler velocimetry system, W_d(t) noise matrix of the augmentation Process of Doppler velocimetry System, X_d(t) is the Doppler velocimetry system augmented state quantity, X_d＝[δV_d δΔ δC]^T，δV_dThe measurement error is a speed deviation error in the Doppler velocity measurement system, delta is a drift angle error in the Doppler velocity measurement system, and delta C is a scale coefficient error in the Doppler velocity measurement system;

the INS/DVL measurement equation is as follows:

wherein the measuring array

V₃(t)＝[m_VE m_VN]^TTo observe white noise, V_dE、V_dNRespectively represents the east speed and the north speed in the Doppler velocity measurement system, and K' is the heading angle of the ship.

4. The time series analysis-based variable-proportion adaptive federated filtering method according to claim 1 or 2, characterized in that the discrete state space model corresponding to each navigation sensor system in step two is:

where k represents the index value of the discrete time state, X_kRepresenting the state quantity at time k, Z_kRepresents the observed quantity at time k, H_kA measuring matrix representing the time k, Γ_k/k-1Is a noise-driven array, W_k-1Is the system state noise at time k-1 and the process noise covariance matrix Q_k＝E[W_kW_k ^T]，V_kFor the system at time k, measure noise, and measure noise covariance matrix R_k＝E[V_kV_k ^T]D, state transition matrix phi_k/k-1The method is obtained by adopting a step-by-step cumulative discretization method:

where J is T/s, T is sampling time, s is step length divided in sampling time, I is unit array, F_i-1Is the state transition matrix at time i-1.

5. The time series analysis-based variable-proportion adaptive federated filtering method according to claim 1 or 2, wherein the information weight obtained by the autoregressive model in step three is specifically:

first, an autoregressive model of the navigation sensor system is determined:

where y (τ) is the output sequence of the navigation sensor system, τ represents the τ -th point of the sequence y (τ), N represents the order of the autoregressive model, a_kRepresenting the error of the model as mean zero and variance σ²Is highThe white noise of the white noise is generated,

representing the coefficients of an Nth order autoregressive model;

then, the autoregressive model coefficients were determined by the Levinson-Durbin recursion algorithm:

wherein,

for coefficients of an Nth order autoregressive modelI is equal to 1, 2, … N,

is the coefficient of an autoregressive model of order N-1Is determined by the estimated value of (c),

to predict the minimum error power of the autoregressive model of order N,to predict the minimum error power of the N-1 order autoregressive model,

as an intermediate parameter, the parameter is,

the 1 st element and the i +1 st element of the Nth row in the autocorrelation function matrix of the output sequence y (tau) of the navigation sensor respectively;

the order is obtained by the final prediction error rule:

wherein xi is the length of the measured data,

error for AR model of order NA differential power;

and further obtaining a predicted value of the output sequence of the navigation sensor system:

taking the prediction error of each navigation sensor system as an information weight theta_i：

y_i(τ) is the output sequence of the ith navigation sensor,

the predicted value of the output sequence of the ith navigation sensor is that i is 0, 1, 2 and 3 respectively correspond to an inertial navigation system, an electrostatic gyro monitor, a satellite positioning navigation system and a Doppler velocity measurement system.

6. The time series analysis-based variable-proportion adaptive federal filtering method as claimed in claim 1 or 2, wherein the information distribution proportion in the step four is proportional to the information weight, and the sum of the weights of the navigation sensor systems is equal to

Wherein beta is₀Is the primary system weight, beta_iThe weight of each subsystem is obtained, n is the number of all subsystems, the main system is an inertial navigation system, the subsystems are an electrostatic gyro monitor, a satellite positioning navigation system and a Doppler velocity measurement system, and finally the weight of each navigation sensor system is obtained as follows:

wherein, i is 0, 1, 2, 3 respectively correspond inertial navigation system, static top monitor, satellite positioning navigation system and doppler speed measuring system.

7. The time series analysis-based variable-proportion adaptive federated filtering method according to claim 1 or 2, characterized in that the global optimum estimation of step five is obtained specifically by the following process:

first, information allocation is obtained:

X_i，k-1＝X_k-1

wherein, P_i，k-1An estimation error variance matrix P representing the ith navigation sensor system_k-1By the ratio beta_iWeighted estimation error variance matrix, Q_i，k-1Covariance matrix Q representing process noise for the ith navigation sensor system_k-1By the ratio beta_iCovariance matrix of process noise after weight distribution, X_i，k-1An index value representing a state quantity of the i-th navigation sensor system, k representing a discrete-time state; the i is 0, 1, 2 and 3 respectively corresponding to an inertial navigation system, an electrostatic gyro monitor, a satellite positioning navigation system and a Doppler velocity measurement system;

and then time updating is carried out:

wherein,

one-step prediction matrix for state of ith navigation sensor system, P_i，k/k-1Predicting mean square error matrix for the ith navigation sensor system in one step, phi_i，k/k-1For state-transfer arrays of the i-th navigation sensor system, Γ_i，k/k-1For the noise-driven array of the ith navigation sensor system,

is a state quantity X_i，k-1The predicted value of (2);

then, observation updating is carried out:

K_{i, k} = P_{i, k / k - 1} H_{i, k}^{T} {(H_{i, k} P_{i, k / k - 1} H_{i, k}^{T} + R_{i, k})}^{- 1}

{\hat{X}}_{i, k} = {\hat{X}}_{i, k / k - 1} + K_{i, k} (Z_{i, k} - H_{i, k} {\hat{X}}_{i, k / k - 1})

P_{i, k} = (I - K_{i, k} H_{i, k}) P_{i, k / k - 1} {(I - K_{i, k} H_{i, k})}^{T} + K_{i, k} R_{i, k} K_{i, k}^{T}

wherein, K_i，kFilter gain for the ith navigation sensor system, H_i，kIs a measurement array of the ith navigation sensor system, R_i，kIs the system measurement noise covariance matrix, Z, of the ith navigation sensor system_i，kIs the observed quantity of the ith navigation sensor system;

and finally obtaining global optimization estimation:

wherein,

is a global estimate, P_gTo estimate the mean square error, m is the number of filters.