全局估计自适应两阶段平方根容积滤波的方法与流程

本发明涉及滤波估计领域，具体地说，特别涉及一种全局估计自适应两阶段平方根容积滤波的方法。

背景技术：

非线性滤波算法是在非线性系统模型下利用离散的传感器观测量来估计目标的连续状态并滤去随机噪声的过程。目前几种常见的非线性kalman滤波有各自的优缺点。扩展kalman滤波(ekf)将非线性函数进行泰勒展开，忽略高维项进行线性化，但这种方法只适合系统模型足够平滑的弱非线性函数，若系统是强非线性系统，则会因为滤波误差较大而失去有效性，同时在计算时需要计算雅克比矩阵，计算量较大。无迹kalman滤波(ukf)不需要计算jacobian矩阵，计算量与ekf相当，在强非线性系统下也有较高的滤波精。但是ukf没有经过严格的数学推导，在状态维数较高时，滤波精度会下降。采用三度spherical-radial容积规则近似非线性函数传递的后验均值和协方差的容积kalman滤波，经过了严密的数学推导，在理论上具有严格的保证。在滤波迭代过程中，有些因素的存在会导致误差协方差矩阵负定，比如数值计算中的舍入误差、初始值误差较大和观测噪声较大等，从而导致滤波器不稳定，甚至无法工作。

技术实现要素：

为了解决现有技术的问题，本发明实施例提供了一种全局估计自适应两阶段平方根容积滤波的方法。所述技术方案如下：

一方面，提供了一种全局估计自适应两阶段平方根容积滤波的方法，包括：

首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积kalman滤波算法；

然后基于sage-husa滤波算法，提出自适应两阶段平方根容积kalman滤波算法；

在两阶段容积kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计。

进一步的，所述平方根两阶段容积kalman滤波算法中的系统模型具体为；

带有随机偏差的非线性高斯系统模型可表示为：

其中k是离散时间序列，xk∈r^n×1是系统的状态向量，bk∈r^p×1是系统偏差向量，zk∈r^m×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列。其中均值为e(vk)＝r，方差和rk满足如下条件：

令

hk(xk)＝hk(xk)+fkbk

系统模型可以改写为如下形式：

xk+1＝fk(xk)+ωk

zk＝hk(xk)+υk

其统计特性满足如下公式(5-2)所示：

初始状态x0、b0与ωk、υk无关，且满足

进一步的，所述平方根两阶段容积kalman滤波算法步骤如下：

step1时间更新:

⑴计算容积点xi,k-|k-1和通过状态转移函数计算得到的传播容积点

⑵估计k时刻的状态预测值和

⑶估计k时刻的状态误差协方差平方根因子

其中定义如公式(5-4)所示：

⑷借助uk估计k时刻的状态误差协方差和其中

step2量测更新:

⑴计算容积点xi,k|k-1和通过观测函数计算得到的传播容积点zi,k|k-1；

⑵估计k时刻的量测预测值

⑶估计k时刻量测误差协方差的平方根因子

szz，k|k-1＝tria([∈k|k-1sr,k])(5-5)

其中∈k|k-1定义如公式(5-4)所示：

⑷估计k时刻的量测误差协方差

⑸估计k时刻的交叉协方差

⑹估计卡尔曼增益和其中

⑺计算k时刻状态估计值和

⑻计算k时刻状态误差协方差估计值和vk。

进一步的，所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于sage-husa算法可得次优常值噪声估计器：

其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积kalman滤波算法中有

xi,j-1|j-1与xi,j|j-1分别表示j-1与j时刻状态估计值与计算得到的容积点。采用map估计器，得到噪声统计特性未知的无偏估计器，结合容积kalman滤波算法，可得无偏常值噪声估计器：

其中表示新息向量。

进一步的，所述系统模型如公式(5-1)所示，公式(5-2)为其噪声统计特性，如果其噪声为时变的情况，需要减弱或消除陈旧量测信息，加大最近量测信息的作用，因此对求和式中的每一项引入不同的加权系数；采用渐消记忆指数加权的方式对时变噪声进行估计，可得无偏时变噪声估计器：

其中dk-1＝(1-b)/(1-b^k)，b为遗忘因子，用来限制滤波器的记忆长度，通常b的取值范围为0.95＜b＜0.99，b的值越大越加重最近量测信息的作用，当噪声变化快时，b的取值应偏大，反之则偏小。

进一步的，所述系统模型形如公式(5-1)所示，在实际应用中，量测噪声容易受到量测误差和环境扰动等方面的影响，因此在公式(5-2)所示的噪声统计特性中，系统噪声ωk为均值为qk，方差为qk的高斯白噪声序列，量测噪声υk也为高斯白噪声序列，但是统计特性未知；

