CN102589550A - 一种应用联邦h∞滤波器实现组合导航精确定位的方法及系统 - Google Patents

Abstract

本发明涉及一种应用联邦H_∞滤波器实现组合导航精确定位的方法及系统，将无线传感器网络中的参考节点分簇，判断未知节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式，多簇融合定位是在覆盖BN节点的每个簇中，INS和WSN分别测量该簇内RN节点与BN节点之间的距离，将距离信息输入该簇的局部滤波器处理，对每簇中INS测量的BN的位置误差和速度误差进行预估；对每簇的局部滤波器得到的数据进行数据融合，得到系统中INS测量的位置误差和速度误差最优预估值，将INS本身测出的导航信息与最优预估值作差，得出导航信息。这种方法充分利用了未知节点的环境信息，实现了相对导航信息与绝对导航信息的融合。

Description

一种应用联邦H∞滤波器实现组合导航精确定位的方法及系统

技术领域

本发明涉及一种联邦H_∞滤波器，特别涉及一种应用联邦H_∞滤波器实现组合导航精确定位的方法及系统，属于多传感器数据融合技术领域。

背景技术

对未知节点精确位置信息的需求推动了精确定位技术的发展，这种趋势在未来很长一段时间里仍将保持不变。虽然全球定位系统(Global positioning systems，GPS)能够提供精确地，具有持续稳定导航精度的导航信息，但是在室内、高层建筑密集的市区、矿井、隧道等环境下，GPS信号失锁，不能进行定位。为了解决这一问题，许多学者将惯性导航系统(Inertial navigation system，INS)与GPS系统进行组合，形成了GPS/INS组合导航系统。虽然INS具有全自主、运动信息全面、短时、高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航精度严重下降。因此，INS对GPS导航信息的补偿只能是短期补偿，而且GPS/INS组合导航系统的导航精度依赖于GPS的导航精度，而在GPS长时间失锁的情况下，组合导航系统无法提供高精度的导航信息。

近年来，无线传感器网络(Wireless Sensors Network，WSN)以其低成本、低功耗和低系统复杂度的特点在短距离定位领域表现出很大的潜力。WSN为在无GPS信号地区，即所谓的“盲区”时，如室内、高层建筑密集的市区、矿井、隧道等环境下未知节点定位提供了可能，但由于WSN采用的通信技术通常为短距离无线通信技术(如ZigBee、WIFI等)，因此若想完成长距离的目标跟踪定位，需要大量的网络节点共同完成，这增加了WSN的网络负担。

在处理组合导航数据融合的问题时，最常用的方法是使用滤波器。最著名的方法是卡尔曼滤波器(Kalman Filte，KF)，然而由于KF是面向线性系统设计的，因此仅对线性定常系统有很好的滤波效果。为了能够实现非线性系统的滤波问题，许多学者通过雅克比矩阵将非线性的系统转化为线性系统进行滤波，从而产生了扩展卡尔曼滤波器(Extended KalmanFilter，EKF)。EKF对非线性系统有很好的滤波效果，是目前实际生产中应用较为广泛的一种滤波方法。但是由于KF、EKF在滤波过程中假设系统的状态噪声和观测噪声均为标准白噪声，所以并不完全符合实际应用中的情况。另外由于设备的硬件问题，导致每一时刻数据的采集周期有小幅不确定的情形，数据获取并不能按理想情况进行，而现有的KF、EKF并没有考虑这种情况。

发明内容

本发明的目的是克服上述不足而提供一种应用联邦H_∞滤波器实现组合导航精确定位的方法及系统。这种方法克服传统滤波方法(如KF、EKF)对系统的噪声有特定要求的问题，更适合实际应用中的多传感器数据融合。与此同时，这种方法充分利用了未知节点的环境信息，实现了相对导航信息与绝对导航信息的融合。在单个簇的基础上，采用联邦H_∞滤波，将局部滤波器的数据进行融合，得到一种比上述任何单一簇精度更高的导航信息。

