一种具有自适应算力调节与发散检测功能的复合粒子滤波方法

1.本发明属于非线性系统状态估计领域，具体涉及一种具有自适应算力调节与发散检测功能的复合粒子滤波方法，适用于同时存在非高斯噪声和外部干扰的非线性系统。


背景技术：

2.非线性系统的状态估计是一个复杂的问题。当状态服从高斯分布时，采用卡尔曼滤波类型的滤波器可以获得较好的结果。但是，当状态分布表现出多模态或者重尾等特性或者系统表现出强非线性时，卡尔曼型滤波器便不再适用。粒子滤波方法是基于统计学的估计方法，用大量的数值计算实现贝叶斯滤波器，可以很好地处理系统的强非线性。
3.此外，实际系统遇到的很多干扰都可以被看作是系统的外部线性干扰，此时，可以对状态进行增广，将线性的干扰状态包含进由滤波器估计的状态。但是，粒子滤波所需的粒子数随状态的维数指数级增加，单纯增加粒子滤波器的维数会大大提高粒子滤波的计算量。因此，在准确对存在外部干扰的非线性系统进行状态估计的同时，兼顾滤波器的计算量，成为一个亟需解决的问题。
4.中国发明专利申请cn201811165547.1中提出了一种基于库尔贝克-莱布勒散度采样改进的高斯粒子滤波算法，但存在两个问题：(1)该文并没有考虑系统存在非高斯型噪声时的情况，在应用场景中会受到一定的限制；(2)每个时刻需要的粒子数与当前采样粒子关联性较大，算法的随机性较强。


技术实现要素：