结合sage-husa算法，首先采用全局估计的方式，对未知的量测噪声统计特性进行全局估计，然后再根据两阶段方法，对滤波器进行分解。

进一步的，所述在两阶段容积kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计具体为：

全局估计自适应两阶段平方根容积kalman滤波算法中均以上角标g标明以示区分，算法步骤如下：

初始化：

step1时间更新：

⑴计算容积点xi,k-|k-1和通过状态转移函数计算得到的传播容积点

⑵由于系统噪声均值不为0，故有

⑶估计k时刻的状态预测值和

得到

⑷估计k时刻的状态误差协方差平方根因子

其中定义如公式(5-26)所示：

⑸估计k时刻的状态误差协方差和

⑹借助uk计算和其中

step2量测更新：

⑴计算容积点xik|k-1和通过观测函数计算得到的传播容积点

⑵由于测量噪声的均值也不为0，估计k时刻的量测预测值

⑶估计k时刻的量测误差协方差的平方根因子

其中定义如公式(5-28)所示：

⑷估计k时刻的量测误差协方差

⑸估计k时刻的交叉协方差

⑹估计卡尔曼增益和其中

⑺计算k时刻状态估计值和

⑻计算k时刻状态误差协方差估计值和vk；

step3噪声估计：

⑴估计k+1时刻量测噪声均值

⑵估计k+1时刻量测噪声方差

step4：如果k<n，n为跟踪时间，转到step1。

进一步的，在全局估计自适应两阶段平方根容积kalman滤波算法中，对于量测噪声统计特性的估计是采用sage-husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件，采用两阶段滤波算法。

本发明实施例提供的技术方案带来的有益效果是：

本发明通过采用saga-husa估计器实时估计噪声的未知统计特性，再将实时更新的统计特性带入kalman滤波算法，改善噪声统计特性未知系统中的滤波估计效果，再用新得到的系统状态和误差协方差再次修正噪声统计特征，以此形成递推循环；在全局估计自适应两阶段平方根容积kalman滤波算法(atsckf-g)中，对于量测噪声统计特性的估计是采用sage-husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件，从而提高了滤波的跟踪精度，使噪声的估计值逐步趋向于真实值，具有自适应的能力。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例的一种全局估计自适应两阶段平方根容积滤波的方法流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

本发明提供了一种全局估计自适应两阶段平方根容积滤波的方法，参见图1，包括：

s100：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积kalman滤波算法；

s200：然后基于sage-husa滤波算法，提出自适应两阶段平方根容积kalman滤波算法；

s300：在两阶段容积kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计。

进一步的，所述平方根两阶段容积kalman滤波算法中的系统模型具体为；

带有随机偏差的非线性高斯系统模型可表示为：

其中k是离散时间序列，xk∈r^n×1是系统的状态向量，bk∈r^p×1是系统偏差向量，zk∈r^m×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列。其中均值为e(υk)＝r，方差和rk满足如下条件：

令

hk(xk)＝hk(xk)+fkbk

系统模型可以改写为如下形式：

xk+1＝fk(xk)+ωk

zk＝hk(xk)+υk

其统计特性满足如下公式(5-2)所示：

初始状态x0、b0与ωk、vk无关，且满足

进一步的，具体地，提出容积kalman滤波的同时，还提出了容积kalman滤波的平方根形式，即在递推运算中使用误差协方差阵的平方根，而不用协方差矩阵，这种方法可以提高滤波器的稳定性，抑制滤波发散。在此引入平方根的思想，得到更为稳定的平方根两阶段容积kalman滤波算法步骤如下：

step1时间更新：

⑴计算容积点xi,k-|k-1和通过状态转移函数计算得到的传播容积点

⑵估计k时刻的状态预测值和

⑶估计k时刻的状态误差协方差平方根因子

其中定义如公式(5-4)所示：

⑷借助uk估计k时刻的状态误差协方差和其中

step2量测更新:

⑴计算容积点xi,k|k-1和通过观测函数计算得到的传播容积点zi,k|k-1；

⑵估计k时刻的量测预测值

⑶估计k时刻量测误差协方差的平方根因子

szz,k|k-1＝tria([∈k|k-1sr,k])(5-5)

其中∈k|k-1定义如公式(5-4)所示：

⑷估计k时刻的量测误差协方差

⑸估计k时刻的交叉协方差

⑹估计卡尔曼增益和其中

⑺计算k时刻状态估计值和

⑻计算k时刻状态误差协方差估计值和vk。