本发明采取的技术方案为：

一种应用联邦H_∞滤波器实现组合导航精确定位的方法，包括步骤如下：

(1)将WSN网络中的参考节点分簇：在WSN中，将自身位置已知的节点定义为参考节点(Reference Nodes，RN)，将自身位置未知的节点定义为未知节点(Blind Nodes，BN)，将在一定区域内(例如10m×10m范围内)的RN节点归为一簇，每簇中随机选择一个节点做为本簇的簇首节点，负责采集本簇中所有RN节点采集到的数据，并负责将这些信息与其他簇进行数据交换；

(2)判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式：INS采集到的信息为每个时刻通过自身陀螺加表测量的数据经过解算得到的当前未知节点BN的位置和速度，WSN采集到的信息为每个时刻BN节点与RN节点之间的距离信息，BN节点通过WSN获取RN节点传来的信号，根据接收到的RN节点的信号判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式：若能够覆盖BN节点的簇仅有一个，则进行单簇定位，若能够覆盖BN节点的簇大于一个，即BN节点处于簇重叠的位置，则进行多簇融合定位；

(3)单簇定位或多簇融合定位：

单簇定位：测量RN节点与BN节点之间的距离，将距离信息输入局部H_∞滤波器，对INS测量的BN的位置误差和速度误差进行预估，将INS本身采集到的信息与预估值作差，得出结果修正的导航信息；

多簇融合定位：a.在覆盖BN节点的每个簇中，测量该簇内RN节点与BN节点之间的距离，将距离信息输入该簇的局部滤波器处理，对每簇中INS测量的BN的位置误差和速度误差进行预估；b.在局部滤波器得到的误差估计的基础上，采用分布式H_∞滤波器(即局部滤波器+主滤波器)，对局部滤波器得到的数据进行数据融合，得到系统中INS测量的位置误差和速度误差最优预估值；c.最后将INS本身测出的导航信息(BN的位置和速度)与最优预估值作差，得出结果修正的导航信息。

上述步骤(3)的步骤a中对每簇中INS测量的BN的位置误差和速度误差进行预估的方法为：

①以每个时刻未知节点在本地相对坐标系的x和y两个方向上的位置误差(e_x，e_y)和速度误差(e_vx，e_vy)作为状态向量构建局部滤波器的系统方程如式(I)所示：

式(I)中，X_c，k为系统的状态变量，T为系统的采样周期，(ω_x，k，ω_y，k)和(ω_vx，k，ω_vy，k)为系统方程每个时刻的位置误差噪声和速度误差噪声，为随机值，ω_c，k为系统的噪声向量，A_c为系统矩阵；

②采用INS测出当前时刻BN节点的位置和速度，采用WSN测出当前时刻BN节点的位置和速度(每簇中有多个参考节点，由测得的参考节点与未知节点的距离通过极大似然估计算法得到一个BN节点的位置，通过BN节点的速度计得到速度信息)，将INS测出当前时刻BN节点的位置减去WSN测出当前时刻BN节点的位置，得到位置误差；将INS测出当前时刻BN节点的速度减去WSN测出当前时刻BN节点的速度，得到速度误差，将计算出的位置误差和速度误差作为观测向量输出，如下式(II)所示：

式(II)中，y_c，k为观测向量，X_c，k为系统的状态变量，(Δr_x，k，Δr_y，k)和(Δv_x，k，Δv_y，k)为观测方程每个时刻的位置误差和速度误差，(υ_x，k，υ_y，k)和(υ_vx，k，υ_vy，k)为观测方程每个时刻的位置误差噪声和速度误差噪声，为随机值，υ_c，k为系统的噪声向量，C_c为观测矩阵；

③用上述得到的式(I)和式(II)，加入每个簇需要估计的量与状态向量的线性关系，即z_c，k＝L_cx_c，k，得到每个簇的系统状态方程，如式(III)所示，