5.为克服现有技术的不足，本发明提供一种具有自适应算力调节与发散检测功能的复合粒子滤波方法，利用该方法能够准确并高效地估计同时存在非高斯噪声和外部干扰的非线性系统状态。
6.为达到上述目的，本发明采用的技术方案为：
7.一种具有自适应算力调节与发散检测功能的复合粒子滤波方法，针对同时存在加性时变干扰和非高斯噪声的非线性系统状态估计问题，对状态进行增广，将加性时变干扰增广到非线性的状态中，采用复合分层滤波器同时估计干扰状态和非线性系统状态。其实现步骤如下：
8.第一步，建立包含加性时变干扰和非高斯噪声的非线性系统增广状态空间模型；
9.第二步，设计含有干扰补偿采样环节的复合粒子滤波算法，克服加性时变干扰对粒子滤波性能的影响，并基于库尔贝克-莱布勒散度计算重采样所需的最优粒子数，并根据重采样结果输出状态估计值和干扰估计值；
10.第三步，构造基于扩展卡尔曼滤波的发散检测单元，在检测到复合粒子滤波的估计误差发散时重置粒子滤波器。
11.进一步地，所述第一步中，所述包含加性时变干扰和非高斯噪声的非线性系统增
广状态空间模型如下：
12.x
k 1
＝f(xk) gdk wk13.yk＝h(xk) vk14.其中，xk是系统状态，f(
·
)是非线性系统模型，g是干扰系数矩阵，h(
·
)是非线性测量模型，wk是服从高斯分布的过程噪声wk～n(wk；0,qk)，qk为过程噪声方差，vk是服从非高斯分布p
vk
的测量噪声dk表示外部干扰，满足：
15.s
k 1
＝wsk ξk16.dk＝vsk17.其中，sk为干扰子系统的状态变量，为干扰子系统的状态矩阵，ω为干扰频率，v是干扰子系统的输出矩阵。ξk为干扰子系统的过程噪声，满足ξk～n(ξk；0,sk)。sk为干扰子系统过程噪声的方差。对状态空间进行扩维，可以写成：
18.l
k 1
＝q(lk) mγk19.yk＝r(lk) vk20.其中，扩维的状态r(lk)＝h(xk)，i是单位矩阵。
21.进一步地，所述第二步的具体实现如下：
22.对于采样时刻k＝1,...,k，根据库尔贝克-莱布勒散度采样，更新k时刻所需的粒子数nk：
[0023][0024]
其中，ε表示真实后验与基于采样的近似之间误差的最大临界值；z
1-δ
基于给定的误差参数δ，表示标准正态分布的上1-δ分位点；m(x
k-1
)表示精度因子，取值与状态值x
k-1
有关；
[0025]
然后，根据粒子权重，重采样得到新的粒子集计算其均值和方差，均值即为估计值，方差为估计误差的协方差；
[0026]
那么，k时刻系统状态的估计值xk为：
[0027][0028]
其中，为重采样粒子集合中的第i个粒子的系统状态值；
[0029]
根据系统方程和过程噪声统计特性对重采样粒子集进行更新，得到粒子集
其中第i个粒子的系统状态值为：
[0030][0031]
其中，是对高斯分布的过程噪声wk～n(wk；0,qk)进行采样而产生的，qk表示过程噪声方差；f(
·
)是非线性系统模型，g是干扰系数矩阵，v是干扰子系统的输出矩阵；
[0032]
采用干扰观测器进行干扰的状态估计；
[0033]
第i个粒子的干扰子系统状态及其估计误差协方差为：
[0034][0035][0036]
其中：
[0037][0038][0039]
其中，i＝1,...,nk；
[0040]
其中，为干扰子系统的状态矩阵，ω为干扰频率；此时的更新的粒子集χ
k 1|k
表示为
[0041]
进一步地，所述第三步的具体实现如下：
[0042]
对于采样时刻k＝1,...,k，计算系统预测的测量值和实际测量值之间的距离指标dk：
[0043]dk
＝[y
k-h(xk)]
t
r-1
[y
k-h(xk)]
[0044]
其中，h(
·
)是非线性测量模型，yk为k时刻的测量值。
[0045]
判断滤波器是否发散：
[0046][0047]
其中，d
max
为滤波器发散的检测阈值；
[0048]
如果滤波器发散，则用扩展卡尔曼滤波)的估计值重置粒子集的初值，即：
[0049]
重置系统状态
[0050][0051]
其中，x
k,ekf
为ekf估计的系统状态。
[0052]
重置干扰子系统的状态和估计误差协方差：
[0053][0054]
其中，i＝1,...,nk；
[0055]
其中，为ekf估计的干扰子系统状态，为ekf估计的估计误差协方差；得到重置的粒子集：
[0056][0057]
本发明与现有技术相比的优点在于：
[0058]
本发明中涉及的一种具有自适应算力调节与发散检测功能的复合粒子滤波方法，针对同时存在加性时变干扰和非高斯噪声的非线性系统状态估计问题，首先，通过对状态进行增广，将加性时变干扰增广到非线性的状态中，设计了一种复合粒子滤波方法，通过干扰观测器估计干扰子系统状态，通过粒子滤波估计非线性系统状态，实现了干扰补偿和非线性状态估计。此外，通过粒子数自适应调节，降低了传统粒子滤波算法的计算量，保证了状态估计的实时性。最后，构造了基于扩展卡尔曼滤波的发散检测单元，在检测到复合粒子滤波的估计误差发散时重置粒子滤波器，实现对估计结果的修正，保证了滤波结果的可靠性。
附图说明
[0059]
图1为本发明的一种具有自适应算力调节与发散检测功能的复合粒子滤波方法的设计流程图。
具体实施方式
[0060]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0061]
如图1所示，本发明的一种具有自适应算力调节与发散检测功能的复合粒子滤波方法具体实施步骤如下：
[0062]
步骤1、建立包含加性时变干扰和非高斯噪声的非线性系统增广状态空间模型：
[0063]
所述包含加性时变干扰和非高斯噪声的非线性系统增广状态空间模型如下：
[0064]
x
k 1
＝f(xk) gdk wk[0065]
yk＝h(xk) vk[0066]
其中，xk是系统状态，f(
·
)是非线性系统模型，g是干扰系数矩阵，h(
·
)是非线性测量模型，wk是服从高斯分布的过程噪声wk～n(wk；0,qk)，qk为过程噪声方差，vk是服从非高斯分布的测量噪声dk表示外部干扰，满足：
[0067]sk 1
＝wsk ξk[0068]dk
＝vsk[0069]
其中，sk为干扰子系统的状态变量，为干扰子系统的状态矩阵，ω为干扰频率，v是干扰子系统的输出矩阵。ξk为干扰子系统的过程噪声，满足ξk～n(ξk；0,sk)。sk为干扰子系统过程噪声的方差。对状态空间进行扩维，可以写成：
[0070]
l
k 1
＝q(lk) mγk[0071]
yk＝r(lk) vk[0072]
其中，扩维的状态r(lk)＝h(xk)，i是单位矩阵。
[0073]
步骤2、设计含有干扰补偿采样环节的复合粒子滤波算法，克服加性时变干扰对粒子滤波性能的影响，并基于库尔贝克-莱布勒散度计算重采样所需的最优粒子数，根据重采样结果输出状态估计值和干扰估计值；然后，构造基于扩展卡尔曼滤波的发散检测单元，在检测到复合粒子滤波的估计误差发散时重置粒子滤波器：
[0074]
首先，初始化滤波器。初始化系统状态，基于初始状态x0的概率密度函数随机产生n0个初始粒子。其中，第i个初始粒子的系统状态值为：
[0075][0076]
其中，x0为系统的初始状态，n0为初始粒子数。
[0077]
初始化干扰子系统的状态，第i个初始粒子的干扰子系统状态及其估计误差协方差为：
[0078][0079]
其中，为干扰子系统的初始状态，为干扰子系统的估计误差协方差。那么，可以得到初始粒子集
[0080]
然后，对于采样时刻k＝1,...,k，循环执行如下步骤：
[0081]
1)计算粒子权重并进行归一化：
[0082]
根据测量值、测量方程和测量噪声，计算粒子集中第i个粒子的权重
[0083][0084]
其中，yk为k时刻的测量值。测量噪声vk服从偏正态分布sn(0,r,α)，其概率密度函数为2φ(vk；0,r)φ(α
t
vk)。其中，φ和φ是正态分布的概率密度函数和累积密度函数，α是形状参数。
[0085]
然后，归一化第i个粒子的权重
[0086][0087]
此时，粒子集合χ
k|k-1
可以表示为：
[0088]
[0089]
2)更新滤波器所需的粒子数并进行重采样：
[0090]
根据库尔贝克-莱布勒散度采样，更新k时刻所需的粒子数nk：
[0091][0092]
其中，ε表示真实后验与基于采样的近似之间误差的最大临界值，一般取0.05；z
1-δ
基于给定的误差参数δ，表示标准正态分布的上1-δ分位点，1-δ一般取0.99；m(x
k-1
)表示精度因子，取值与状态值x
k-1
有关。然后，根据粒子权重，重采样得到新的粒子集可以计算其均值和方差，均值即为估计值，方差为估计误差的协方差。
[0093]
那么，k时刻系统状态的估计值xk为：
[0094][0095]
其中，为重采样粒子集合中的第i个粒子的系统状态值。
[0096]
3)根据系统方程更新粒子集：
[0097]
根据系统方程和过程噪声统计特性对重采样粒子集进行更新，得到粒子集其中第i个粒子的系统状态值为：
[0098][0099]
其中，是对高斯分布的过程噪声wk～n(wk；0,qk)进行采样而产生的，qk表示过程噪声方差。f(
·
)是非线性系统模型，g是干扰系数矩阵，v是干扰子系统的输出矩阵。
[0100]
4)干扰的状态估计：
[0101]
采用干扰观测器进行干扰的状态估计。第i个粒子的干扰子系统状态及其估计误差协方差为：
[0102][0103][0104]
其中：
[0105][0106][0107]
此时的更新的粒子集χ
k 1|k
可以表示为：
[0108][0109]
第三步、构造基于扩展卡尔曼滤波的发散检测单元，在检测到复合粒子滤波的估
计误差发散时重置粒子滤波器：
[0110]
计算系统预测的测量值和实际测量值之间的距离指标dk：
[0111]dk
＝[y
k-h(xk)]
t
r-1
[y
k-h(xk)]
[0112]
判断滤波器是否发散：
[0113][0114]
其中，d
max
为滤波器发散的检测阈值。如果滤波器发散，则用ekf的估计值重置粒子集的初值，即：
[0115]
重置系统状态
[0116][0117]
其中，x
k,ekf
为ekf估计的系统状态。
[0118]
重置干扰子系统的状态和估计误差协方差
[0119][0120]
其中，为ekf估计的干扰子系统状态，为ekf估计的估计误差协方差。那么，可以得到重置的粒子集：
[0121][0122]
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
[0123]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。