一种迭代容积点无迹卡尔曼滤波方法与流程

本发明属于通信导航技术领域，涉及一种迭代容积点无迹卡尔曼滤波方法。

背景技术：

卡尔曼滤波技术是通过系统输入输出观测数据，对系统状态进行最优估计的算法，具有重要的意义，在通信,导航,制导与控制等多领域得到了较好的应用。在含有已知噪声的线性定常系统中，一般的线性卡尔曼滤波即可有效使用，但是在非线性系统中，由于状态转移矩阵无法被线性的表示，因此有多种改进的卡尔曼滤波技术产生。

扩展卡尔曼滤波是将非线性系统函数按照泰勒展开，取其线性部分进行状态矩阵的求解，此方法虽然相比于其他非线性滤波在计算量上较小，但无法解决高自由度非线性强的系统估计问题。容积卡尔曼滤波和无迹卡尔曼滤波是一种以点来拟合线的滤波技术，通过在非线性函数中取一系列的点，然后通过点集的映射来拟合整个非线性函数，无迹卡尔曼滤波流程如图1所示，容积点的特点是权重相同，但是容积点无法很好拟合强非线性函数的统计特性，而无迹卡尔曼滤波的sigma点在滤波过程中权重会变为负数，这会使滤波的结果发散。

综上所述，面对实际环境中多种传感器间的非线性耦合、协同导航模式，传统的滤波方式无法将计算量问题、非线性滤波发散问题、负权重问题协同处理，需要得到改进。

技术实现要素：

为解决上述问题，本发明公开了一种迭代容积点无迹卡尔曼滤波方法，在原有卡尔曼滤波方法上进行改进，能够有效应用在高自由度强非线性的系统中。本发明首先给出了新的sigma点的选取方法；其次根据滤波过程中权重正定性的要求，给出了各sigma点的权重系数，更接近状态量的统计特性，从而解决了传统无迹卡尔曼滤波由于误差协方差矩阵的非正定性引起的滤波发散问题；再次，给出了一般性的容积点卡尔曼滤波算法的设计流程；最后利用参数迭代的方式，在线诊断滤波中sigma点权重的正定性。

为了达到上述目的，本发明提供如下技术方案：

一种迭代容积点无迹卡尔曼滤波算法，包括如下步骤：

1)迭代容积点无迹卡尔曼滤波算法sigma点的选取：将容积卡尔曼滤波算法中选取的容积点添加到无迹卡尔曼滤波算法的sigma点中，形成新的sigma点集在线计算状态量的均值和协方差；

2)重新确定sigma点的加权系数：重新确定滤波过程中sigma点的权重系数，在线计算上一时刻状态预测值的平均值，并与通过sigma点非线性映射加权平均计算后的状态量相比，确定权重系数的正定性，通过对sigma点加权平均计算后的状态量平均值必须等于前一步状态预测值的平均值，并且保证在整个滤波过程中权重均是正定的；

3)给出容积点无迹卡尔曼滤波算法的流程：利用改进后的sigma点拟合非线性函数，并通过非线性映射后的统计特性更新最优估计的状态量和协方差矩阵；

4)迭代计算容积点无迹卡尔曼滤波算法：在容积点无迹卡尔曼滤波中引入卡尔曼增益迭代系数，并实时检测滤波过程中权重的正定性，避免滤波的发散。

进一步的，所述步骤1)中迭代容积点无迹卡尔曼滤波算法sigma点的选取的具体步骤包括：

(1.1)在容积积分中，利用2n个等权球面点来积分计算∫f(y)dy，其中f(y)是任意非线性系统函数，其中，n是函数变量的个数；

(1.2)在原有的无迹卡尔曼滤波的2n+1个sigma点中添加容积点，形成新的4n+1个sigma点集，通过对sigma点加权平均计算后的状态值平均值必须等于前一步状态预测值的平均值，新的sigma点定义为：

其中，m是初始状态量的均值；p是初始状态量的误差协方差矩阵；k为经验取值；α决定sigma点的散布程度；t是矩阵的转置；ei定义为：

进一步的，所述步骤2)中重新确定sigma点的加权系数的具体步骤包括：

(2.1)在原有的容积点等权重系数中引入微调参数λ、κ、α、β，抑制无迹卡尔曼滤波sigma点权重的非正定性，将权重定义为：

进一步的，所述步骤3)具体包括如下步骤：

(3.1)利用所述步骤1)和2)中改进的sigma点进行非线性函数的映射，对系统进行状态量的一步预测，并估计预测更新中的协方差矩阵：

其中，x是状态量，q是噪声的协方差矩阵，u是输入，f(χk,uk)是状态量；

(3.2)使用新的微调参数λ和来计算sigma点，实现重排传递函数点，即χ′k+1/k和w′i，计算卡尔曼滤波系数，并完成本次状态量的最优估计：

