一种新型双卡尔曼滤波方法技术

本发明专利技术公开了一种新型双卡尔曼滤波方法。该方法包括确定当前NLOS误差范围；根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的状态误差协方差和噪声误差协方差；将调整后的状态误差协方差和噪声误差协方差输入经典卡尔曼滤波器中进行当前时刻的状态估计；将经典卡尔曼滤波器输出的状态估计值和状态误差方差输入至输入扩展卡尔曼滤波器中，扩展卡尔曼滤波器以经典卡尔曼滤波器输出的状态估计值和状态误差方差分别作为扩展卡尔曼滤波器的测量值和测量噪声方差进行预测。本发明专利技术可以较快收敛，不存在较大的系统误差波动，可以抑制短时间或较长时间持续的NLOS误差和离群值。间或较长时间持续的NLOS误差和离群值。间或较长时间持续的NLOS误差和离群值。

【技术实现步骤摘要】
一种新型双卡尔曼滤波方法


[0001]本专利技术涉及滤波方法
，具体涉及一种新型双卡尔曼滤波方法。

技术介绍

[0002]1960年，R. E. Kalman发表了一篇以递归算法为基础的用于解决线性离散系统的著作，在该著作中，卡尔曼滤波摆脱了曾广泛应用的维纳滤波的缺点，自此之后，随着计算机的专利技术，计算能力的大幅提升，卡尔曼滤波器已经广泛地应用于科研、军事以及民用领域。卡尔曼滤波的优势在于，在不知道系统的确切模型的情况下，它不仅用于估计信号的当前状态，还可以用于预测信号的将来状态，所以卡尔曼滤波广泛适用于雷达和导航系统，对既定目标进行循迹。它的基本思想可以表述为，通过前一时刻的估计值以及当前时刻的观测值的条件下得到准确值以及预测下一时刻的估计值，整个过程的准则为最小均方差准则，过程中引入了信号与噪声的状态方程，建立了系统状态方程和系统测量方程。基本的卡尔曼滤波算法适用于解决线性离散系统的状态以及参数估计问题。实际应用中，卡尔曼滤波可以将一个系统的整个过程视为从前一个状态变为下一个状态、状态不断更新的过程，卡尔曼滤波对于系统的建模也是基于状态转换的。经典的卡尔曼滤波器的计算过程分为：预测状态，卡尔曼增益计算，估计输出。
[0003]经典卡尔曼滤波器的系统状态方程和观测方程均为线性方程，但是实际场景中状态方程或观测方程都可能为非线性方程，这时就要对状态方程或观测方程等非线性函数在当前估计状态的平均值附近进行线性化，在每个迭代过程执行线性化操作，并将得到的雅可比矩阵用于预测和更新卡尔曼滤波器算法的状态，这就是扩展卡尔曼滤波(Extended Kalman Filter, EKF)算法的基本思路。
[0004]雅可比矩阵的计算较为复杂，计算量较大，当系统不可微时，雅可比矩阵不存在，且在高度非线性系统中，雅可比矩阵的线性化效果不好。
[0005]为了解决非线性模型中复杂的概率密度分布问题，学者提出了无迹卡尔曼滤波(Unscented Kalman Filter, UKF)算法，该算法按照一定规律采样，计算概率密度函数的均值和方差，并按照均值和方差转换为高斯密度函数进行处理，该算法的主要思想是，选取一组最小的采样点(Sigma 点)，让它们的均值和协方差与已知的概率分布相同，每个Sigma点通过非线性系统模型计算非线性变换后输出点的均值和协方差，并计算它们的经验高斯分布，用来更新新的状态值。
[0006]无迹卡尔曼滤波算法步骤分为：预测和更新步骤。其中预测步骤包含：计算Sigma点，转换Sigma点，计算预测状态值和预测误差的协方差值。更新步骤包含：从转换的Sigma点预测测量值，求预测测量值的平均值和方差，计算互协方差值和卡尔曼增益，计算修正的状态值和状态误差方差。
[0007]现有的卡尔曼滤波算法的变体，应用较广的还有容积卡尔曼滤波(Cubature Kalman Filter, CKF)，该算法是由加拿大学者Arasaratnam和Haykin在2009年于硕士学位论文中提出，CKF应用了基于三阶球面径向容积准则，使用一组容积点来逼近具有附加高斯
噪声的非线性系统的状态均值和协方差，是理论上当前最接近贝叶斯滤波的近似算法，是解决非线性系统状态估计的强有力工具。其中，将积分形式变换成球面径向积分形式和三阶球面径向准则是最为重要的两个步骤。
[0008]除了容积卡尔曼滤波算法之外，还有一种应用较广的卡尔曼滤波算法是自适应调整协方差值的卡尔曼滤波(Adaptive Kalman Filter, AKF)算法，该算法是1969年由学者A. P. Sage和G. W. Husa提出的一种自适应滤波算法，它在进行状态估计的同时还可以通过量测输出在线实时地估计系统的噪声参数，该算法中是同时对状态误差协方差Q和噪声误差协方差R进行调整，但是也有学者提出同时对Q和R噪声参数进行调整往往是不可能的。
[0009]对于双卡尔曼滤波的结合法，清华大学的王向华在2010年提出了一种基于两次卡尔曼滤波的观测噪声自适应算法，该算法通过任一时刻两次步长不同的卡尔曼滤波结果来判断当前对R的估计精度，并实时自适应地调整R，进而获得适当的卡尔曼滤波增益K，减少因R估计不当造成的跟踪误差。
[0010]现有技术有以下缺陷：1、经典卡尔曼滤波算法、扩展卡尔曼滤波算法、无迹卡尔曼滤波算法、容积卡尔曼滤波算法无法实现Q和R的调整，但是在实际的目标跟踪过程中，由于目标远近等各种客观因素的影响，观测噪声是随时变化的，在以上滤波算法中Q和R均为恒定值，且需要依靠经验值提前设定，噪声方差不方便确定且不变可能会造成跟踪结果不理想。
[0011]2、经典卡尔曼滤波算法、扩展卡尔曼滤波算法、无迹卡尔曼滤波算法、容积卡尔曼滤波算法在测量值出现一个较大的NLOS距离误差时，会出现短期或持续一段时间的跟踪失效，定位效果差，难以应对复杂室内环境中非视距产生的较短或较长时间的时延对系统产生的较大误差影响。
[0012]3、经典卡尔曼滤波算法只适用于线性系统而不能用于非线性系统，扩展卡尔曼滤波、无迹卡尔曼滤波、容积卡尔曼滤波算法适用于非线性系统，其运算速度较经典卡尔曼滤波算法更慢，自适应卡尔曼滤波算法适用于线性系统，但是也可以经过改进从而适用于非线性系统。
[0013]4、自适应卡尔曼滤波算法可以在迭代过程中对噪声参数实时调整，但是Q和R不能同时进行调整，一般只对R值进行调整，且当测量值出现一个较大的NLOS距离误差时，自适应卡尔曼滤波器可能会发散，难以收敛。
[0014]5、两次不同步长的双卡尔曼滤波算法在每个采样时刻比较两种状态的后验估计值，根据估计值的比较来判断目标的运动状态是否发生了变化，自适应调整R，提高目标的跟踪精度。当测量值异常值时，虽然双卡尔曼滤波器可以自适应调整R，使其状态后验估计比标准卡尔曼滤波器的状态后验估计更接近真值，但其状态后验估计误差仍然很大。因此，双卡尔曼滤波方法的精度不能满足实际应用的要求。此外，当测量值中存在异常值时，由于双卡尔曼滤波方法中的一个滤波器步长较大，系统延迟会产生较大的跟踪误差，状态后验估计的收敛速度也会降低。