$\left\{\begin{array}{c}{x}_{c,k+1}=A_c{x}_{c,k}+B_c{w}_{c,k}\\ y_{c,k}=C_c{x}_{c,k}+D_c{\mathrm{\ξ}}_{c,k}\\ \begin{array}{cc}{z}_{c,k}=L_c{x}_{c,k}& k=\mathrm{0,1,2},...\end{array}\end{array}\right.---\left(\mathrm{III}\right)$

其中，x_c，k∈Rⁿ为第c个簇的系统状态，A_c为每个局部滤波器的系统矩阵，B_c为每个局部滤波器的系统噪声矩阵，这里取单位阵，C_c为每个局部滤波器的观测矩阵，D_c为每个局部滤波器的观测噪声矩阵，这里取单位阵，w_c，k和ξ_c,k分别为每个簇的系统噪声和观测噪声，为随机值；

④根据每个簇的系统状态和观测方程，应用式(III)中的x_c，k+1和y_c，k，定义下式：

$\left\{\begin{array}{c}{\hat{x}}_{c,k+1}=A_c{\hat{x}}_{c,k}+K_c(y_{c,k}-C_c{\hat{x}}_{c,k})\\ {\hat{z}}_{c,k}=L_c{\hat{x}}_{c,k}\end{array}\right.---\left(\mathrm{IV}\right)$

其中和分别为x_c，k和z_c，k的观测值，K_c为卡尔曼增益，定义 ${\tilde{x}}_{c,k}=x_{c,k}-{\hat{x}}_{c,},$ ${\tilde{z}}_{c,k}=z_{c,k}-{\hat{z}}_{c,k},$ 得到式(V)：

$\left\{\begin{array}{c}{\tilde{x}}_{c,k+1}={\tilde{A}}_c{\tilde{x}}_{c,k}+{\tilde{B}}_c{\mathrm{\ζ}}_{i,k}\\ {\tilde{z}}_{c,k}=z_{c,k}-{\hat{z}}_{c,k}=L_c{\tilde{x}}_{c,k}\end{array}\right.---\left(V\right)$

其中，

通过式(V)得到每簇中INS测量的BN的位置误差和速度误差预估向量。

上述步骤(3)的步骤b中对局部滤波器得到的数据进行数据融合的方法是：

构建全局系统状态预估值

满足式(VI)，通过下面所述的不等式(VII-1)、(VII-2)求解权重矩阵，将权重矩阵代入式(VI)最终得主滤波器的最优误差估计，

${\hat{x}}_k=\underset{c=1}{\overset{N}{\mathrm{\Σ}}}{M}_c{\hat{x}}_{c,k}---\left(\mathrm{VI}\right)$

其中，

式中M_c表示各分布子系统融合信息输出的权重矩阵，，其和为单位阵I；利用下述不等式，当其存在可行解，且解大于0时，看滤波器的能量性能指标γ_c，c＝1，2，...，N和选取使得能量性能指标最小的一组参数的作为最后权重矩阵；

如果线性矩阵不等式组(VII)(VII-1-2全部包括)存在可行解P_c＝P′_c＞0，Q_c，M_c，c＝1，2，...，N，则系统(V)是渐进稳定的，并且存在H_∞性能指标γ_c，c＝1，2，...，N和γ_c的大小决定了各簇H_∞滤波器的干扰抑制度，

的大小决定了联邦H_∞滤波器的干扰抑制度，和

越大，干扰抑制度越小，滤波器鲁棒性越差。(I是单位阵，L的定义见上面的式子)

$\left[\begin{array}{cccc}-P_c& 0& A^{\&prime;}{P}_c-C_c^{\&prime;}{Q^{\&prime;}}_c& L_c^{\&prime;}\\ 0& -{{\mathrm{\&gamma;}}_c}^{2}I& B_1^{\&prime;}{P}_i-D_{c1}^{\&prime;}{Q^{\&prime;}}_c& 0\\ P_{c}A-Q_c{C}_c& P_c{B}_1-Q_c{D}_{c1}& -P_c& 0\\ L_c& 0& 0& -I\end{array}\right]<0---(\mathrm{VII}-1)$

$\left[\begin{array}{cccc}-\tilde{P}& 0& {\tilde{A}}^{\&prime;}\tilde{P}& {\tilde{M}}^{\&prime;}\\ 0& -{\widetilde{\mathrm{\&gamma;}}}^{2}I& {\tilde{K}}^{\&prime;}\tilde{P}& 0\\ \tilde{P}\tilde{A}& \tilde{P}\tilde{K}& -\tilde{P}& 0\\ \tilde{M}& 0& 0& -I\end{array}\right]<0---(\mathrm{VII}-2)$

式中

$\tilde{P}=\left[\begin{array}{cccc}{P}_1& 0& ...& 0\\ 0& P_2& ...& 0\\ 0& 0& ...& 0\\ 0& 0& ...& P_N\end{array}\right],$ $\tilde{A}=\mathrm{diag}(A-P_1^{-1}{Q}_1{C}_1,A-P_2^{-1}{Q}_2{C}_2,...,A-B_N^{-1}{Q}_N{C}_N)$

$\tilde{K}=\mathrm{diag}{(B}_1-P_1^{-1}{Q}_1{D}_{11},B_1-P_2^{-1}{Q}_2{D}_{21},...,B_1-P_N^{-1}{Q}_N{D}_N),$

(参数均来自分布子系统方程及其H_∞滤波器的参数矩阵，共N个分布子系统，下标表示分布子系统的序号)。

实现上述方法的系统，包括RN节点部分和BN节点部分，RN节点部分由RN节点、无线WSN模块、超声测距模块和时间同步模块组成；BN节点部分由BN节点、无线WSN模块、INS导航模块、中央数据处理模块和时间同步模块组成；RN节点部分的RN节点分为多簇，每簇通过无线WSN模块、时间同步模块连有一个局部滤波器，各簇的局部滤波器通过一个主滤波器连接导航计算机；每个局部滤波器都与BN节点部分连接。

为了降低WSN的网络负担，本发明提出先将WSN网络进行分簇，在每一个簇内，将WSN和INS进行组合，利用基于WSN的无线定位技术测量未知节点的相对导航信息，将相对导航信息与INS上得到的绝对导航信息通过分布式H∞滤波器进行数据融合，得到理想的导航信息。该方法以每个时刻未知节点的在两个方向上的位置误差和速度误差作为状态向量，以每一时刻根据两种系统采集得到的数据计算出的位置误差和速度误差作为观测向量输出，由状态向量和观测向量输出重新构建一个渐近稳定的系统，即H_∞滤波器。通过H_∞滤波器来预估每一时刻最优的状态向量。这种方法克服传统滤波方法(如KF、EKF)对系统的噪声有特定要求的问题，更适合实际应用中的多传感器数据融合。与此同时，这种方法充分利用了未知节点的环境信息，实现了相对导航信息与绝对导航信息的融合。在单个簇的基础上，采用多个滤波器联邦H_∞滤波，将局部滤波器的数据进行融合，得到一种比上述任何单一簇精度更高的导航信息。

附图说明

图1为本发明应用联邦H_∞滤波器实现组合导航精确定位系统；

图2为单个簇利用H_∞滤波组合导航方法示意图；

图3为多簇应用联邦H_∞滤波组合导航方法示意图；

图4本发明的方法流程图；

图5为实施例1的参考节点、未知节点分布；

图6为实施例1中对局部滤波器得到的数据进行数据融合的流程图；

其中：1.RN节点a，2.RN节点b，3.RN节点c，4.BN节点部分，5.超声测距模块，6.时间同步模块，7.无线WSN模块，8.INS导航模块，9.局部滤波器，10.主滤波器，11.导航计算机，12.导航信息。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

一种应用联邦H_∞滤波器实现组合导航精确定位的系统，包括RN节点部分和BN节点部分，RN节点部分由RN节点、无线WSN模块7、超声测距模块5和时间同步模块6组成；BN节点部分由BN节点、无线WSN模块7、INS导航模块8、中央数据处理模块和时间同步模块6组成；RN节点部分的RN节点分为多簇，每簇通过无线WSN模块7、时间同步模块6连有一个局部滤波器9，各簇的局部滤波器9通过一个主滤波器10连接导航计算机；每个局部滤波器9都与BN节点部分连接。

下面给出一个具体的实现例子。

在下图所示的环境下，将参考节点分成两个簇(如图5所示)，其中簇1和簇2分别包括4个参考节点。BN节点沿着图示的轨迹运动。

如图6所示，本实施例的方法步骤如下：

(1)通过INS采集加表信息，在采集到加表信息之后进行解算，获得INS测量的BN节点的位置和速度信息，通过测量，在交汇部分的第一时刻通过INS测量得到的BN的位置信息为(51.8732，22.0200)。

(2)BN节点通过WSN获取RN节点传来的信号。根据接收到的RN节点的信号判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式，实例中簇1和簇2都可以覆盖到BN节点。

(3)上述(2)测到的能够覆盖BN节点的簇大于一个，则进行多簇融合定位。在能够覆盖BN节点的簇中，首先测量RN节点和BN节点之间的距离，通过距离信息得到两个簇WSN测出的BN节点的位置信息，这里为簇1测量得到的BN位置为(29.2414，10.3006)；簇2测量得到的BN位置为(29.0979，11.1479)。

将INS测量得到的BN位置信息减去两个簇得到的BN的位置信息，得到两个簇中INS的预估误差，这里为簇1(22.6317，11.7195)＝(51.8732，22.0200)-(29.2414，10.3006)，簇2(22.7752，10.8722)(这里的数值是通过局部滤波器得到的误差预估，不是位置和速度，注意是误差信息)。将上述误差信息输入局部H_∞滤波器，对INS的误差进行预估。

首先求得满足图6中线性矩阵不等式的P，Q，K阵。

对簇1，得到的阵分别为：

$P_1=1\&times;{10}^7\left[\begin{array}{cccc}6.2490& 0.2520& -0.0081& -0.0066\\ 0.2520& 6.2489& -0.0066& -0.0084\\ -0.0081& -0.0066& 6.2560& 0.2469\\ -0.0066& -0.0084& 0.2469& 6.2560\end{array}\right],$ $Q_1=1\&times;{10}^7\left[\begin{array}{cccc}7.8113& 0.3150& -0.0102& -0.0083\\ 0.3150& 7.8113& -0.0083& -0.0105\\ -0.0102& -0.0083& 7.8200& 0.3087\\ -0.0083& -0.0105& 0.3087& 7.8200\end{array}\right],$

γ₁＝1.9804×10^-12， $K_1=1\&times;{10}^{-15}\left[\begin{array}{cccc}0.1110& 0.0069& -0.0164& -0.0034\\ 0.0020& 0.1110& -0.0157& -0.0224\\ -0.0095& 0.0038& -0.4441& 0.0131\\ -0.0012& -0.0028& 0.0122& 0.1110\end{array}\right],$

对簇2，得到的阵分别为：

$P_2=1\&times;{10}^8\left[\begin{array}{cccc}1.9973& 0.0530& -0.0142& -0.0141\\ 0.0530& 1.9971& -0.0142& -0.0141\\ -0.0142& -0.0142& 2.0443& 0.0995\\ -0.0141& -0.0141& 0.0995& 2.0439\end{array}\right],$ $Q_2=1\&times;{10}^8\left[\begin{array}{cccc}1.9473& 0.0530& -0.0142& -0.0241\\ 0.0530& 1.9671& -0.0342& -0.0141\\ -0.0142& -0.0342& 1.0443& 0.0995\\ -0.0241& -0.0141& 0.0995& 2.1439\end{array}\right],$

γ₂＝6.3708×10^-14， $K_2=1\&times;{10}^{-15}\left[\begin{array}{cccc}0.3331& 0.0881& -0.0082& 0.0038\\ 0.1953& -0.2220& 0.0031& 0.0017\\ 0.0102& 0.0007& -0.2220& 0.0131\\ 0.0128& 0.0103& 0.0541& 0.2220\end{array}\right],$

通过上述矩阵分别求出单独簇中的局部滤波器测量出的误差最优估计在最终主滤波器的权重M，这里簇1的权重M1为

$M_1=\left[\begin{array}{cccc}0.9998& -0.0001& -0.0007& -0.0001\\ -0.0001& 0.9998& -0.0001& -0.0007\\ -0.0007& -0.0001& 0.9962& -0.0001\\ -0.0001& -0.0007& -0.0001& 0.9962\end{array}\right]$

簇2的权重M2为

$M_2=\left[\begin{array}{cccc}0.0002& 0.0001& 0.0007& 0.0001\\ 0.0001& 0.0002& 0.0001& 0.0007\\ 0.0007& 0.0001& 0.0038& 0.0001\\ 0.0001& 0.0007& 0.0001& 0.0038\end{array}\right].$

在局部滤波器得到的INS误差估计的基础上，对局部滤波器得到的数据进行数据融合，通过X_k＝M₁X_C1，k+M₂X_C2，k得到主滤波器的最优误差估计(22.5978，11.6314)。将这一时刻内INS本身测出的导航信息(51.8732，22.0200)减去主滤波器的最优误差估计，最后得到修正后的结果(29，2754，10.3886)。

上述实例为分布式滤波器对INS测量的BN节点位置误差的预估，BN节点速度误差的预估与位置误差的预估步骤相似，这里不再详细阐述。

Claims (5)

1.一种应用联邦H_∞滤波器实现组合导航精确定位的方法，其特征是，包括步骤如下：

(1)将WSN网络中的参考节点分簇：在WSN中，将自身位置已知的节点定义为参考节点RN，将自身位置未知的节点定义为未知节点BN，将在一定区域内的RN节点归为一簇，每簇中随机选择一个节点做为本簇的簇首节点，负责采集本簇中所有RN节点采集到的数据，并负责将这些信息与其他簇进行数据交换；

(2)判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式：INS采集到的信息为每个时刻通过自身陀螺加表测量的数据经过解算得到的当前未知节点BN的位置和速度，WSN采集到的信息为每个时刻BN节点与RN节点之间的距离信息，BN节点通过WSN获取RN节点传来的信号，根据接收到的RN节点的信号判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式：若能够覆盖BN节点的簇仅有一个，则进行单簇定位，若能够覆盖BN节点的簇大于一个，即BN节点处于簇重叠的位置，则进行多簇融合定位；

(3)单簇定位或多簇融合定位：

单簇定位：INS测量RN节点与BN节点之间的距离，将距离信息输入局部H_∞滤波器，对INS测量的BN的位置误差和速度误差进行预估，将INS采集到的信息与预估值作差，得出结果修正的导航信息；

多簇融合定位：a.在覆盖BN节点的每个簇中，INS测量该簇内RN节点与BN节点之间的距离，将距离信息输入该簇的局部滤波器处理，对每簇中INS测量的BN的位置误差和速度误差进行预估；b.在局部滤波器得到的误差估计的基础上，采用分布式H_∞滤波器，对局部滤波器得到的数据进行数据融合，得到系统中INS测量的位置误差和速度误差最优预估值；c.最后将INS本身测出的导航信息与最优预估值作差，得出结果修正的导航信息。

2.根据权利要求1所述的一种应用联邦H_∞滤波器实现组合导航精确定位的方法，其特征是，步骤(3)的步骤a中对每簇中INS测量的BN的位置误差和速度误差进行预估的方法为：

①以每个时刻未知节点在本地相对坐标系的x和y两个方向上的位置误差(e_x，e_y)和速度误差(e_vx，e_vy)作为状态向量构建局部滤波器的系统方程如式(I)所示：

式(I)中，X_c，k为系统的状态变量，T为系统的采样周期，(ω_x，k，ω_y，k)和(ω_vx，k，ω_vy，k)为系统方程每个时刻的位置误差噪声和速度误差噪声，为随机值，ω_c，k为系统的噪声向量，A_c为系统矩阵；

②采用INS测出当前时刻BN节点的位置和速度，采用WSN测出当前时刻BN节点的位置和速度，将INS测出当前时刻BN节点的位置减去WSN测出当前时刻BN节点的位置，得到位置误差；将INS测出当前时刻BN节点的速度减去WSN测出当前时刻BN节点的速度，得到速度误差，将计算出的位置误差和速度误差作为观测向量输出，如下式(II)所示：

式(II)中，y_c，k为观测向量，X_c，k为系统的状态变量，(Δr_x，k，Δr_y，k)和(Δv_x，k，Δv_y，k)为观测方程每个时刻的位置误差和速度误差，(υ_x，k，υ_y，k)和(υ_vx，k，υ_vy，k)为观测方程每个时刻的位置误差噪声和速度误差噪声，为随机值，υ_c，k为系统的噪声向量，C_c为观测矩阵；

③用上述得到的式(I)和式(II)，加入每个簇需要估计的量与状态向量的线性关系，即z_c，k＝L_cx_c，k，得到每个簇的系统状态方程，如式(III)所示，

$\left\{\begin{array}{c}{x}_{c,k+1}=A_c{x}_{c,k}+B_c{w}_{c,k}\\ y_{c,k}=C_c{x}_{c,k}+D_c{\mathrm{\&xi;}}_{c,k}\\ \begin{array}{cc}{z}_{c,k}=L_c{x}_{c,k}& k=\mathrm{0,1,2},...\end{array}\end{array}\right.---\left(\mathrm{III}\right)$

其中，x_c，k∈Rⁿ为第c个簇的系统状态，A_c为每个局部滤波器的系统矩阵，B_c为每个局部滤波器的系统噪声矩阵，这里取单位阵，C_c为每个局部滤波器的观测矩阵，D_c为每个局部滤波器的观测噪声矩阵，这里取单位阵，w_c，k和ξ_c，k分别为每个簇的系统噪声和观测噪声，为随机值；

④根据每个簇的系统状态和观测方程，应用式(III)中的x_c，k+1和y_c，k，定义下式：

$\left\{\begin{array}{c}{\hat{x}}_{c,k+1}=A_c{\hat{x}}_{c,k}+K_c(y_{c,k}-C_c{\hat{x}}_{c,k})\\ {\hat{z}}_{c,k}=L_c{\hat{x}}_{c,k}\end{array}\right.---\left(\mathrm{IV}\right)$

其中和

分别为x_c，k和z_c，k的观测值，K_c为卡尔曼增益，定义 ${\tilde{x}}_{c,k}=x_{c,k}-{\hat{x}}_{c,},$ ${\tilde{z}}_{c,k}=z_{c,k}-{\hat{z}}_{c,k},$ 得到式(V)：

$\left\{\begin{array}{c}{\tilde{x}}_{c,k+1}={\tilde{A}}_c{\tilde{x}}_{c,k}+{\tilde{B}}_c{\mathrm{\&zeta;}}_{i,k}\\ {\tilde{z}}_{c,k}=z_{c,k}-{\hat{z}}_{c,k}=L_c{\tilde{x}}_{c,k}\end{array}\right.---\left(V\right)$

其中， ${\tilde{A}}_c=A_c-K_c{C}_c,$ ${\tilde{B}}_c=B_1-K_c{D}_c,$

通过式(V)得到每簇中INS测量的BN的位置误差和速度误差预估向量。

3.根据权利要求1所述的一种应用联邦H_∞滤波器实现组合导航精确定位的方法，其特征是，步骤(3)的步骤b中分布式H_∞滤波器对局部滤波器得到的数据进行数据融合的方法是：

构建全局系统状态预估值

满足式(VI)，通过下面所述的不等式(VII-1)、(VII-2)求解权重矩阵，将权重矩阵代入式(VI)最终得主滤波器的最优误差估计；

${\hat{x}}_k=\underset{c=1}{\overset{N}{\mathrm{\&Sigma;}}}{M}_c{\hat{x}}_{c,k}---\left(\mathrm{VI}\right)$

其中，

式中M_c表示各分布子系统融合信息输出的权重矩阵，其和为单位阵I；

利用下述不等式，当其存在可行解，且解大于0时，看滤波器的能量性能指标γ_c，c＝1，2，...，N和

选取使得能量性能指标最小的一组参数的作为最后权重矩阵：

$\left[\begin{array}{cccc}-P_c& 0& A^{\&prime;}{P}_c-C_c^{\&prime;}{Q^{\&prime;}}_c& L_c^{\&prime;}\\ 0& -{{\mathrm{\&gamma;}}_c}^{2}I& B_1^{\&prime;}{P}_i-D_{c1}^{\&prime;}{Q^{\&prime;}}_c& 0\\ P_{c}A-Q_c{C}_c& P_c{B}_1-Q_c{D}_{c1}& -P_c& 0\\ L_c& 0& 0& -I\end{array}\right]<0---(\mathrm{VII}-1)$

$\left[\begin{array}{cccc}-\tilde{P}& 0& {\tilde{A}}^{\&prime;}\tilde{P}& {\tilde{M}}^{\&prime;}\\ 0& -{\widetilde{\mathrm{\&gamma;}}}^{2}I& {\tilde{K}}^{\&prime;}\tilde{P}& 0\\ \tilde{P}\tilde{A}& \tilde{P}\tilde{K}& -\tilde{P}& 0\\ \tilde{M}& 0& 0& -I\end{array}\right]<0---(\mathrm{VII}-2)$

式中

$\tilde{P}=\left[\begin{array}{cccc}{P}_1& 0& ...& 0\\ 0& P_2& ...& 0\\ 0& 0& ...& 0\\ 0& 0& ...& P_N\end{array}\right],$ $\tilde{A}=\mathrm{diag}(A-P_1^{-1}{Q}_1{C}_1,A-P_2^{-1}{Q}_2{C}_2,...,A-P_N^{-1}{Q}_N{C}_N)$

$\tilde{K}=\mathrm{diag}{(B}_1-P_1^{-1}{Q}_1{D}_{11},B_1-P_2^{-1}{Q}_2{D}_{21},...,B_1-P_N^{-1}{Q}_N{D}_N),$

$\tilde{M}=[M_1,M_2,...,M_N].$

4.根据权利要求1所述的一种应用联邦H_∞滤波器实现组合导航精确定位的方法，其特征是，所述的RN节点归为一簇的划分区域为10m×10m范围内。

5.实现权利要求1-4任意一项所述的方法的系统，包括RN节点部分和BN节点部分，其特征是，RN节点部分由RN节点、无线WSN模块、超声测距模块和时间同步模块组成；BN节点部分由BN节点、无线WSN模块、INS导航模块、中央数据处理模块和时间同步模块组成；RN节点部分的RN节点分为多簇，每簇通过无线WSN模块、时间同步模块连有一个局部滤波器，各簇的局部滤波器通过一个主滤波器连接导航计算机；每个局部滤波器都与BN节点部分连接。

Images