其中，υ为sigma点通过观测矩阵的映射；h为观测矩阵；为观测量的权重；rk为观测噪声；y为观测值；k为卡尔曼增益；

(3.3)将式(3.2)中pk+1和用于下次循环计算的新sigma点集：

进一步的，所述步骤4)具体包括如下步骤：

(4.1)定义参数j如下，用于索引在迭代过程中的次数：

(4.2)将如权利要求1所述步骤1)中式(1.1)的参数表示为p0＝pk+1,j+1，计算sigma点，重复更新如权利要求1步骤3)中的卡尔曼滤波流程；

(4.3)引入参数计算迭代过程中新的状态量，将式(4.1)中的状态量形式改写为：

其中，g为比例参数；

(4.4)定义参数为：

其中，h为观测向量函数；

(4.5)定义不等式为：

用于在线诊断滤波过程中权重的正定性，并且在每一次迭代中计算j＝j+1,g＝ηg，直到迭代过程结束，其中，r为观测噪声协方差矩阵。

与现有技术相比，本发明具有如下优点和有益效果：

本发明能够有效应用在含有随机噪声的高自由度强非线性的系统中，协同处理解决计算量问题、非线性滤波发散问题、负权重问题，能够有效提高状态量的估计精度和实时性，不会使滤波结果发散。本发明相对于容积卡尔曼滤波可以更好地拟合非线性系统函数的统计特性，相对于无迹卡尔曼滤波可以避免sigma点权重的非正定性。

附图说明

图1是无迹卡尔曼滤波算法的流程图。

图2是本发明提供的迭代计算容积点卡尔曼滤波方法的流程图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明所述的一种迭代容积点卡尔曼滤波方法，其流程如图2所示，包含以下步骤：

步骤1：初始化非线性系统的状态初值与协方差矩阵，并将容积卡尔曼滤波算法中选取的容积点添加到无迹卡尔曼滤波算法的sigma点中，形成新的sigma点集在线计算状态量的均值和协方差，具体包括如下子步骤：

在容积积分中，利用2n个等权球面点(n是函数变量的个数)来积分计算∫f(y)dy，其中f(y)是任意非线性系统函数。

在原有的无迹卡尔曼滤波的2n+1个sigma点中添加容积点，形成新的4n+1个sigma点集，本发明方法中通过对sigma点加权平均计算后的状态值平均值必须等于前一步状态预测值的平均值，新的sigma点定义为：

其中，m是初始状态量的均值；p是初始状态量的误差协方差矩阵；k为经验取值，通常取0；α决定sigma点的散布程度，通常取一个小的正值；t是矩阵的转置；ei定义为：

其中，i是单位矩阵。

步骤2：重新确定滤波过程中sigma点的权重系数，在线计算上一时刻状态预测值的平均值，并与通过sigma点非线性映射加权平均计算后的状态量相比，并令sigma点加权平均计算后的状态量平均值必须等于前一步状态预测值的平均值，确定权重系数的正定性，重新确定权重系数的过程如下：

在原有的容积点等权重系数中引入微调参数λ、κ、α、β，抑制无迹卡尔曼滤波sigma点权重的非正定性，将权重定义为：

步骤3：利用步骤1和步骤2中改进的sigma点集求解状态转移矩阵，建立非线性系统的预测更新模型，最优估计本时刻的状态量，具体包括如下过程：

改进的sigma点进行非线性函数的映射，对系统进行状态量的一步预测，并估计预测更新中的协方差矩阵。

其中，x是状态量，q是噪声的协方差矩阵，u是输入，f(χk,uk)是状态量。

步骤4：计算预测更新中的协方差矩阵以及卡尔曼增益，在下一时刻中继续更新状态量，具体包括如下过程：

使用新的微调参数λ和来计算sigma点，实现重排传递函数点，即χ′k+1/k和w′i，计算卡尔曼滤波系数，并完成本次状态量的最优估计。

其中，υ为sigma点通过观测矩阵的映射；h为观测矩阵；为观测量的权重；rk为观测噪声；y为观测值；k为卡尔曼增益。

将式(4.1)中pk+1和用于下次循环计算的新sigma点集：

p′＝pk+1

步骤5：在卡尔曼增益中引入迭代系数，在线诊断权重系数的正定性，重复计算迭代过程中新的状态量，具体包括如下过程：

引入参数计算迭代过程中新的状态量，将式(4.1)中的状态量形式改写为：

其中，g为比例参数，j用于所以迭代过程的次数，且如下定义：

其中，x为迭代过程中索引的状态量。

将如步骤1中式(1.1)的参数表示为p0＝pk+1,j+1，计算sigma点，重复更新步骤3中的卡尔曼滤波流程。

定义参数为：

其中，h为观测向量函数。

定义不等式为：

用于在线诊断滤波过程中权重的正定性，其中，r为观测噪声协方差矩阵，并且在每一次迭代中j＝j+1,g＝ηg，直到迭代过程结束。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。