进一步的，具体地，上述的各类容积kalman滤波算法的推导，前提条件均是假设系统噪声和量测噪声统计特性精确已知，但在实际应用中，系统噪声和量测噪声的统计特性未知或时变的情况普遍存在，这使得常规容积kalman滤波算法失去了最优性，估计精度下降，甚至引起了滤波发散。本小节主要研究能够对噪声的未知统计特性在线估计修正的自适应两阶段容积kalman滤波算法。

其中，sage-husa算法是无偏次优极大后验噪声估计算法，其原理清晰，计算简单，可同时估计噪声的均值和方差，被广泛的应用于各类自适应滤波器中。

所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于sage-husa算法可得次优常值噪声估计器：

其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积kalman滤波算法中有

xi,j-1|j-1与xi,j|j-1分别表示j-1与j时刻状态估计值与计算得到的容积点。采用map估计器，得到噪声统计特性未知的无偏估计器，结合容积kalman滤波算法，可得无偏常值噪声估计器：

其中表示新息向量。

进一步的，所述系统模型如公式(5-1)所示，公式(5-2)为其噪声统计特性，如果其噪声为时变的情况，需要减弱或消除陈旧量测信息，加大最近量测信息的作用，因此对求和式中的每一项引入不同的加权系数；采用渐消记忆指数加权的方式对时变噪声进行估计，可得无偏时变噪声估计器：

其中dk-1＝(1-b)/(1-b^k)，b为遗忘因子，用来限制滤波器的记忆长度，通常b的取值范围为0.95＜b＜0.99，b的值越大越加重最近量测信息的作用，当噪声变化快时，b的取值应偏大，反之则偏小。

具体地，由公式(5-18)-(5-20)可知，通过map估计器得到的两个噪声估计器之间不是完全独立无关的，而且非独立的，因此必须已知系统噪声ωk和量测噪声υk中的一个才能对未知噪声进行估计修正，否则会引起滤波发散。

进一步的，所述系统模型形如公式(5-1)所示，在实际应用中，量测噪声容易受到量测误差和环境扰动等方面的影响，因此在公式(5-2)所示的噪声统计特性中，系统噪声ωk为均值为qk，方差为qk的高斯白噪声序列，量测噪声υk也为高斯白噪声序列，但是统计特性未知；

结合sage-husa算法，首先采用全局估计的方式，对未知的量测噪声统计特性进行全局估计，然后再根据两阶段方法，对滤波器进行分解；在这个过程中，因使用的是平方根容积kalman滤波算法，避免了误差协方差pk|k-1失去正定性导致的滤波发散，不需要进行发散判断，即可保持滤波的正常递推进行。

进一步的，所述在两阶段容积kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计具体为：

全局估计自适应两阶段平方根容积kalman滤波算法中均以上角标g标明以示区分，算法步骤如下：

初始化：

step1时间更新：

⑴计算容积点xi，k-|k-1和通过状态转移函数计算得到的传播容积点

⑵由于系统噪声均值不为0，故有

⑶估计k时刻的状态预测值和

得到

⑷估计k时刻的状态误差协方差平方根因子

其中定义如公式(5-26)所示：

⑸估计k时刻的状态误差协方差和

⑹借助uk计算和其中

step2量测更新：

⑴计算容积点xi,k|k-1和通过观测函数计算得到的传播容积点

⑵由于测量噪声的均值也不为0，估计k时刻的量测预测值

⑶估计k时刻的量测误差协方差的平方根因子

其中定义如公式(5-28)所示：

⑷估计k时刻的量测误差协方差

⑸估计k时刻的交叉协方差

⑹估计卡尔曼增益和其中

⑺计算k时刻状态估计值和

⑻计算k时刻状态误差协方差估计值和vk；

step3噪声估计：

⑴估计k+1时刻量测噪声均值

⑵估计k+1时刻量测噪声方差

step4：如果k<n，n为跟踪时间，转到step1。

进一步的，在全局估计自适应两阶段平方根容积kalman滤波算法中，对于量测噪声统计特性的估计是采用sage-husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件，采用两阶段滤波算法。

本发明实施例提供的技术方案带来的有益效果是：

本发明通过采用saga-husa估计器实时估计噪声的未知统计特性，再将实时更新的统计特性带入kalman滤波算法，改善噪声统计特性未知系统中的滤波估计效果，再用新得到的系统状态和误差协方差再次修正噪声统计特征，以此形成递推循环；在全局估计自适应两阶段平方根容积kalman滤波算法(atsckf-g)中，对于量测噪声统计特性的估计是采用sage-husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件，从而提高了滤波的跟踪精度，使噪声的估计值逐步趋向于真实值，具有自适应的能力。

以上仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。