CN103322999A - 一种适用于多艇导航的保留历史状态的信息滤波算法 - Google Patents

Abstract

一种适用于多艇导航的保留历史状态的信息滤波算法，涉及一种协同导航技术。它实现了在减少了KF、EKF等常用滤波方法在协同导航应用的计算量的基础上，解决了多水面艇导航问题。其方法为：建立水面艇的***方程和观测方程；将非线性模型线性化；通过观测更新得到实时信息参数；进行时间更新；利用信息参数求得状态均值。本发明适用于多水面艇导航。

Description

一种适用于多艇导航的保留历史状态的信息滤波算法

技术领域

本发明属于导航技术领域，涉及一种适用于多艇导航的保留历史状态的信息滤波算法。

背景技术

水面艇是指在水面行驶的水面船艇。随着开发海洋认识的加深，水面艇以其机动性强、成本低等优势成为研究热点。多艇导航利用***中某些艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时，协同导航可以在一定程度上恢复这些艇的导航能力。因此研究多水面艇的协同导航具有重要的理论价值和实际意义。

目前，对于多水面艇协同导航技术研究相对较少，但是协同导航技术在多无人平台上的应用并不鲜见。常用的方法是结合具体的导航传感器采用各种改进的卡尔曼滤波方法(KF)进行数据融合。相关文献指出在集中式KF算法中，以各个平台为处理节点，利用KF将各个节点的导航信息融合到某个主节点上，能够大大提高主节点的导航精度，在此基础上，将主节点的高精度信息反馈给各个节点从而提高多平台的整体导航能力。但是它在应用上有其缺陷，在观测更新时要改变所有与观测状态相关的状态的矩参数，使得计算量增大，通信复杂度较大，这将阻碍协同导航在军事领域的应用。通过研究机器人同步定位与建图(SLAM)，发现带滞留状态的信息滤波可以取得比卡尔曼滤波较好的效果，并且滤波状态联合分布矩阵是稀疏的，计算量更优。相关文献借鉴SLAM提出一种将带滞留状态的增广信息滤波应用多运动平台的协同导航分散式设计方法。

发明内容

为了克服现有技术中存在的缺陷，本发明提供了一种适用于多艇导航的保留历史状态的信息滤波算法，将一种带历史状态的信息滤波应用于多艇的协同导航中。该方法将关键历史保留在联合状态的信息矩阵中，利用联合分布信息矩阵的稀疏性以及状态添加和观测更新仅改变与自身状态相关信息来降低运算复杂度。最后通过仿真，既保证了艇的协同定位的估计精度，又验证了该算法性能的优越性。

其技术方案如下：

一种适用于多艇导航的保留历史状态的信息滤波算法，包括以下步骤：

步骤一、建立多水面艇的运动方程，选取艇的纬度、经度和航向角作为状态量，首先建立平面2维坐标系x-y，记k时刻第i条艇状态为

为k时刻第i条艇在x轴的坐标，

为k时刻第i条艇在y轴的坐标，为k时刻第i条艇的航向角，运动方程可表达为

