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

摘要

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

主权项

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 {\phi }_k^i·\mathrm{\Delta t}+w^i\\ y_{k+1}^i=y_k^i+v_k^i\sin {\phi }_k^i·\mathrm{\Delta t}+w^i\\ {\phi }_{k+1}^i={\phi }_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{{\partial h}^i}{{\partial x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$${▿h}_i^{\mathrm{ij}}=\frac{{\partial h}^{\mathrm{ij}}}{{\partial x}_k^i}{|}_{x_k^i={\hat{x}}_k^i},$${▿h}_j^{\mathrm{ij}}=\frac{{\partial h}^{\mathrm{ij}}}{{\partial 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{\Delta }{=}\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{\Delta }{=}\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为状态方差。