技术实现思路

[0015]本专利技术的目的是针对现有技术存在的不足，提供一种新型双卡尔曼滤波方法。
[0016]为实现上述目的，本专利技术提供了一种新型双卡尔曼滤波方法，包括以下步骤：
步骤1、将当前时刻经典卡尔曼滤波器的测量值与根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值进行比较，以确定NLOS误差范围；步骤2、根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的状态误差协方差和噪声误差协方差；步骤3、将调整后的状态误差协方差和噪声误差协方差输入经典卡尔曼滤波器中进行当前时刻的状态估计，以使所述经典卡尔曼滤波器输出当前时刻的状态估计值与状态变换矩阵的乘积和状态误差方差；步骤4、将经典卡尔曼滤波器输出的当前时刻的状态估计值与状态变换矩阵的乘积和状态误差方差输入至输入扩展卡尔曼滤波器中，所述扩展卡尔曼滤波器以当前时刻的状态估计值与状态变换矩阵本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种新型双卡尔曼滤波方法，其特征在于，包括：步骤1、将当前时刻经典卡尔曼滤波器的测量值与根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值进行比较，以确定NLOS误差范围；步骤2、根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的状态误差协方差和噪声误差协方差；步骤3、将调整后的状态误差协方差和噪声误差协方差输入经典卡尔曼滤波器中进行当前时刻的状态估计，以使所述经典卡尔曼滤波器输出当前时刻的状态估计值与状态变换矩阵的乘积和状态误差方差；步骤4、将经典卡尔曼滤波器输出的当前时刻的状态估计值与状态变换矩阵的乘积和状态误差方差输入至输入扩展卡尔曼滤波器中，所述扩展卡尔曼滤波器以当前时刻的状态估计值与状态变换矩阵的乘积作为测量值，且其以经典卡尔曼滤波器输出的状态误差方差作为测量噪声方差进行预测，并输出目标的位置信息。2.根据权利要求1所述的新型双卡尔曼滤波方法，其特征在于，所述步骤1具体包括：计算残差绝对值，为当前时刻经典卡尔曼滤波器的测量值，为根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值，k为大于1的自然数；根据残差绝对值的大小确定误差范围，具体如下：其中，为低误差阈值，为中误差阈值，为高误差阈值，。3.根据权利要求2所述的新型双卡尔曼滤波方法，其特征在于，所述步骤2具体包括：根据残差绝对值设置标志位flag，所述标志位flag包括0、1、2、3；根据标志位flag将经典卡尔曼滤波器的状态误差协方差和噪声误差协方差按以下方式进行调整：其中，、、分别为第一状态误差协方差调整系数、第二状态误差协方差调整参系数和第三状态误差协方差调整系数，、、分别为第一状态误差方差调整系数、第二状态误差方差调整系数、第三状态误差方差调整系数，为经典卡尔曼滤波器的状态误差协方差和噪声误差协方差的初始值。4.根据权利要求1所述的新型双卡尔曼滤波方法，其特征在...

【专利技术属性】
技术研发人员：林敏，刘倩云，
申请(专利权)人：睿迪纳南京电子科技有限公司，
类型：发明
国别省市：