一种适用于多艇导航的保留历史状态的信息滤波算法，涉及一种协同导航技术。它实现了在减少了KF、EKF等常用滤波方法在协同导航应用的计算量的基础上，解决了多水面艇导航问题。其方法为：建立水面艇的系统方程和观测方程；将非线性模型线性化；通过观测更新得到实时信息参数；进行时间更新；利用信息参数求得状态均值。本发明专利技术适用于多水面艇导航。
【技术实现步骤摘要】
【技术保护点】
一种适用于多艇导航的保留历史状态的信息滤波算法，其特征在于，包括以下步骤：步骤一、建立多水面艇的运动方程选取艇的纬度、经度和航向角作为状态量，首先建立平面2维坐标系x?y，记k时刻第i条艇状态为为k时刻第i条艇在x轴的坐标，为k时刻第i条艇在y轴的坐标，为k时刻第i条艇的航向角，运动方程可表达为xk+1i=xki+vkicosφki·Δt+wiyk+1i=yki+vkisinφki·Δt+wiφk+1i=φki+wi其中Δt是航迹推算更新周期，系统噪声wi，航向角和速度构成控制输入这样k时刻n条艇的导航状态为Xk+1＝[x1；x2；…；xn]k；步骤二、建立多水面艇的观测方程，选取艇的位置信息或者速度信息和各艇之间观测距离为量测量，单艇观测方程可选为zi=zxizyi=100010xi+vik时刻选取艇间距为观测量(设i艇和j艇)zkij=(xki-xkj)2+(yki-ykj)2+vkij其中系统噪声wi和观测噪声vi、vij均是零均值白噪声，且相互独立不相关；步骤三、系统运动方程和观测方程的线性化对该非线性系统在估计的均值附近进行线性化处理，对每个艇有：其中f为非线性系统方程，则整个系统n条艇的状态转移阵Fk为对于单艇i观测，其量测方程雅可比矩阵为Hki=[0...▿hi...0]对于艇i，j间观测，其量测方程雅可比矩阵为Hkij=[0...▿hiij...▿hjij...0]其中，是线性化因子，▿hii=∂hi∂xki|xki=x^ki,▿hiij=∂hij∂xki|xki=x^ki,▿hjij=∂hij∂xkj|xkj=x^kj,h为非线性观测方程；步骤四、历史状态的保留，以及信息参数的求取考虑系统k?1时刻和k时刻的联合状态为[Xk?1Xk]，信息参数为y=yk-1-Fk-1TQk-1-1[Xk-Fk-1Xk-1]Q-1[Xk-Fk-1Xk-1]=Δyk-1yk;Y=Yk-1+Fk-1TQk-1-1Fk-1-Fk-1TQk-1-1-Qk-1-1Fk-1Qk-1-1=ΔYk-1,k-1Yk-1,kYk-1,kTYk,k;其中，y为信息向量，Y为信息矩阵，Fk?1，Qk?1分别为k?1时刻，系统线性化后的状态转移矩阵和系统噪声阵，由运动方程，k+1时刻添加状态Xk+1，得到联合状态[Xk?1XkXk+1]，结合信息参数算法过程，可得信息参数为：ya=yk-1yk-FkTQk-1[Xk+1-FkXk]Qk-1[Xk+1-FkXk]Ya=Yk-1,k-1Yk-1,k0Yk-1,kTYk,k+FkTQk-1Fk-FkTQk-10-Qk-1FkQk-1;其中，ya为状态添加后的信息向量，Ya为状态添加后的信息矩阵，Fk，Qk分别为k时刻，系统线性化后的状态转移矩阵和系统噪声阵；步骤五、系统的观测更新，通过观测zk+1来更新[Xk?1XkXk+1]的联合信息参数，观测更新后的信息参数为：ya＝[ya(1)；ya(2)；ya(3)+ik+1]；Ya=Ya(1,1)Ya(1,2)Ya(1,3)Ya(1,2)TYa(2,2)Ya(2,3)Ya(1,3)TYa(2,3)TYa(3,3)+Ik+1;其中ik+1，Ik+1是观测更新贡献的信息向量更新和信息矩阵更新，对于k+1时刻单艇观测，信息向量的更新为ik+1i=(Hk+1i)T(Rk+1i)-1(zk+1i-hi(x^k+1i)+Hk+1iX^k+1(-))=[0...ik+1(i)...0]T其中，ik+1i=(▿hi)T(Rk+1i)-1(zk+1i-hi(x^k+1i)+▿hix^(k+1)-),为k+1时刻量测噪声阵，是关于的非线性函数，为雅可比矩阵，为k?1时刻的状态估计；信息矩阵的更新为：Ik+1=(Hk+1i)T(Rr+1i)-1Hk+1i其中，Ik+1(i,i)=(▿hi)T(Rk+1i)-1▿hi,对于k+1时刻平台i、j间相对观测，信息向量的更新为ik+1=(Hk+1ij)T(Rk+1ij)-1(zk+1ij-hij(x^k+1i,x^k+1j)+Hk+1ijx^(k+1)-)=[0...ik+1(i)...ik+1(j)...0]T其中，ik+1(...
【技术特征摘要】
【专利技术属性】
技术研发人员：徐博，肖永平，杨羡，郑启辉，黄文军，池姗姗，王文佳，陈春，郭宇，田学林，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。