CN115355910A - 一种基于二级Sigma点的UKF滤波算法 - Google Patents

Abstract

本发明公开了一种基于二级Sigma点的UKF滤波算法，属于滤波算法领域；具体为：首先，选择弹道再入目标平面模型的状态估计均值为中心点，根据状态的估计方差，进行UT变换，得到2n+1个一级Sigma点；然后，以每个一级Sigma点为中心点，分别进行UT变换，并根据状态协方差矩阵P_zz,k+1自适应调节Sigma点集的散布范围，计算得到对应的2n+1个二级Sigma点集；最后，通过Sigma点集估计效果评估算法，使用标量化后的新息表征二级Sigma点集逼近于真实观测量的程度，并选择标量化新息最小的二级Sigma点集或一级Sigma点集作为估计精度最优的Sigma点集，用于计算卡尔曼增益，更新状态估计。本发明有更好的收敛速度与稳定性，且基于Sigma点集对称分布的特性，降低了一定的计算量，不影响滤波算法的估计效果。

Description

一种基于二级Sigma点的UKF滤波算法

技术领域

本发明属于滤波算法领域，涉及一种基于二级Sigma点的UKF(Unscented KalmanFilter，无迹卡尔曼滤波)滤波算法。

背景技术

对状态进行快速准确的估计在导航定位中具有重要意义；UKF滤波算法使用Sigma点采样的方法近似非线性概率分布，其算法核心是使用UT变换来处理均值和协方差的非线性传递。许多导航定位模型具有很强的非线性关系，当强非线性系统的滤波初值偏差较大时，UKF算法收敛速度将变慢，甚至容易发散。

当系统为强非线性时，滤波算法对当前状态的估计误差更加敏感，克服当前状态的估计误差对算法产生的不良干扰的方法包括：扩大UT变换中Sigma点集的范围、更新系统当前状态的估计均值与误差协方差。

扩大Sigma点集的范围虽然能够更准确地表现真实状态，但是受到方程非线性的限制，Sigma点集的范围不能过大；二级Sigma点的UKF算法在一级Sigma点集的基础上计算二级Sigma点，从而计算多组状态估计的均值与误差协方差，经筛选能够在不增大Sigma点集的基础上，提高算法的估计速度与稳定性。

弹道再入目标平面模型为典型的非线性模型，以弹道再入目标平面模型为例，由于其受初始状态估计偏差的影响，标准UKF算法收敛速度慢易发散。

发明内容

针对系统模型为强非线性且滤波初值偏差较大时，UKF算法收敛速度慢易发散的问题，本发明提供了一种基于二级Sigma点的UKF滤波算法，以弹道再入目标平面模型为例，实现对强非线性系统的最优估计，具有收敛速度快、稳定性好的优点。

所述基于二级Sigma点的UKF滤波算法，通过以下步骤实现：

首先，选择弹道再入目标平面模型的状态估计均值为中心点，根据状态的估计方差，进行UT变换，得到(2n+1)个一级Sigma点；n为状态变量的维数；

然后，以每个一级Sigma点为中心点，分别进行UT变换，并根据方差P_zz,k+1自适应调节Sigma点集的散布范围，计算得到多组二级Sigma点集；

最后，使用标量化后的新息表征二级Sigma点集逼近于真实观测量的程度，并选择标量化新息最小的二级Sigma点集或一级Sigma点集作为估计精度最优的Sigma点集，用于计算卡尔曼增益，更新状态估计。

进一步，本发明还设计了二级Sigma点的UKF滤波算法的简化计算方法，利用Sigma点的对称分布性，减少计算二级Sigma点集的数量，进一步减少算法的计算量。

本发明的优点和积极效果在于：

(1)本发明一种基于二级Sigma点的UKF滤波算法，简单易行，用于强非线性系统，在滤波初值不准的情况下，基于二级Sigma点的UKF滤波算法较标准UKF算法，有更好的收敛速度与稳定性。

(2)本发明一种基于二级Sigma点的UKF滤波算法的简化计算方法，考虑到计算量的影响，基于Sigma点集对称分布的特性，能够降低一定的计算量，且不影响滤波算法的估计效果。

附图说明

图1是本发明构建二级Sigma点的UKF滤波算法的原理图；

图2是本发明一种基于二级Sigma点的UKF滤波算法的流程图；

图3是本发明基于二级Sigma点的UKF滤波算法与标准UKF滤波算法的滤波结果对比图。

具体实施方式

下面结合附图和实施例来说明本发明的技术方案。

本发明一种基于二级Sigma点的UKF滤波算法，包括二级Sigma点集计算算法、新息标量化算法、Sigma点集估计效果评估算法、简化计算的Sigma点估计效果评估算法以及基于二级Sigma点集的后验估计算法。

如图1所示，以二维状态变量为例，所绘制的基于二级Sigma点的UKF滤波算法的基本原理是：通过计算二级Sigma点集，能够将UT变换的中心点移动到靠近真实值的位置，从而提高滤波算法的收敛速度与稳定性。