$\left\{\begin{array}{c}{x}_{k+1}^i=x_k^i+v_k^i\cos {\mathrm{\φ}}_k^i\·\mathrm{\Δt}+w^i\\ y_{k+1}^i=y_k^i+v_k^i\sin {\mathrm{\φ}}_k^i\·\mathrm{\Δt}+w^i\\ {\mathrm{\φ}}_{k+1}^i={\mathrm{\φ}}_k^i+w^i\end{array}\right.$

其中Δt是航迹推算更新周期，***噪声wⁱ，航向角和速度构成控制输入

这样k时刻n条艇的导航状态为X_k+1＝[x¹；x²；…；xⁿ]_k；

步骤二、建立多水面艇的观测方程，选取艇的位置信息或者速度信息和各艇之间观测距离为量测量，

单艇观测方程可选为 $z^i=\left[\begin{array}{c}{z}_x^i\\ z_y^i\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]x^i+v^i$

k时刻选取艇间距为观测量(设i艇和j艇)

$z_k^{\mathrm{ij}}=\sqrt{{(x_k^i-x_k^j)}^2+{(y_k^i-y_k^j)}^2}+v_k^{\mathrm{ij}}$

其中***噪声wⁱ和观测噪声vⁱ、v^ij均是零均值白噪声，且相互独立不相关；

步骤三、***运动方程和观测方程的线性化

对该非线性***在估计的均值附近进行线性化处理，对每个艇有：

其中f为非线性***方程，则整个***n条艇的状态转移阵F_k为

对于单艇i观测，其量测方程雅可比矩阵为

$H_k^i=[0...\▿h^i...0]$

对于艇i，j间观测，其量测方程雅可比矩阵为

$H_k^{\mathrm{ij}}=[0...\▿h_i^{\mathrm{ij}}...{\▿h}_j^{\mathrm{ij}}...0]$

其中，

是线性化因子， ${\▿h}_i^i=\frac{{\∂h}^i}{{\∂x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$ ${\▿h}_i^{\mathrm{ij}}=\frac{{\∂h}^{\mathrm{ij}}}{{\∂x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$ ${\▿h}_j^{\mathrm{ij}}=\frac{{\∂h}^{\mathrm{ij}}}{{\∂x}_k^j}{|}_{x_k^j={\hat{x}}_k^j},$ h为非线性观测方程；

步骤四、历史状态的保留，以及信息参数的求取

考虑***k-1时刻和k时刻的联合状态为[X_k-1X_k]，信息参数为

$y=\left[\begin{array}{c}{y}_{k-1}-{F_{k-1}}^T{Q_{k-1}}^{-1}[X_k-F_{k-1}{X}_{k-1}]\\ Q^{-1}[X_k-F_{k-1}{X}_{k-1}]\end{array}\right]\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{c}{y}_{k-1}\\ y_k\end{array}\right];$

$Y=\left[\begin{array}{cc}{Y}_{k-1}+F_{k-1}^T{Q_{k-1}}^{-1}{F}_{k-1}& -F_{k-1}^T{Q_{k-1}}^{-1}\\ -{Q_{k-1}}^{-1}{F}_{k-1}& {Q_{k-1}}^{-1}\end{array}\right]\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{Y}_{k-1,k-1}& Y_{k-1,k}\\ Y_{k-1,k}^T& Y_{k,k}\end{array}\right];$

其中，y为信息向量，Y为信息矩阵，F_k-1，Q_k-1分别为k-1时刻，***线性化后的状态转移矩阵和***噪声阵.

由运动方程，k+1时刻添加状态X_k+1，得到联合状态[X_k-1X_kX_k+1]，结合信息参数算法过程，可得信息参数为：

$y_a=\left[\begin{array}{c}{y}_{k-1}\\ y_k-{F_k}^T{Q_k}^{-1}[X_{k+1}-F_k{X}_k]\\ {Q_k}^{-1}[X_{k+1}-F_k{X}_k]\end{array}\right]$ $Y_a=\left[\begin{array}{ccc}{Y}_{k-1,k-1}& Y_{k-1,k}& 0\\ Y_{k-1,k}^T& Y_{k,k}+{F_k}^T{Q_k}^{-1}{F}_k& -{F_k}^T{Q_k}^{-1}\\ 0& -{Q_k}^{-1}{F}_k& {Q_k}^{-1}\end{array}\right];$

其中，y_a为状态添加后的信息向量，Y_a为状态添加后的信息矩阵，F_k，Q_k分别为k时刻，***线性化后的状态转移矩阵和***噪声阵。

步骤五、***的观测更新，

通过观测z_k+1来更新[X_k-1X_kX_k+1]的联合信息参数，观测更新后的信息参数为：

y_a＝[y_a(1)；y_a(2)；y_a(3)+i_k+1]；

$Y_a=\left[\begin{array}{ccc}{Y}_a\left(\mathrm{1,1}\right)& Y_a\left(\mathrm{1,2}\right)& Y_a\left(\mathrm{1,3}\right)\\ Y_a{\left(\mathrm{1,2}\right)}^T& Y_a\left(\mathrm{2,2}\right)& Y_a\left(\mathrm{2,3}\right)\\ Y_a{(1,3)}^T& Y_a{\left(\mathrm{2,3}\right)}^T& Y_a\left(\mathrm{3,3}\right)+I_{k+1}\end{array}\right];$

其中i_k+1，I_k+1是观测更新贡献的信息向量更新和信息矩阵更新，

对于k+1时刻单艇观测，信息向量的更新为

$i_{k+1}^i={\left(H_{k+1}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}(z_{k+1}^i-h_i\left({\hat{x}}_{k+1}^i\right)+H_{k+1}^i{\hat{X}}_{k+1(-)})$

$={[0...i_{k+1}\left(i\right)...0]}^T$

其中， $i_{k+1}^i={\left({\▿h}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}(z_{k+1}^i-h_i\left({\hat{x}}_{k+1}^i\right)+{\▿h}^i{\hat{x}}_{(k+1)-}),$

为k+1时刻量测噪声阵，

是关于

的非线性函数，

为雅可比矩阵，

为k-1时刻的状态估计。

信息矩阵的更新为：

$I_{k+1}={\left(H_{k+1}^i\right)}^T{\left(R_{r+1}^i\right)}^{-1}{H}_{k+1}^i$

其中， $I_{k+1}(i,i)={\left({\▿h}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}{\▿h}^i,$

对于k+1时刻平台i、j间相对观测，信息向量的更新为

$i_{k+1}={\left(H_{k+1}^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+H_{k+1}^{\mathrm{ij}}{\hat{x}}_{(k+1)-})$

$={[0...i_{k+1}\left(i\right)...i_{k+1}\left(j\right)...0]}^T$

其中， $i_{k+1}\left(i\right)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+{\▿h}_i^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^i+{\▿h}_j^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^j)$

$i_{k+1}\left(j\right)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+{\▿h}_i^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^i+{\▿h}_j^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^j)$

观测贡献的信息矩阵更新为：

$I_{k+1}={\left(H_{k+1}^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{H}_{k+1}^{\mathrm{ij}}$

$=\left[\begin{array}{ccccccc}0& ...& 0& ...& 0& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& I_{k+1}(i,i)& ...& I_{k+1}(i,j)& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& I_{k+1}(j,i)& ...& I_{k+1}(j,j)& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& 0& ...& 0& ...& 0\end{array}\right]$

其中，

$I_{k+1}(i,i)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_i^{\mathrm{ij}}$

$I_{k+1}(i,j)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_j^{\mathrm{ij}}$

$I_{k+1}(j,i)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_i^{\mathrm{ij}}$

$I_{k+1}(j,j)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_j^{\mathrm{ij}}$

步骤六、信息滤波的时间更新，

时间更新是由多个时刻k-1，k，k+1…状态的联合概率求k时刻状态的边缘概率，在引入历史状态的信息滤波中，维持的是多个时刻k-1，k，k+1……联合状态的信息参数，而当联合状态的维数较高时，由于引入历史状态的信息滤波联合状态的信息矩阵是稀疏的，可以很容易由联合状态求得k时刻状态的边缘概率；

步骤七、对于状态均值

的求取，

通过求解Y＝P^-1，实现，其中Y为信息矩阵

为信息向量，P为状态方差。

与现有技术相比，本发明的有益效果：

本发明提出了一种适用于多艇导航的保留历史状态的信息滤波算法，与传统信息滤波相比，该算法交换时间更新与观测更新的执行顺序。由于在进行观测更新时，滤波状态不仅含有当前时刻的状态，还含有历史时刻的状态。它拥有观测更新仅涉及观测艇和被观测艇的信息量的优势，以及将历史信息保留在联合状态时信息矩阵的稀疏性优势.

附图说明

图1是多水面艇轨迹示意图；

图2是B艇在X方向的误差曲线图；

图3是B艇在Y方向的误差曲线图；

图4是B艇航向角的误差曲线图。

具体实施方式

下面结合附图和具体实施方式对本发明的技术方案作进一步详细地说明。

一种适用于多艇导航的保留历史状态的信息滤波算法，包括以下步骤：

步骤一、建立多水面艇的运动方程，选取艇的纬度、经度和航向角作为状态量，首先建立平面2维坐标系x-y，记k时刻第i条艇状态为

为k时刻第i条艇在x轴的坐标，

为k时刻第i条艇在y轴的坐标，

为k时刻第i条艇的航向角，运动方程可表达为

$\left\{\begin{array}{c}{x}_{k+1}^i=x_k^i+v_k^i\cos {\mathrm{\φ}}_k^i\·\mathrm{\Δt}+w^i\\ y_{k+1}^i=y_k^i+v_k^i\sin {\mathrm{\φ}}_k^i\·\mathrm{\Δt}+w^i\\ {\mathrm{\φ}}_{k+1}^i={\mathrm{\φ}}_k^i+w^i\end{array}\right.$

其中Δt是航迹推算更新周期，***噪声wⁱ，速度和航向角构成控制输入

这样k时刻n条艇的导航状态为X_k+1＝[x¹；x²；…；xⁿ]_k；

步骤二、建立多水面艇的观测方程，选取艇的位置信息或者速度信息和各艇之间观测距离为量测量，

单艇观测方程可选为

$z^i=\left[\begin{array}{c}{z}_x^i\\ z_y^i\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]x^i+v^i$

k时刻选取艇间距为观测量(设i艇和j艇)

$z_k^{\mathrm{ij}}=\sqrt{{(x_k^i-x_k^j)}^2+{(y_k^i-y_k^j)}^2}+v_k^{\mathrm{ij}}$

其中***噪声wⁱ和观测噪声vⁱ、v^ij均是零均值白噪声，且相互独立不相关；

步骤三、***运动方程和观测方程的线性化

对该非线性***在估计的均值附近进行线性化处理，对每个艇有：

其中f为非线性***方程，则整个***n条艇的状态转移阵F_k为

对于单艇i观测，其量测方程雅可比矩阵为

$H_k^i=[0...\▿h^i...0]$

对于艇i，j间观测，其量测方程雅可比矩阵为

$H_k^{\mathrm{ij}}=[0...\▿h_i^{\mathrm{ij}}...{\▿h}_j^{\mathrm{ij}}...0]$

其中，是线性化因子， ${\▿h}_i^i=\frac{{\∂h}^i}{{\∂x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$ ${\▿h}_i^{\mathrm{ij}}=\frac{{\∂h}^{\mathrm{ij}}}{{\∂x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$ ${\▿h}_j^{\mathrm{ij}}=\frac{{\∂h}^{\mathrm{ij}}}{{\∂x}_k^j}{|}_{x_k^j={\hat{x}}_k^j},$ h为非线性观测方程；

步骤四、历史状态的保留，以及信息参数的求取

考虑***k-1时刻和k时刻的联合状态为[X_k-1X_k]，信息参数为

$y=\left[\begin{array}{c}{y}_{k-1}-{F_{k-1}}^T{Q_{k-1}}^{-1}[X_k-F_{k-1}{X}_{k-1}]\\ Q^{-1}[X_k-F_{k-1}{X}_{k-1}]\end{array}\right]\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{c}{y}_{k-1}\\ y_k\end{array}\right];$

$Y=\left[\begin{array}{cc}{Y}_{k-1}+F_{k-1}^T{Q_{k-1}}^{-1}{F}_{k-1}& -F_{k-1}^T{Q_{k-1}}^{-1}\\ -{Q_{k-1}}^{-1}{F}_{k-1}& {Q_{k-1}}^{-1}\end{array}\right]\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{Y}_{k-1,k-1}& Y_{k-1,k}\\ Y_{k-1,k}^T& Y_{k,k}\end{array}\right];$

其中，y为信息向量，Y为信息矩阵，F_k-1，Q_k-1分别为k-1时刻，***线性化后的状态转移矩阵和***噪声阵。

由运动方程，k+1时刻添加状态X_k+1，得到联合状态[X_k-1X_kX_k+1]，结合信息参数算法过程，可得信息参数为：

$y_a=\left[\begin{array}{c}{y}_{k-1}\\ y_k-{F_k}^T{Q_k}^{-1}[X_{k+1}-F_k{X}_k]\\ {Q_k}^{-1}[X_{k+1}-F_k{X}_k]\end{array}\right]$ $Y_a=\left[\begin{array}{ccc}{Y}_{k-1,k-1}& Y_{k-1,k}& 0\\ Y_{k-1,k}^T& Y_{k,k}+{F_k}^T{Q_k}^{-1}{F}_k& -{F_k}^T{Q_k}^{-1}\\ 0& -{Q_k}^{-1}{F}_k& {Q_k}^{-1}\end{array}\right];$

其中，y_a为状态添加后的信息向量，Y_a为状态添加后的信息矩阵，F_k，Q_k分别为k时刻，***线性化后的状态转移矩阵和***噪声阵。

步骤五、***的观测更新，

通过观测z_k+1来更新[X_k-1X_kX_k+1]的联合信息参数，观测更新后的信息参数为：

y_a＝[y_a(1)；y_a(2)；y_a(3)+i_k+1]；

$Y_a=\left[\begin{array}{ccc}{Y}_a\left(\mathrm{1,1}\right)& Y_a\left(\mathrm{1,2}\right)& Y_a\left(\mathrm{1,3}\right)\\ Y_a{\left(\mathrm{1,2}\right)}^T& Y_a\left(\mathrm{2,2}\right)& Y_a\left(\mathrm{2,3}\right)\\ Y_a{(1,3)}^T& Y_a{\left(\mathrm{2,3}\right)}^T& Y_a\left(\mathrm{3,3}\right)+I_{k+1}\end{array}\right];$

其中i_k+1，I_k+1是观测更新贡献的信息向量更新和信息矩阵更新，

对于k+1时刻单艇观测，信息向量的更新为

$i_{k+1}^i={\left(H_{k+1}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}(z_{k+1}^i-h_i\left({\hat{x}}_{k+1}^i\right)+H_{k+1}^i{\hat{X}}_{k+1(-)})$

$={[0...i_{k+1}\left(i\right)...0]}^T$

其中， $i_{k+1}^i={\left({\▿h}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}(z_{k+1}^i-h_i\left({\hat{x}}_{k+1}^i\right)+{\▿h}^i{\hat{x}}_{(k+1)-}),$ 为k+1时刻量测噪声阵，

是关于

的非线性函数，

为雅可比矩阵，

为k-1时刻的状态估计。

信息矩阵的更新为：

$I_{k+1}={\left(H_{k+1}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}{H}_{k+1}^i$

其中， $I_{k+1}(i,i)={\left({\▿h}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}{\▿h}^i.$

对于k+1时刻平台i、j间相对观测，信息向量的更新为

$i_{k+1}={\left(H_{k+1}^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+H_{k+1}^{\mathrm{ij}}{\hat{x}}_{(k+1)-})$

$={[0...i_{k+1}\left(i\right)...i_{k+1}\left(j\right)...0]}^T$

其中， $i_{k+1}\left(i\right)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+{\▿h}_i^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^i+{\▿h}_j^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^j)$

$i_{k+1}\left(j\right)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+{\▿h}_i^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^i+{\▿h}_j^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^j)$

观测贡献的信息矩阵更新为：

$I_{k+1}={\left(H_{k+1}^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{H}_{k+1}^{\mathrm{ij}}$

$=\left[\begin{array}{ccccccc}0& ...& 0& ...& 0& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& I_{k+1}(i,i)& ...& I_{k+1}(i,j)& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& I_{k+1}(j,i)& ...& I_{k+1}(j,j)& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& 0& ...& 0& ...& 0\end{array}\right]$

其中，

$I_{k+1}(i,i)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_i^{\mathrm{ij}}$

$I_{k+1}(i,j)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_j^{\mathrm{ij}}$

$I_{k+1}(j,i)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_i^{\mathrm{ij}}$

$I_{k+1}(j,j)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_j^{\mathrm{ij}}$

步骤六、信息滤波的时间更新，

时间更新是由多个时刻k-1，k，k+1…状态的联合概率求k时刻状态的边缘概率，在引入历史状态的信息滤波中，维持的是多个时刻k-1，k，k+1……联合状态的信息参数，而当联合状态的维数较高时，由于引入历史状态的信息滤波联合状态的信息矩阵是稀疏的，可以很容易由联合状态求得k时刻状态的边缘概率；

步骤七、对于状态均值

的求取，

通过求解Y＝P^-1，

实现，其中Y为信息矩阵

为信息向量，P为状态方差。

为了说明所述方法的有益效果，在以下初始条件下对三艘水面艇的协同导航进行了仿真，仿真结果如图1、图2、图3及图4所示，并对其进行了分析比较。

初始条件：

三艘水面水面艇分别标识为A、B、C艇。仿真不考虑水流的扰动，每条艇都装备里程计、激光测距仪和一定的通信设备，A艇并给予GPS接收机，可以进行GPS观测。

离散时间间隔T：0.01s

水面艇最大移动速度：4m/s。

测量速度标准差：1m/s

航向角标准差：1

测距噪声标准差：5m

单艇观测噪声标准差：2m

仿真时间：10s

分析比较：

图1仿真了一次实验中A、B、C三条艇的参考轨迹、独自导航和协同导航的定位结果。由初始仿真设置知，独自导航时除艇A可以利用GPS信息校正里程计累积误差外，其它两艇只能利用里程计信息进行航迹推算。仿真结果表明，利用保留历史状态的信息滤波进行协同导航提高了水面艇的定位精度，并且没有GPS装置的艇也共享了导航信息从而提高了自身定位精度。

进一步进行仿真观察，图2～图4给出了B艇各状态量的误差曲线图，基于本导航算法的各状态量误差小于单独导航误差，说明该方法可以保证水面艇导航的估计精度，从而验证了算法的正确性。

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (1)

1.一种适用于多艇导航的保留历史状态的信息滤波算法，其特征在于，包括以下步骤：

步骤一、建立多水面艇的运动方程选取艇的纬度、经度和航向角作为状态量，首先建立平面2维坐标系x-y，记k时刻第i条艇状态为

为k时刻第i条艇在x轴的坐标，

为k时刻第i条艇在y轴的坐标，

为k时刻第i条艇的航向角，运动方程可表达为

$\left\{\begin{array}{c}{x}_{k+1}^i=x_k^i+v_k^i\cos {\mathrm{\φ}}_k^i\·\mathrm{\Δt}+w^i\\ y_{k+1}^i=y_k^i+v_k^i\sin {\mathrm{\φ}}_k^i\·\mathrm{\Δt}+w^i\\ {\mathrm{\φ}}_{k+1}^i={\mathrm{\φ}}_k^i+w^i\end{array}\right.$

其中Δt是航迹推算更新周期，***噪声wⁱ，航向角和速度构成控制输入

这样k时刻n条艇的导航状态为X_k+1＝[x¹；x²；…；xⁿ]_k；

步骤二、建立多水面艇的观测方程，选取艇的位置信息或者速度信息和各艇之间观测距离为量测量，

单艇观测方程可选为

$z^i=\left[\begin{array}{c}{z}_x^i\\ z_y^i\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]x^i+v^i$

k时刻选取艇间距为观测量(设i艇和j艇)

$z_k^{\mathrm{ij}}=\sqrt{{(x_k^i-x_k^j)}^2+{(y_k^i-y_k^j)}^2}+v_k^{\mathrm{ij}}$

其中***噪声wⁱ和观测噪声vⁱ、v^ij均是零均值白噪声，且相互独立不相关；

步骤三、***运动方程和观测方程的线性化

对该非线性***在估计的均值附近进行线性化处理，对每个艇有：

其中f为非线性***方程，则整个***n条艇的状态转移阵F_k为

对于单艇i观测，其量测方程雅可比矩阵为

$H_k^i=[0...\▿h^i...0]$

对于艇i，j间观测，其量测方程雅可比矩阵为

$H_k^{\mathrm{ij}}=[0...\▿h_i^{\mathrm{ij}}...{\▿h}_j^{\mathrm{ij}}...0]$

其中，是线性化因子， ${\▿h}_i^i=\frac{{\∂h}^i}{{\∂x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$ ${\▿h}_i^{\mathrm{ij}}=\frac{{\∂h}^{\mathrm{ij}}}{{\∂x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$ ${\▿h}_j^{\mathrm{ij}}=\frac{{\∂h}^{\mathrm{ij}}}{{\∂x}_k^j}{|}_{x_k^j={\hat{x}}_k^j},$ h为非线性观测方程；

步骤四、历史状态的保留，以及信息参数的求取

考虑***k-1时刻和k时刻的联合状态为[X_k-1X_k]，信息参数为

$y=\left[\begin{array}{c}{y}_{k-1}-{F_{k-1}}^T{Q_{k-1}}^{-1}[X_k-F_{k-1}{X}_{k-1}]\\ Q^{-1}[X_k-F_{k-1}{X}_{k-1}]\end{array}\right]\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{c}{y}_{k-1}\\ y_k\end{array}\right];$

$Y=\left[\begin{array}{cc}{Y}_{k-1}+F_{k-1}^T{Q_{k-1}}^{-1}{F}_{k-1}& -F_{k-1}^T{Q_{k-1}}^{-1}\\ -{Q_{k-1}}^{-1}{F}_{k-1}& {Q_{k-1}}^{-1}\end{array}\right]\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{Y}_{k-1,k-1}& Y_{k-1,k}\\ Y_{k-1,k}^T& Y_{k,k}\end{array}\right];$

其中，y为信息向量，Y为信息矩阵，F_k-1，Q_k-1分别为k-1时刻，***线性化后的状态转移矩阵和***噪声阵，

由运动方程，k+1时刻添加状态X_k+1，得到联合状态[X_k-1X_kX_k+1]，结合信息参数算法过程，可得信息参数为：

$y_a=\left[\begin{array}{c}{y}_{k-1}\\ y_k-{F_k}^T{Q_k}^{-1}[X_{k+1}-F_k{X}_k]\\ {Q_k}^{-1}[X_{k+1}-F_k{X}_k]\end{array}\right]$ $Y_a=\left[\begin{array}{ccc}{Y}_{k-1,k-1}& Y_{k-1,k}& 0\\ Y_{k-1,k}^T& Y_{k,k}+{F_k}^T{Q_k}^{-1}{F}_k& -{F_k}^T{Q_k}^{-1}\\ 0& -{Q_k}^{-1}{F}_k& {Q_k}^{-1}\end{array}\right];$

其中，y_a为状态添加后的信息向量，Y_a为状态添加后的信息矩阵，F_k，Q_k分别为k时刻，***线性化后的状态转移矩阵和***噪声阵；

步骤五、***的观测更新，

通过观测z_k+1来更新[X_k-1X_kX_k+1]的联合信息参数，观测更新后的信息参数为：

y_a＝[y_a(1)；y_a(2)；y_a(3)+i_k+1]；

$Y_a=\left[\begin{array}{ccc}{Y}_a\left(\mathrm{1,1}\right)& Y_a\left(\mathrm{1,2}\right)& Y_a\left(\mathrm{1,3}\right)\\ Y_a{\left(\mathrm{1,2}\right)}^T& Y_a\left(\mathrm{2,2}\right)& Y_a\left(\mathrm{2,3}\right)\\ Y_a{(1,3)}^T& Y_a{\left(\mathrm{2,3}\right)}^T& Y_a\left(\mathrm{3,3}\right)+I_{k+1}\end{array}\right];$

其中i_k+1，I_k+1是观测更新贡献的信息向量更新和信息矩阵更新，

对于k+1时刻单艇观测，信息向量的更新为

$i_{k+1}^i={\left(H_{k+1}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}(z_{k+1}^i-h_i\left({\hat{x}}_{k+1}^i\right)+H_{k+1}^i{\hat{X}}_{k+1(-)})$

$={[0...i_{k+1}\left(i\right)...0]}^T$

其中， $i_{k+1}^i={\left({\▿h}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}(z_{k+1}^i-h_i\left({\hat{x}}_{k+1}^i\right)+{\▿h}^i{\hat{x}}_{(k+1)-}),$

为k+1时刻量测噪声阵，

是关于的非线性函数，

为雅可比矩阵，为k-1时刻的状态估计；

信息矩阵的更新为：

$I_{k+1}={\left(H_{k+1}^i\right)}^T{\left(R_{r+1}^i\right)}^{-1}{H}_{k+1}^i$

其中， $I_{k+1}(i,i)={\left({\▿h}^i\right)}^T{\left(R_{k+1}^i\right)}^{-1}{\▿h}^i,$

对于k+1时刻平台i、j间相对观测，信息向量的更新为

$i_{k+1}={\left(H_{k+1}^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+H_{k+1}^{\mathrm{ij}}{\hat{x}}_{(k+1)-})$

$={[0...i_{k+1}\left(i\right)...i_{k+1}\left(j\right)...0]}^T$

其中， $i_{k+1}\left(i\right)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+{\▿h}_i^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^i+{\▿h}_j^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^j)$

$i_{k+1}\left(j\right)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}(z_{k+1}^{\mathrm{ij}}-h^{\mathrm{ij}}({\hat{x}}_{k+1}^i,{\hat{x}}_{k+1}^j)+{\▿h}_i^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^i+{\▿h}_j^{\mathrm{ij}}{\hat{x}}_{(k+1)-}^j)$

观测贡献的信息矩阵更新为：

$I_{k+1}={\left(H_{k+1}^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{H}_{k+1}^{\mathrm{ij}}$

$=\left[\begin{array}{ccccccc}0& ...& 0& ...& 0& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& I_{k+1}(i,i)& ...& I_{k+1}(i,j)& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& I_{k+1}(j,i)& ...& I_{k+1}(j,j)& ...& 0\\ .& & .& & .& & .\\ .& ...& .& ...& .& ...& .\\ .& & .& & .& & .\\ 0& ...& 0& ...& 0& ...& 0\end{array}\right]$

其中，

$I_{k+1}(i,i)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_i^{\mathrm{ij}}$

$I_{k+1}(i,j)={\left({\▿h}_i^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_j^{\mathrm{ij}}$

$I_{k+1}(j,i)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_i^{\mathrm{ij}}$

$I_{k+1}(j,j)={\left({\▿h}_j^{\mathrm{ij}}\right)}^T{\left(R_{k+1}^{\mathrm{ij}}\right)}^{-1}{\▿h}_j^{\mathrm{ij}}$

步骤六、信息滤波的时间更新，

时间更新是由多个时刻k-1，k，k+1…状态的联合概率求k时刻状态的边缘概率，在引入历史状态的信息滤波中，维持的是多个时刻k-1，k，k+1……联合状态的信息参数，而当联合状态的维数较高时，由于引入历史状态的信息滤波联合状态的信息矩阵是稀疏的，可以很容易由联合状态求得k时刻状态的边缘概率；

步骤七、对于状态均值

的求取，

通过求解Y＝P^-1，

实现，其中Y为信息矩阵

为信息向量，P为状态方差。

Images