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

一种基于二级sigma点的ukf滤波算法
技术领域
1.本发明属于滤波算法领域，涉及一种基于二级sigma点的ukf(unscented kalman filter，无迹卡尔曼滤波)滤波算法。


背景技术：

2.对状态进行快速准确的估计在导航定位中具有重要意义；ukf滤波算法使用sigma点采样的方法近似非线性概率分布，其算法核心是使用ut变换来处理均值和协方差的非线性传递。许多导航定位模型具有很强的非线性关系，当强非线性系统的滤波初值偏差较大时，ukf算法收敛速度将变慢，甚至容易发散。
3.当系统为强非线性时，滤波算法对当前状态的估计误差更加敏感，克服当前状态的估计误差对算法产生的不良干扰的方法包括：扩大ut变换中sigma点集的范围、更新系统当前状态的估计均值与误差协方差。
4.扩大sigma点集的范围虽然能够更准确地表现真实状态，但是受到方程非线性的限制，sigma点集的范围不能过大；二级sigma点的ukf算法在一级sigma点集的基础上计算二级sigma点，从而计算多组状态估计的均值与误差协方差，经筛选能够在不增大sigma点集的基础上，提高算法的估计速度与稳定性。
5.弹道再入目标平面模型为典型的非线性模型，以弹道再入目标平面模型为例，由于其受初始状态估计偏差的影响，标准ukf算法收敛速度慢易发散。


技术实现要素：