具体为：对于非线性程度强的系统，通过二级Sigma点集计算算法，在一级Sigma点的基础上计算二级Sigma点集。通过新息标量化算法，将观测新息转化为标量，便于直观分析。通过Sigma点集估计效果评估算法，根据二级Sigma点集逼近于真实观测量的程度，选取最优二级Sigma点集，用于后续卡尔曼滤波算法中状态的先验估计，从而提高了非线性传递函数的转化精度。通过基于二级Sigma点集的后验估计算法更新滤波算法的状态估计。

此外，通过简化计算的Sigma点估计效果评估算法，对Sigma点对称分布这一特性加以利用，能够减少计算二级Sigma点的数量，从而减少基于二级Sigma点的UKF滤波算法的计算量；

整个滤波算法应用于强非线性系统，能够将标准UKF滤波算法原本偏离的状态估计收敛到真值。尤其在初值不准确的情况下，该基于二级Sigma点的UKF滤波算法较标准UKF有明显的改进。

本发明基于二级Sigma点的UKF滤波算法，流程图如图2所示，具体内容如下：

步骤一：设置滤波的当前状态，选择弹道再入目标平面模型的当前状态估计为中心点，得到Sigma点集为一级Sigma点集；

具体过程为：

以弹道再入目标平面模型的状态先验估计

作为中心点，根据方差P_k+1,k，计算一级Sigma点集：

式(1)-(4)中，i＝1,2,...,n，n为状态先验估计

的维数，α、κ为常数，Sigma点

到中心点

的距离与

相关，

表示矩阵

的第i列。

步骤二：通过二级Sigma点计算算法，以一级Sigma点集内的样本点作为中心点，进行UT变换，得到2n+1个二级Sigma点集，n为状态的维数；

具体为：

以一级Sigma样本点

g＝0,1,2,....,2n为UT变换的中心点，计算二级Sigma点集

计算式表示如式(5)-(8)所示：

式(5)-(8)中，i＝1,2,...,n；k_j表示尺度因子缩放系数，用于调节二级Sigma点

的散布范围；若状态变量取值在二级Sigma点集

附近对抖动较为敏感，则减小尺度因子缩放系数k_j，减小二级Sigma点

的散布范围，反之则增加尺度因子缩放系数k_j，增加二级Sigma点

的散布范围。因此，尺度因子缩放系数k_j能够根据模型的非线性程度自适应缩放Sigma点的散布范围；Sigma点的散布范围越小，UT变换的精度越高。

使用方差P_zz,k+1对角线上的元素代表状态变量在Sigma点集附近对抖动的敏感程度，当方差P_zz,k+1(j,j)较大时，表示j维状态变量在Sigma点集附近对抖动的敏感程度变强，当方差P_zz,k+1(j,j)较小时，表示j维状态变量在Sigma点集附近对抖动的敏感程度变弱。

因此设定尺度因子缩放系数k_j的调节率，如式(9)

式(9)中，P₀和P₁分别表示误差协方差P_zz,k+1的阈值，为防止尺度因子缩放系数k_j过大或过小，k_j0是尺度因子的上界，k_j1是尺度因子的下界。

步骤三：通过新息标量化算法，计算2n+1个二级Sigma点集，以及每个二级Sigma点集的转换为标量的观测变量的新息。

具体为：

根据观测信息Z_k+1计算二级Sigma点集

的观测新息

见式(10)-(12)。

为滤波模型的观测方程，

为经

转换后的状态估计。

为计算均值

的权重，满足：

λ＝α²(n+κ)-n (14)

使用

表示单位为i的状态变量的观测新息

的绝对值，对单位相同的观测变量的观测新息的绝对值求平均，对单位不同的观测变量的观测新息的绝对值求积，得到一个能够统一全部量纲的标量化的观测新息

例如本实例中观测的信息为雷达测量的角度与模长，新息标量化算法计算角度与模长的乘积，得到单位为长度的标量化的观测新息

步骤四：通过Sigma点集估计效果评估算法，根据标量化的观测新息，选择出估计精度最好的Sigma点集，用于后续卡尔曼滤波算法中状态的先验估计的均值与方差。

选取最优Sigma点的原理如式(16)所示，根据标量化的观测新息

g＝0,1,2,....,2n，选取二级Sigma点集

的中心点

作为状态的先验估计，选取二级Sigma点集

的观测新息

作为观测新息。

步骤五：基于二级Sigma点集的后验估计算法，计算新息与卡尔曼增益，对先验估计进行调节，输出滤波结果。

根据式(17)-(21)计算UKF滤波算法的卡尔曼增益K_k+1，并对状态估计进行更新，得到

W_c ^(j)为计算协方差P_xz,k+1、P_zz,k+1的权重，R_k+1为测量噪声协方差矩阵。

