一种用于水下导航的非线性滤波方法

摘要

本发明公开了一种用于水下导航的非线性滤波方法，用于从水下地形导航随机微分模型中求得弱解。本发明通过使用了三次样条插值函数来分段逼近导航随机微分模型的弱解可得到状态的先验概率密度函数，其中利用前向柯尔莫哥洛夫方程解决了构造三次样条插值点的难点问题，然后再由贝叶斯公式得到状态的后验概率密度函数。本发明方法充分利用了三次样条插值具有计算简单、稳定性好、收敛性有保证、易于在计算机上实现并且能保证整体曲线的光滑性等特性能更好跟踪系统状态各种可能性，具有很高的估计精度、收敛速度和收敛光滑性，可以很好的跟踪系统状态的变化。

主权项

一种用于水下导航的非线性滤波方法，其特征在于，首先为水下地形辅助导航系统建立状态微分与量测离散方程，并得到一个用来表示潜器运动状态的弱解方程，然后通过以下步骤对弱解方程进行逼近来对潜器的运动状态进行估计：步骤一、将初始时刻的条件概率密度作为正态分布，选定投影区间[a,b]和分段区间a＝x0＜x1＜x2＜...＜xn＝b，构造三次样条插值函数pi(x)： p i ( x ) = M i - 1 ( x i - x ) 3 6 h i + M i ( x - x i - 1 ) 3 6 h i + A i ( x - x i - 1 ) + B i , x ∈ [ x i - 1 , x i ] , i = 1,2 , · · · , n - - - ( 1 ) 其中，Ai,Bi为关于时间的积分常数，先验系数Mi＝p″(xi)，p″(xi)为三次样条插值函数p(x)在节点xi处的二阶导数，n表示分段区间的个数，为大于0的整数，hi表示第i个分段区间的长度，hi＝xi‑xi‑1；所述的条件概率密度满足前向柯尔莫哥洛夫方程： L ( p ) = ∂ p ∂ t = 1 2 Σ r , s = 1 m ∂ 2 ( g ( x , t ) Q ( t ) g T ( x , t ) · p ) ∂ x r ∂ x s - Σ r = 1 m ∂ ( f ( x , t ) · p ) ∂ x r - - - ( 2 ) 其中，p表示条件概率密度，t表示时间，f(x,t)表示潜器运动状态变化与潜器运动状态之间的关系，g(x,t)表示噪声扩散，Q(t)为过程噪声的协方差，m表示潜器运动状态空间的维数，xr与xs表示在m维空间中x向量的坐标；L(p)表示前向柯尔莫哥洛夫算子；步骤二、通过上一个时刻的后验概率密度函数得到插值点的值，再利用前向柯尔莫哥洛夫方程构造当前时刻的三次样条插值函数的插值点，把插值点代入式(1)中，得到用Mi,Mi‑1表示的积分常数Ai,Bi，从而得到只含先验系数的先验概率密度函数；步骤三、利用三次样条插值函数在每个节点处的一阶连续导数和边界条件列出n+1个方程组，用追赶法求出先验系数Mi，将先验系数Mi代入式(1)后，得到当前时刻的先验概率密度；步骤四、根据当前时刻的量测信息确定似然概率密度，把先验概率密度和似然概率密度带入贝叶斯公式，得到当前时刻的潜器运动状态的后验概率密度；步骤五、判断仿真时间是否已到，若仿真时间未到，转步骤二继续执行；若仿真时间到达，则输出仿真结果，结束本次仿真。