6.针对系统模型为强非线性且滤波初值偏差较大时，ukf算法收敛速度慢易发散的问题，本发明提供了一种基于二级sigma点的ukf滤波算法，以弹道再入目标平面模型为例，实现对强非线性系统的最优估计，具有收敛速度快、稳定性好的优点。
7.所述基于二级sigma点的ukf滤波算法，通过以下步骤实现：
8.首先，选择弹道再入目标平面模型的状态估计均值为中心点，根据状态的估计方差，进行ut变换，得到(2n+1)个一级sigma点；n为状态变量的维数；
9.然后，以每个一级sigma点为中心点，分别进行ut变换，并根据方差p
zz,k+1
自适应调节sigma点集的散布范围，计算得到多组二级sigma点集；
10.最后，使用标量化后的新息表征二级sigma点集逼近于真实观测量的程度，并选择标量化新息最小的二级sigma点集或一级sigma点集作为估计精度最优的sigma点集，用于计算卡尔曼增益，更新状态估计。
11.进一步，本发明还设计了二级sigma点的ukf滤波算法的简化计算方法，利用sigma点的对称分布性，减少计算二级sigma点集的数量，进一步减少算法的计算量。
12.本发明的优点和积极效果在于：
13.(1)本发明一种基于二级sigma点的ukf滤波算法，简单易行，用于强非线性系统，在滤波初值不准的情况下，基于二级sigma点的ukf滤波算法较标准ukf算法，有更好的收敛
速度与稳定性。
14.(2)本发明一种基于二级sigma点的ukf滤波算法的简化计算方法，考虑到计算量的影响，基于sigma点集对称分布的特性，能够降低一定的计算量，且不影响滤波算法的估计效果。
附图说明
15.图1是本发明构建二级sigma点的ukf滤波算法的原理图；
16.图2是本发明一种基于二级sigma点的ukf滤波算法的流程图；
17.图3是本发明基于二级sigma点的ukf滤波算法与标准ukf滤波算法的滤波结果对比图。
具体实施方式
18.下面结合附图和实施例来说明本发明的技术方案。
19.本发明一种基于二级sigma点的ukf滤波算法，包括二级sigma点集计算算法、新息标量化算法、sigma点集估计效果评估算法、简化计算的sigma点估计效果评估算法以及基于二级sigma点集的后验估计算法。
20.如图1所示，以二维状态变量为例，所绘制的基于二级sigma点的ukf滤波算法的基本原理是：通过计算二级sigma点集，能够将ut变换的中心点移动到靠近真实值的位置，从而提高滤波算法的收敛速度与稳定性。
21.具体为：对于非线性程度强的系统，通过二级sigma点集计算算法，在一级sigma点的基础上计算二级sigma点集。通过新息标量化算法，将观测新息转化为标量，便于直观分析。通过sigma点集估计效果评估算法，根据二级sigma点集逼近于真实观测量的程度，选取最优二级sigma点集，用于后续卡尔曼滤波算法中状态的先验估计，从而提高了非线性传递函数的转化精度。通过基于二级sigma点集的后验估计算法更新滤波算法的状态估计。
22.此外，通过简化计算的sigma点估计效果评估算法，对sigma点对称分布这一特性加以利用，能够减少计算二级sigma点的数量，从而减少基于二级sigma点的ukf滤波算法的计算量；
23.整个滤波算法应用于强非线性系统，能够将标准ukf滤波算法原本偏离的状态估计收敛到真值。尤其在初值不准确的情况下，该基于二级sigma点的ukf滤波算法较标准ukf有明显的改进。
24.本发明基于二级sigma点的ukf滤波算法，流程图如图2所示，具体内容如下：
25.步骤一：设置滤波的当前状态，选择弹道再入目标平面模型的当前状态估计为中心点，得到sigma点集为一级sigma点集；
26.具体过程为：
27.以弹道再入目标平面模型的状态先验估计作为中心点，根据方差p
k+1,k
，计算一级sigma点集：
28.[0029][0030][0031][0032]
式(1)-(4)中，i＝1,2,...,n，n为状态先验估计的维数，α、κ为常数，sigma点到中心点的距离与相关，表示矩阵的第i列。
[0033]
步骤二：通过二级sigma点计算算法，以一级sigma点集内的样本点作为中心点，进行ut变换，得到2n+1个二级sigma点集，n为状态的维数；
[0034]
具体为：
[0035]
以一级sigma样本点g＝0,1,2,....,2n为ut变换的中心点，计算二级sigma点集计算式表示如式(5)-(8)所示：
[0036][0037][0038][0039][0040]
式(5)-(8)中，i＝1,2,...,n；kj表示尺度因子缩放系数，用于调节二级sigma点的散布范围；若状态变量取值在二级sigma点集附近对抖动较为敏感，则减小尺度因子缩放系数kj，减小二级sigma点的散布范围，反之则增加尺度因子缩放系数kj，增加二级sigma点的散布范围。因此，尺度因子缩放系数kj能够根据模型的非线性程度自适应缩放sigma点的散布范围；sigma点的散布范围越小，ut变换的精度越高。
[0041]
使用方差p
zz,k+1
对角线上的元素代表状态变量在sigma点集附近对抖动的敏感程度，当方差p
zz,k+1
(j,j)较大时，表示j维状态变量在sigma点集附近对抖动的敏感程度变强，当方差p
zz,k+1
(j,j)较小时，表示j维状态变量在sigma点集附近对抖动的敏感程度变弱。
[0042]
因此设定尺度因子缩放系数kj的调节率，如式(9)
[0043][0044]
式(9)中，p0和p1分别表示误差协方差p
zz,k+1
的阈值，为防止尺度因子缩放系数kj过
大或过小，k
j0
是尺度因子的上界，k
j1
是尺度因子的下界。
[0045]
步骤三：通过新息标量化算法，计算2n+1个二级sigma点集，以及每个二级sigma点集的转换为标量的观测变量的新息。
[0046]
具体为：
[0047]
根据观测信息z
k+1
计算二级sigma点集的观测新息见式(10)-(12)。
[0048][0049][0050][0051]
为滤波模型的观测方程，为经转换后的状态估计。为计算均值的权重，满足：
[0052][0053]
λ＝α2(n+κ)-n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0054]
使用表示单位为i的状态变量的观测新息的绝对值，对单位相同的观测变量的观测新息的绝对值求平均，对单位不同的观测变量的观测新息的绝对值求积，得到一个能够统一全部量纲的标量化的观测新息
[0055][0056]
例如本实例中观测的信息为雷达测量的角度与模长，新息标量化算法计算角度与模长的乘积，得到单位为长度的标量化的观测新息
[0057]
步骤四：通过sigma点集估计效果评估算法，根据标量化的观测新息，选择出估计精度最好的sigma点集，用于后续卡尔曼滤波算法中状态的先验估计的均值与方差。
[0058]
选取最优sigma点的原理如式(16)所示，根据标量化的观测新息g＝0,1,2,....,2n，选取二级sigma点集的中心点作为状态的先验估计，选取二级sigma点集的观测新息作为观测新息。
[0059][0060]
步骤五：基于二级sigma点集的后验估计算法，计算新息与卡尔曼增益，对先验估计进行调节，输出滤波结果。
[0061]
根据式(17)-(21)计算ukf滤波算法的卡尔曼增益k
k+1
，并对状态估计进行更新，得到
[0062][0063][0064][0065][0066][0067]wc(j)
为计算协方差p
xz,k+1
、p
zz,k+1
的权重，r
k+1
为测量噪声协方差矩阵。
[0068][0069]
另外，本发明基于简化计算的sigma点估计效果评估算法，根据一级sigma点扩展的方向进行分类，将扩展方向相同或相反的一级sigma点集分为同一类，共分成n类，其中n表示系统的状态的维数，每类一级sigma点表示为：
[0070][0071]
式(22)中，i表示状态变量的第i维，
[0072][0073][0074][0075]
i＝1,2,...,n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)
[0076]
根据采集到的观测信息，计算中三个一级sigma点距离系统真实状态的程度，选择最接近真值的一级sigma点为中心点：
[0077][0078]
计算出对应的一级sigma点集ai＝0,1,2经ut变换后的n个
二级sigma点集
[0079][0080][0081][0082][0083]
m＝0,1,2,...,n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(30)
[0084]
根据式(24)选取中三个sigma点距离系统真实状态的程度选择其中的某一个sigma点，忽略了ut变换中均值的阶数大于1的项，因此丧失了一定的判断sigma点距离系统真实状态的程度的准确性，但是在一定程度上，依然能够选择比一级sigma点效果更好的二级sigma点，对较偏的初值起到矫正作用。
[0085]
实施例中：
[0086]
采用弹道再入目标模型，分别使用标准ukf与基于二级sigma点的ukf算法，滤波结果如图3所示。本发明应用于强非线性系统，能够将标准ukf偏离的状态估计收敛到真实状态。尤其在初值不准确的情况下，该基于二级sigma点的ukf滤波算法较标准ukf有明显的改进。