另外，本发明基于简化计算的Sigma点估计效果评估算法，根据一级Sigma点扩展的方向进行分类，将扩展方向相同或相反的一级Sigma点集分为同一类，共分成n类，其中n表示系统的状态的维数，每类一级Sigma点表示为：

式(22)中，i表示状态变量的第i维，

i＝1,2,...,n (24)

根据采集到的观测信息，计算

中三个一级Sigma点距离系统真实状态的程度，选择最接近真值的一级Sigma点

为中心点：

计算出对应的一级Sigma点集

a_i＝0,1,2经UT变换后的n个二级Sigma点集

m＝0,1,2,...,n (30)

根据式(24)选取

中三个Sigma点距离系统真实状态的程度选择其中的某一个Sigma点，忽略了UT变换中均值的阶数大于1的项，因此丧失了一定的判断Sigma点距离系统真实状态的程度的准确性，但是在一定程度上，依然能够选择比一级Sigma点效果更好的二级Sigma点，对较偏的初值起到矫正作用。

实施例中：

采用弹道再入目标模型，分别使用标准UKF与基于二级Sigma点的UKF算法，滤波结果如图3所示。本发明应用于强非线性系统，能够将标准UKF偏离的状态估计收敛到真实状态。尤其在初值不准确的情况下，该基于二级Sigma点的UKF滤波算法较标准UKF有明显的改进。

Claims (6)

1.一种基于二级Sigma点的UKF滤波算法，其特征在于，具体步骤为：

首先，选择弹道再入目标平面模型的状态估计均值为中心点，根据状态的估计方差，进行UT变换，得到2n+1个一级Sigma点，n为状态变量的维数；

然后，以每个一级Sigma点为中心点，分别进行UT变换，并根据方差P_zz,k+1自适应调节Sigma点集的散布范围，计算得到对应的2n+1个二级Sigma点集；

最后，通过Sigma点集估计效果评估算法，使用标量化后的新息表征二级Sigma点集逼近于真实观测量的程度，并选择标量化新息最小的二级Sigma点集或一级Sigma点集作为估计精度最优的Sigma点集，用于计算卡尔曼增益，更新状态估计。

2.根据权利要求1所述的一种基于二级Sigma点的UKF滤波算法，其特征在于，所述Sigma点集估计效果评估算法，替换为如下简化计算的Sigma点估计效果评估算法，利用Sigma点的对称分布性进行分类，减少二级Sigma点集的数量，具体为：

首先，根据一级Sigma点扩展的方向进行分类，将扩展方向相同或相反的一级Sigma点集分为同一类，共分成n类，其中n表示系统的状态的维数；

每类一级Sigma点表示为：

i表示状态变量的第i维；

然后，根据采集到的观测信息，计算三类一级Sigma点的中心点距离系统真实状态的程度，选择最接近真值的一级Sigma点为中心点，经UT变换后扩展为n个二级Sigma点集，计算n个二级Sigma点集转化为标量后的观测变量的新息；根据转化为标量后的观测变量的新息，自适应选择估计精度最好的Sigma点集。

3.根据权利要求1所述的一种基于二级Sigma点的UKF滤波算法，其特征在于，所述对每个一级Sigma点为中心点，进行UT变换，得到多组二级Sigma点集的具体过程为：

首先，以状态先验估计

作为中心点，根据方差P_k+1,k，计算一级Sigma点集：

i＝1,2,...,n，n为状态先验估计

的维数，α、κ为常数，Sigma点

到中心点

的距离与

相关，

表示矩阵

的第i列；

然后，以一级Sigma样本点

为UT变换的中心点，计算二级Sigma点集

计算式表示如下所示：

k_j表示尺度因子缩放系数，用于调节二级Sigma点

的散布范围。

4.根据权利要求3所述的一种基于二级Sigma点的UKF滤波算法，其特征在于，所述尺度因子缩放系数k_j的调节率，如下：

P₀和P₁分别表示误差协方差P_zz,k+1的阈值，为防止尺度因子缩放系数k_j过大或过小，k_j0是尺度因子的上界，k_j1是尺度因子的下界。

5.根据权利要求1所述的一种基于二级Sigma点的UKF滤波算法，其特征在于，对所述2n+1个二级Sigma点集转换为标量化的观测变量的新息，具体为：

根据观测信息Z_k+1计算二级Sigma点集

的观测新息

如下：

为滤波模型的观测方程；

为经

转换后的状态估计；

为计算均值

的权重。

6.根据权利要求1所述的一种基于二级Sigma点的UKF滤波算法，其特征在于，对所述估计精度最优的Sigma点集，计算卡尔曼增益，更新状态估计，具体为：

首先，计算UKF滤波算法的卡尔曼增益K_k+1，并对状态估计进行更新，得到

为计算协方差P_xz,k+1和P_zz,k+1的权重，R_k+1为测量噪声协方差矩阵。

Images