专利名称：一种用于水下导航的非线性滤波方法
技术领域：
本发明涉及水下地形辅助导航研究领域，特别涉及一种存在非高斯噪声的非线性系统下，通过一种分段函数逼近导航随机微分模型弱解的用于水下导航的非线性滤波方法。
背景技术：
精确导航技术是确保水下潜器有效利用和安全回收的关键技术之一。目前，水下地形辅助导航系统大多采用以惯导和多普勒计程仪组合为主，辅以如GPS、重力、地形等其它校正信息的综合导航技术。滤波是将信号中特定波段频率滤除的操作，是抑制和防止干扰的一项重要措施，但是由于水下工作环境非常复杂、存在诸多不确定性因素，使得传统滤波方法的应用受到极大限制。水下地形辅助导航过程本质上是一个最优估计问题。邓自立先生把最优估计理论要解决的问题分为三类1、模型参数的估计问题；2、时间序列、信号或状态的估计问题；3、 信息融合估计问题。地形导航问题属于第二类问题。求解方法的发展与最优估计理论的发展是一致的。最优估计经历了最小二乘法、Kalman滤波和贝叶斯估计滤波三个发展阶段(1)最优估计最基本的方法是最小二乘法，这类估计方法只需要建立测量模型， 它的基本原理是实际观测值与模型计算值的误差平方和最小，其只保证量测误差的方差最小，对测量数据进行的是批处理，不利于实时处理。(2)卡尔曼(Kalman)滤波方法，基于状态空间理论和射影理论解决状态估计问题。卡尔曼滤波采用递推计算，适宜计算机实现。对于具有高斯分布噪声的线性系统，可以得到系统状态的递推最小均方差估计。卡尔曼滤波已广泛应用于各种领域，如导航制导、 目标跟踪、信号处理、控制等。卡尔曼最初提出的滤波理论只适用于线性系统，Bucy等人提出了扩展卡尔曼滤(Extended Kalman Filtering，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域(可参考文献R S Bucy，K D Senne. Digital synthesis of nonlinear f liters [J], Automatica, 1971, 7 =287-298)。EKF的基本思想是将非线性系统进行线性化， 然后进行卡尔曼滤波，因此EKF是一种次优滤波。与对非线性函数线性化相比，对状态符合高斯分布的近似要简单的多，基于这种思想，Julier等人提出无迹卡尔曼滤波(Unscented Kalman Filter,UKF)(可参考文献S. J. Julier, J. K. Uhlmann. non-divergent estimation algorithm in the presence of unknown correlations[C]. Proceedings of the IEEE American Control Conference, Albuquerque, NM, USA, 1997,4 :2369-2373)，认为状态的概率密度分布可通过能捕获密度函数的均值和方差的sigma点来描述，通过UT (Unscented Transction)变换获得sigma点和它们的权值，而后把通过系统的状态方程传递的sigma点加权求和，得到后验均值和协方差。具体可参考文献Tine L,Herman B, Joris D S.于2004 年发表的文章Kalman Filters for nonlinear systems :a comparison of performance。现有的Kalman滤波、EKF和UKF滤波仅适用于具有高斯分布噪声的系统，都以求得系统状态概率分布的二阶矩为目的，估计精度不是很高。并且在具体的实际应用中，系统
5状态概率的实际分布也可能不是正态分布，采用这类滤波方法都是通过求得密度函数的均值和方差来描述状态的概率密度分布，然而系统状态基于量测信息的后验概率密度最能代表状态的实际分布，所以直接对系统状态概率密度函数进行求解更能明确系统状态各种可能性。(3)基于贝叶斯(Bayesian)估计的滤波方法，求解的是系统状态基于量测信息的后验概率密度。使用贝叶斯估计的难点是求解状态的先验概率密度函数和进行积分运算。 只有很少数的系统可以求得贝叶斯估计的最优解，如线性高斯系统可由Kalman滤波进行求解，大多数系统是采用近似或逼近的方式进行求解，例如粒子滤波(Partical Filter)方法是采用一组带有权值的粒子(随机样本)通过状态传递方程来逼近状态的概率密度，将积分运算转化为有限样本点的加权和。三次样条插值算法也属于基于贝叶斯估计的范畴。文献公开号为CN1012^630 于2008-7-23公开的中国发明专利申请《基于三次样条函数的插值方法》，该文献中应用三次样条插值算法实现了一种边缘自适应处理的图像缩放方法，提供了一种插值后图像光滑，轮廓清晰的基于三次样条函数的自适应插值方法，所采用的技术方案是，先检测插值点的位置状态，然后对处于不同的位置状态的插值点再采用不同的处理方法进行插值运算。 近几年，粒子滤波算法在贝叶斯估计方面应用非常广泛，文献张共愿，赵忠在2006年6月发表的《粒子滤波及其在导航系统中的应用综述》，该文献中提出了传统的扩展卡尔曼滤波方法要求对非线性系统近似线性化，有可能会引入较大的模型误差，但应用粒子滤波可解决这一问题，粒子滤波算法可以直接应用于原系统的非线性模型当中，并且不需考虑系统噪声和量测噪声是否为高斯白噪声，都能得到很好的滤波效果。该文献中介绍了粒子滤波的理论基础-贝叶斯估计及具体的实现方式-蒙特卡罗方法；指出粒子滤波存在的退化问题，并从减小退化现象入手将重要性采样和再采样方法引入到算法之中；最后阐述了粒子滤波在导航系统中的一些应用。然而针对粒子滤波的退化问题，使其难以保持较高的收敛性和光滑性，如何更好的跟踪系统状态的变化，提高其分布精度，是目前水下地形导航随机微分模型求解领域上亟待解决的问题，但文献3中并没有给出解决的方法和启示。
发明内容
本发明的目的是为了解决以水下地形导航系统为背景，建立的导航随机微分模型的弱解方程的弱解很难求得的问题，提出了一种用于水下导航的非线性滤波方法，采用一种三次样条插值函数来分段逼近其解可得到状态的先验概率密度函数，再由贝叶斯公式得到状态的后验概率密度函数，同时也解决了构造三次样条插值条件的难点问题，实现更好的跟踪系统状态的变化，具有很高的估计精度、收敛速度和收敛光滑性。本发明一种用于水下导航的非线性滤波方法，首先为水下导航建立状态微分与量测离散方程，并得到一个用来表示潜器运动状态的弱解方程，然后通过以下步骤对弱解方程进行逼近来对潜器的运动状态进行估计步骤一、将初始时刻的条件概率密度作为正态分布，选定投影区间[a，b]和分段区间a = < X1 < & <丨< ι = b，构造三次样条插值函数Pi(X)Pi (χ) = Mm (& Χ) +Μ, (X _ 1) + A1 (χ - χ.,, )+ij,,x e [χ^, Xi]，i = 1,2, ...，η
6/2, 6/2,
6
其中，Ai, Bi*关于时间的积分常数，系数Mi = p〃 (Xi)5P" (Xi)为三次样条插值函数P(X)在节点Xi处的二阶导数，η表示分段区间的个数，为大于0的整数，hi表示第i个分段区间的长度，步骤二、通过上一个时刻的后验概率密度函数得到插值点的值，再利用前向柯尔莫哥洛夫方程构造当前时刻的三次样条插值函数的插值点，把插值点代入式(1)中，得到用Mi, Mi^1把表示的常数Ai, Bi,从而得到只含先验系数Mi, Mi^1的先验概率密度函数；步骤三、利用三次样条插值函数在每个节点处的一阶连续导数和边界条件列出 n+1个方程组，用追赶法求出先验系数Mi,将先验系数Mi代入式(1)得到先验概率密度函数；步骤四、根据当前时刻的量测信息确定似然概率密度，把先验概率密度和似然概率密度带入贝叶斯公式，得到当前潜器运动状态的后验概率密度；步骤五、判断仿真时间是否已到，若仿真时间未到，转步骤二继续执行；若仿真时间到达，则输出仿真结果，结束本次仿真。本发明的优点与积极效果在于(1)本发明滤波方法在保证图像平滑性的基础上，增加边缘锐度，同时有效的消除了原三次样条插值函数插值方法中，由于图像边缘两侧非连续状态像素互相干扰导致的边缘两侧虚像的问题；( 本发明滤波方法充分利用了三次样条插值的特性，通过前向柯尔莫哥洛夫方程构造三次样条插值条件，在函数空间中对状态的先验概率密度函数进行分段逼近，再通过贝叶斯公式得到系统状态的后验概率密度，具有很高的估计精度、收敛速度和收敛光滑性，可以很好的跟踪系统状态的变化。
图1是本发明滤波方法的流程示意图；图2中，(a)是本发明滤波方法在第1秒时的潜器运动状态的概率密度示意图； (b)是本发明滤波方法在第2秒时的潜器运动状态的概率密度示意图；(c)是本发明滤波方法在第31秒时的潜器运动状态的概率密度示意图；(d)是本发明滤波方法在第32秒时的潜器运动状态的概率密度示意图；(e)是本发明滤波方法在第33秒时的潜器运动状态的概率密度示意图；(f)是本发明滤波方法在第50秒时的潜器运动状态的概率密度示意图；图3A是时间间隔为0. 1秒时，本发明实施例中采用本发明方法与采用粒子滤波方法进行仿真得到的位置均值的对比示意图；图:3B是时间间隔为0. 1秒时，本发明实施例中采用本发明方法与采用粒子滤波方法进行仿真得到的位置标准差的对比示意图；图4A是时间间隔为0. 01秒时，本发明实施例中采用本发明方法与采用粒子滤波方法进行仿真得到的位置均值的对比示意图；图4B是时间间隔为0. 01秒时，本发明实施例中采用本发明方法与采用粒子滤波方法进行仿真得到的位置标准差的对比示意图；图5A是本发明实施例中潜器运动状态均方根误差的对比示意图；图5B是本发明实施例中潜器运动状态的标准差的对比示意图。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。本发明一种用于水下导航的非线性滤波方法，目的是利用基于贝叶斯估计的滤波方法的优点，提供一种三次样条插值分段逼近导航随机微分模型弱解的方法，从而有效地保证了其可以更好的跟踪系统状态的变化，本发明的非线性滤波方法具有很高的估计精度、收敛速度和收敛光滑性。以水下地形辅助导航系统为背景，潜器的运动状态可用随机过程X (t)表示，假设对于时间te
=Q(t)dt，0⑴
q是m维白噪声过程，E[ekekT] = R(k)，R{k)eW^。β t与q相互独立，tk表示采样时间。 Q(t)、R(k)分别表示为过程噪声和量测噪声的协方差，简记i = Rk。随机微分方程的解作为随机过程，可以抽象地用一个泛函Xt = J[x0, β t，t]表示。 随机微分方程有强解和弱解两种，仅有一些类型的随机微分方程有强解的封闭解。弱解是连续函数空间中的一个概率，由转移函数决定，针对本发明方法，弱解就是指下面得到的先验概率。在系数μ (x, t)、σ (χ, t)合适的条件下，潜器的运动状态的条件概率密度ρ (X， t|zt)满足前向柯尔莫哥洛夫方程
权利要求
1. 一种用于水下导航的非线性滤波方法，其特征在于，首先为水下地形辅助导航系统建立状态微分与量测离散方程，并得到一个用来表示潜器运动状态的弱解方程，然后通过以下步骤对弱解方程进行逼近来对潜器的运动状态进行估计步骤一、将初始时刻的条件概率密度作为正态分布，选定投影区间[a，b]和分段区间a =X0 < X1 < X2 < ... < Xn = b,构造三次样条插值函数Pi (χ)P1 (X) = Mia (Χ' Χ) +Μ, (Χ _ ；‘') +4 (x- X1,!e [Xi^1, Xi],i = 1,2, ...，n (1)6/2 6/2ιI其中，Ai, Bi为关于时间的积分常数，先验系数Mi = p" (Xi)jp" (Xi)为三次样条插值函数P(X)在节点Xi处的二阶导数，η表示分段区间的个数，为大于0的整数，h表示第i个分段区间的长度，步骤二、通过上一个时刻的后验概率密度函数得到插值点的值，再利用前向柯尔莫哥洛夫方程构造当前时刻的三次样条插值函数的插值点，把插值点代入式(1)中，得到用Mi, Mi^1表示的积分常数Ai, Bi,从而得到只含先验系数的先验概率密度函数；步骤三、利用三次样条插值函数在每个节点处的一阶连续导数和边界条件列出n+1个方程组，用追赶法求出先验系数Mi,将先验系数Mi代入式(1)后，得到当前时刻的先验概率密度；步骤四、根据当前时刻的量测信息确定似然概率密度，把先验概率密度和似然概率密度带入贝叶斯公式，得到当前时刻的潜器运动状态的后验概率密度；步骤五、判断仿真时间是否已到，若仿真时间未到，转步骤二继续执行；若仿真时间到达，则输出仿真结果，结束本次仿真。
2.根据权利要求1所述的一种用于水下导航的非线性滤波方法，其特征在于，步骤一中所述的条件概率密度满足前向柯尔莫哥洛夫方程Lrydp_l^d2(g{x,t)Q{t)gT(x,t)-p) ^d(f(x,t)-p)⑵dt 2SxrSxs台 OXr其中，P表示条件概率密度，t表示时间，f (X，t)表示潜器运动状态变化与潜器运动状态之间的关系，g(x，t)表示噪声扩散，Q(t)为过程噪声的协方差，m表示潜器运动状态空间的维数，xr与Xs表示在m维空间中χ向量的坐标。
3.根据权利要求1所述的一种用于水下导航的非线性滤波方法，其特征在于，所述的步骤二具体是首先，在节点Xi、Xi-!处的三次样条插值函数P (Xi)、P(Xh)满足前向柯尔莫哥洛夫方程，简Α(χ，0“，卯”g’/M"，有d{gQgT■ p) _d(gQgT) d{p)d(f-p) d(f) | d(p),( ( -i O SL^ &, 1 JOXs OXs OXsOXr OXr OXr则得^{sQ^-p) ^(gQg^) ^ | 2 8{gQg^)d(p) | ^t O2(^) ⑶dxrdxs SxrSxs dxrdxs dxrdxs
4.根据权利要求1所述的一种用于水下导航的非线性滤波方法，其特征在于，步骤J 中所述的η+1个方程组，具体是
5.根据权利要求1所述的一种用于水下导航的非线性滤波方法，其特征在于，所述的步骤四具体是首先，确定当前时刻的似然概率密度P(zk+1/x)为
6.根据权利要求1所述的一种用于水下导航的非线性滤波方法，其特征在于，步骤五所述的仿真结果是指步骤四中得到的潜器运动状态的后验概率密度的均值和方差
全文摘要
本发明公开了一种用于水下导航的非线性滤波方法，用于从水下地形导航随机微分模型中求得弱解。本发明通过使用了三次样条插值函数来分段逼近导航随机微分模型的弱解可得到状态的先验概率密度函数，其中利用前向柯尔莫哥洛夫方程解决了构造三次样条插值点的难点问题，然后再由贝叶斯公式得到状态的后验概率密度函数。本发明方法充分利用了三次样条插值具有计算简单、稳定性好、收敛性有保证、易于在计算机上实现并且能保证整体曲线的光滑性等特性能更好跟踪系统状态各种可能性，具有很高的估计精度、收敛速度和收敛光滑性，可以很好的跟踪系统状态的变化。
文档编号G01C21/00GK102252672SQ20111010895
公开日2011年11月23日 申请日期2011年4月28日 优先权日2011年4月28日
发明者刘厂, 张振兴, 李刚, 赵玉新, 陈立娟 申请人:哈尔滨工程大学