一种基于Consensus-ADMM的带估计约束的非线性集值滤波方法与流程

一种基于consensus-admm的带估计约束的非线性集值滤波方法
技术领域
1.本发明涉及滤波器技术领域，具体为一种基于consensus-admm的带估计约束的非线性集值滤波方法。


背景技术：

2.集值滤波方法被广泛应用于目标追踪、导航和信号处理等领域，能够在动态系统的状态扰动和观测噪声统计性质未知但有界时，获得系统状态的定界估计。在复杂的实际应用场景中，动态系统通常是非线性的，并且在进行状态估计时，会期望状态的估计值落在某些特定的子空间中。事实上，由于集值滤波给出的估计一般是一个规则的状态定界区域，而非线性系统会将一个规则的集合映射到一个不规则的集合，因此将集值滤波方法扩展到非线性动态系统是很困难的，特别是当需要保证在线使用时。此外，不同于贝叶斯滤波方法，即使滤波过程中使用了状态的约束信息，集值滤波方法给出的状态估计值一般也不会满足约束，因此如果期望集值滤波给出的状态估计值落在某些子空间时，需要考虑估计约束。技术难点在于动态系统中的非线性函数难以处理，集值滤波的估计值难以满足估计约束条件，直接影响滤波性能，难以满足状态估计等方面的迫切需求。
3.非线性集值滤波方法的关键是对动态系统中非线性函数的处理，目前非线性集值滤波方法主要通过将非线性函数在当前的估计值点进行线性化，然后使用不同的方法对高阶余项进行定界，并将余项定界作为过程或观测噪声信息的补充，最后可以使用线性系统的集值滤波进行状态估计。然而对于一般的非线性函数，线性化的近似效果可能极差，这就导致了要用更大的定界区域来对高阶余项进行定界，状态估计的效果也会受到影响。因此王治国等人提出了一种对偶集值滤波方法[文献1：zhiguo wang,xiaojing shen,haiqi liu,fanqin meng,and yunmin zhu.dual set membership filter with minimizing nonlinear transformation of ellipsoid.ieee transactions on automatic control,pages 1-1,2021]，此方法不需要线性化动态系统中的非线性函数，而是通过半无限规划将非线性系统转化为线性系统，以此获得更紧的状态定界椭球。但是，此方法只考虑了无估计约束的情况，在许多实际应用场景中，例如盲多用户检测、车辆导航与定位等场景中，状态估计通常需要满足特定的约束条件。因此，考虑具有各种估计约束的集值滤波方法是非常必要的。


技术实现要素：

[0004]
针对上述问题，本发明的目的在于提供一种基于consensus-admm的带估计约束的非线性集值滤波方法，能够通过求解半无限规划问题将非线性动态系统转化为线性系统，获得更紧的状态定界椭球，并且椭球的中心点满足估计约束。技术方案如下：
[0005]
一种基于consensus-admm的带估计约束的非线性集值滤波方法，包括以下步骤：
[0006]
步骤1：输入k＝0时刻的状态定界椭球作为迭代初始值；
[0007]
对于时刻k＝0到k-1，执行以下步骤：
[0008]
步骤2：根据k时刻的状态定界椭球、状态转移方程、过程噪声信息和k 1时刻的估计约束计算k 1时刻的状态预报椭球；详细步骤如下：
[0009]
步骤2.1：所述k时刻的状态定界椭球表示为：表示k时刻状态定界椭球的中心点，同时表示k时刻的状态估计值，pk表示k时刻状态定界椭球的形状矩阵；
[0010]
所述k时刻的状态转移方程表示为：x
k 1
＝fk(xk) wk；fk(
·
)表示非线性一致连续可微的状态转移函数，wk表示过程噪声；xk和x
k 1
分别表示k时刻和k 1时刻的真实状态；
[0011]
所述k时刻的过程噪声信息表示为：过程噪声属于椭球集中；0表示椭球的中心点，qk表示椭球的形状矩阵；
[0012]
所述k 1时刻的估计约束表示为：g
k 1
(
·
)是连续可微的估计约束函数；
[0013]
根据所述状态转移方程和估计约束，通过求解下述半无限规划问题，得到集合tk＝{fk(xk):xk∈ek}的覆盖椭球
[0014][0015][0016][0017][0018]
其中，目标函数f(
·
)为trace函数或logdet函数；表示覆盖椭球的中心点；表示覆盖椭球的形状矩阵；
[0019]
步骤2.2：根据集合tk＝{fk(xk):xk∈ek}的覆盖椭球和过程噪声信息，通过求解下述优化问题得到k 1时刻的状态预报椭球
[0020]
min f(p
k 1|k
)
[0021][0022]
其中，表示k 1时刻状态预报椭球的中心点，同时表示k 1时刻的状态预报估计值；p
k 1|k
表示k 1时刻状态预报椭球的形状矩阵；表示minkowski和；
[0023]
步骤3：根据k 1时刻的状态预报椭球、观测值、观测方程、观测噪声信息和估计约束，计算k 1时刻的状态定界椭球；详细步骤如下：
[0024]
步骤3.1：所述k 1时刻的观测方程表示为：y
k 1
＝h
k 1
(x
k 1
) v
k 1
；y
k 1
表示观测值，h
k 1
(
·
)表示非线性一致连续可微的观测函数，v
k 1
表示观测噪声；
[0025]
所述k 1时刻的观测噪声信息表示为：观测噪声属于椭球集中；0表示椭球的中心点，r
k 1
表示椭球的形状矩阵；
[0026]
根据所述观测值、观测方程和观测噪声信息，通过求解下述半无限规划问题，得到观测反解椭球
[0027][0028]
[0029][0030]
其中，表示观测反解椭球的中心点；表示观测反解椭球的形状矩阵；
[0031]
步骤3.2：根据状态预报椭球、观测反解椭球和估计约束，通过求解下述半无限规划问题得到k 1时刻的状态定界椭球
[0032]
min f(p
k 1
)
[0033][0034][0035][0036]
其中，表示k 1时刻状态定界椭球的中心点，同时表示k 1时刻的状态估计值；p
k 1
表示k 1时刻状态定界椭球的形状矩阵；
[0037]
步骤4：更新时刻参数k＝k 1，返回步骤2。
[0038]
进一步的，所述步骤2.2中的优化问题的最优解所确定的状态预报椭球的中心点满足因此当步骤2.1中的优化问题的最优解所确定的集合覆盖椭球的中心点满足估计约束时，状态预报椭球的中心点就会满足估计约束，由于取状态预报椭球的中心点作为状态的预报估计值，因此能够得出状态的预报估计值满足估计约束；此外，当所述步骤2.2中的优化问题的目标函数取trace函数时，该优化问题的最优解有解析表达式。
[0039]
更进一步的，所述步骤2.1、3.1和3.2中的半无限规划问题的形式一致，将其表示为统一的优化问题进行求解，求解如下的优化问题：
[0040]
min logdet(p)
[0041][0042][0043][0044]
其中，logdet(p)表示矩阵p的logdet函数；p表示矩阵优化变量；r表示集合t中的元素；表示向量值优化变量；集合t是一个紧集，g(
·
)是一个连续可微的函数；这里考虑logdet函数作为优化问题的目标函数，或将目标函数替换为trace函数；通过对集合t进行离散化，同时引入新的变量zi，将上述优化问题表示成consensus的形式：
[0045]
min logdet(p)
[0046]
s.t.(r
i-zi)
t
p-1
(r
i-zi)≤1
[0047][0048][0049]
i∈i
[0050]
其中，i＝{1,2,...,s}，s表示从集合t中选取的离散点的个数；ri∈t；因此得到该优化问题的consensus-admm算法的具体迭代步骤：
[0051]
步骤a：初始化优化变量和参数；
[0052]
步骤b：将zi，p看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的zi，p；
[0053]
步骤c：使用更新后的zi，p，将看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的
[0054]
步骤d：使用更新后的zi，p，更新拉格朗日乘子向量λi，返回步骤b；
[0055]
为简化consensus-admm算法中变量zi和p的联合更新，改为变量zi和p进行交替更新，因此上述步骤b选择如下迭代步骤：
[0056]
步骤b.1：将p看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的p；
[0057]
步骤b.2：使用更新后的p，将zi看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的zi。
[0058]
其中，增广拉格朗日函数为：
[0059][0060]
式中，为拉格朗日乘子向量λi的转置，ρ为惩罚参数，约束条件为的转置，ρ为惩罚参数，约束条件为
[0061]
进一步的，若选取迭代步骤a、b、c、d求解consensus形式的半无限规划问题，当迭代过程产生的优化变量的序列是良定的，并且当迭代序列趋于无穷时，序列收敛，序列zi收敛到序列那么迭代序列中的任何极限点都是原consensus形式的半无限规划问题的kkt点。
[0062]
更进一步的，迭代步骤b.1中更新p的优化问题是凸优化问题，通过fw算法有效求解；迭代步骤b.2中更新zi的优化问题是qcqp-1问题，通过二分法或牛顿法有效求解；迭代步骤c中更新的优化问题通过拉格朗日算法或牛顿法求解；此外，对于迭代步骤3.1中的半无限规划问题以及当估计约束函数为线性函数时，迭代步骤c中更新的优化问题有解析表达式。
[0063]
本发明的有益效果是：
[0064]
1)所提出的基于consensus-admm的带估计约束的非线性集值滤波方法通过求解半无限规划问题将非线性动态系统转化为线性系统，而不是将非线性动态系统线性化，以此获得更紧的状态定界椭球，并且给出的状态定界椭球的中心点满足估计约束。
[0065]
2)在求解半无限规划问题时，采用了consensus-admm算法，并提供了所提出的consensus-admm算法用于求解离散化的半无限规划问题时的弱收敛性。
[0066]
3)将consensus-admm算法的每个迭代步都分解成了可以有效求解的子问题。此外，对于线性估计约束，得到了一个状态更新的解析表达式。
附图说明
[0067]
图1为本发明中基于consensus-admm的带估计约束的非线性集值滤波方法的流程图。
[0068]
图2为本发明实施例子中状态定界椭球体积大小对比图。
[0069]
图3为本发明实施例子中均方根误差(rmse)对比图。
[0070]
图4和图5为本发明实施例子中状态估计值的约束满足情况，其中，图4为约束(1)的满足情况，图5为约束(2)的满足情况。
具体实施方式
[0071]
下面结合附图和具体实施例子对本发明做进一步详细说明。如图1所示，本发明基于consensus-admm的带估计约束的非线性集值滤波方法的流程图，方法步骤具体如下：
[0072]
步骤1：输入k＝0时刻的状态定界椭球作为迭代初始值。
[0073]
对于时刻k＝0到k-1，执行以下步骤：
[0074]
步骤2：根据k时刻的状态定界椭球、状态转移方程、过程噪声信息和k 1时刻的估计约束计算k 1时刻的状态预报椭球。
[0075]
详细步骤如下：
[0076]
步骤2.1：所述k时刻的状态定界椭球表示为：表示k时刻状态定界椭球的中心点，同时表示k时刻的状态估计值，pk表示k时刻状态定界椭球的形状矩阵；
[0077]
所述k时刻的状态转移方程表示为：x
k 1
＝fk(xk) wk；fk(
·
)表示非线性一致连续可微的状态转移函数，wk表示过程噪声；xk和x
k 1
分别表示k时刻和k 1时刻的真实状态；
[0078]
所述k时刻的过程噪声信息表示为：过程噪声属于椭球集中；0表示椭球的中心点，qk表示椭球的形状矩阵；
[0079]
所述k 1时刻的估计约束表示为：g
k 1
(
·
)是连续可微的估计约束函数；
[0080]
根据所述状态转移方程和估计约束，通过求解下述半无限规划问题，得到集合tk＝{fk(xk):xk∈ek}的覆盖椭球
[0081][0082][0083][0084][0085]
其中，目标函数f(
·
)为trace函数或logdet函数；表示覆盖椭球的中心点；表示覆盖椭球的形状矩阵。
[0086]
步骤2.2：根据集合tk＝{fk(xk):xk∈ek}的覆盖椭球和过程噪声信息，通过求解下述优化问题得到k 1时刻的状态预报椭球
[0087]
min f(p
k 1|k
)
[0088][0089]
其中，表示k 1时刻状态预报椭球的中心点，同时表示k 1时刻的状态预报估计值；p
k 1|k
表示k 1时刻状态预报椭球的形状矩阵；表示minkowski和。
[0090]
步骤3：根据k 1时刻的状态预报椭球、观测值、观测方程、观测噪声信息和估计约束，计算k 1时刻的状态定界椭球。
[0091]
详细步骤如下：
[0092]
步骤3.1：所述k 1时刻的观测方程表示为：y
k 1
＝h
k 1
(x
k 1
) v
k 1
；y
k 1
表示观测值，h
k 1
(
·
)表示非线性一致连续可微的观测函数，v
k 1
表示观测噪声；
[0093]
所述k 1时刻的观测噪声信息表示为：观测噪声属于椭球集中；0表示椭球的中心点，r
k 1
表示椭球的形状矩阵；
[0094]
根据所述观测值、观测方程和观测噪声信息，通过求解下述半无限规划问题，得到观测反解椭球
[0095][0096][0097][0098]
其中，表示观测反解椭球的中心点；表示观测反解椭球的形状矩阵。
[0099]
步骤3.2：根据状态预报椭球、观测反解椭球和估计约束，通过求解下述半无限规划问题得到k 1时刻的状态定界椭球
[0100]
min f(p
k 1
)
[0101][0102][0103][0104]
其中，表示k 1时刻状态定界椭球的中心点，同时表示k 1时刻的状态估计值；p
k 1
表示k 1时刻状态定界椭球的形状矩阵。
[0105]
步骤4：更新时刻参数k＝k 1，返回步骤2。
[0106]
上述基于consensus-admm的带估计约束的非线性集值滤波方法中所述步骤2.2中的优化问题的最优解所确定的状态预报椭球的中心点满足因此当步骤2.1中的优化问题的最优解所确定的集合覆盖椭球的中心点满足估计约束时，状态预报椭球的中心点就会满足估计约束，由于取状态预报椭球的中心点作为状态的预报估计值，因此能够得出状态的预报估计值满足估计约束；此外，当所述步骤2.2中的优化问题的目标函数取trace函数时，该优化问题的最优解有解析表达式。
[0107]
上述基于consensus-admm的带估计约束的非线性集值滤波方法中所述步骤2.1、3.1和3.2中的半无限规划问题的形式一致，将其表示为统一的优化问题进行求解，求解如下的优化问题：
[0108]
min logdet(p)
[0109][0110][0111][0112]
其中，logdet(p)表示矩阵p的logdet函数；p表示矩阵优化变量；r表示集合t中的
元素；表示向量值优化变量；集合t是一个紧集，g(
·
)是一个连续可微的函数；这里考虑logdet函数作为优化问题的目标函数，当目标函数是trace函数时也得到类似的结果；通过对集合t进行离散化，同时引入新的变量zi，将上述优化问题表示成consensus的形式：
[0113]
min logdet(p)
[0114]
s.t.(r
i-zi)
t
p-1
(r
i-zi)≤1
[0115][0116][0117]
i∈i
[0118]
其中i＝{1,2,...,s}，s表示从集合t中选取的离散点的个数；ri∈t。
[0119]
因此得到该优化问题的consensus-admm算法的具体迭代步骤：
[0120]
步骤a：初始化优化变量和参数。
[0121]
步骤b：将zi，p看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的zi，p；
[0122]
为简化consensus-admm算法中变量zi和p的联合更新，改为变量zi和p进行交替更新。
[0123]
因此上述步骤b选择如下迭代步骤：
[0124]
步骤b.1：将p看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的p；
[0125]
步骤b.2：使用更新后的p，将zi看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的zi。
[0126]
步骤c：使用更新后的zi，p，将看作优化变量，在满足相应的约束条件下极小化增广拉格朗日函数，得到更新后的
[0127]
步骤d：使用更新后的zi，p，更新拉格朗日乘子向量λi，返回步骤b。
[0128]
其中，增广拉格朗日函数为：
[0129][0130]
式中，为拉格朗日乘子向量λi的转置，ρ为惩罚参数，约束条件为的转置，ρ为惩罚参数，约束条件为
[0131]
若选取迭代步骤a、b、c、d求解consensus形式的半无限规划问题，当迭代过程产生的优化变量的序列是良定的，并且当迭代序列趋于无穷时，序列收敛，序列zi收敛到序列那么迭代序列中的任何极限点都是原consensus形式的半无限规划问题的kkt点。
[0132]
迭代步骤b.1中更新p的优化问题是凸优化问题，通过fw算法有效求解；迭代步骤b.2中更新zi的优化问题是qcqp-1问题，通过二分法或牛顿法有效求解；迭代步骤c中更新的优化问题通过拉格朗日算法或牛顿法求解；此外，对于步骤3.1中的半无限规划问题以及当估计约束函数为线性函数时，迭代步骤c中更新的优化问题有解析表达式。
[0133]
本发明提出基于consensus-admm的带估计约束的非线性集值滤波方法，该方法通过求解半无限规划问题将非线性动态系统转化为线性系统，首先根据状态转移函数和估计约束，通过求解半无限规划问题，得到集合覆盖椭球，再利用两个椭球的minkowski和的最
小覆盖椭球的性质，得到中心点满足估计约束的状态预报椭球。接下来，根据观测值、观测方程和观测噪声信息，通过求解半无限规划问题，得到观测反解椭球，最后根据状态预报椭球、观测反解椭球和估计约束，通过求解半无限规划问题得到状态定界椭球。
[0134]
由于所提出的非线性集值滤波方法中涉及到的半无限规划问题的形式一致，因此可以采取统一的算法求解。在求解半无限规划问题时，本发明采用了consensus-admm算法，并提供了所提出的consensus-admm算法用于求解离散化的半无限规划问题时的弱收敛性。此外，本发明将所提出的用于求解半无限规划问题的consensus-admm算法的每个迭代步都分解成了可以有效求解的子问题，进一步地，对于线性估计约束，还得到了一个状态更新的解析表达式。
[0135]
本实施例子具体描述本发明基于consensus-admm的带估计约束的非线性集值滤波方法用于分析目标跟踪场景下状态估计准确度的比较，采用目标跟踪场景下的状态估计模型对比本发明方法与对偶集值滤波方法和投影-对偶集值滤波方法进行了模拟试验，其中投影-对偶集值滤波方法的状态估计值是通过将对偶集值滤波方法状态定界椭球的中心点投影到约束子空间上得到的。状态定界椭球体积大小、状态估计值均方根误差(rmse)和估计约束满足情况作为滤波器估计效果的指标，monte carlo仿真次数为50。
[0136]
具体来说：考虑笛卡尔二维坐标系下匀速直线运动(cv)模型，系统状态分别表示k时刻目标的位置分量和速度分量。状态转移函数和观测函数分别表示为：
[0137][0138]
其中，过程噪声和观测噪声所属椭球的形状矩阵分别为qk＝10i，rk＝diag{152,0.52}，估计约束(1)表示为估计约束(2)表示为采样时间间隔t＝5，初始状态为(0,0,25,50)，a＝10000，b＝12000，初始定界椭球的中心点为初始状态附近的随机扰动，初始定界椭球的形状矩阵为p0＝1002i，wk＝(sin(kπ/2),2sin(kπ/2),sin(kπ/4),2sin(kπ/4))。
[0139]
效果分析：仿真结果如下：
[0140]
图2是将状态定界椭球的形状矩阵的logdet作为时刻的函数所绘制的。图2表明，使用本发明方法得到的状态定界椭球的体积小于对偶集值滤波得到的状态定界椭球的体积。图3是本发明方法、对偶集值滤波和投影-对偶集值滤波的均方根误差与时刻的关系。图3表明，本发明方法的状态估计值的均方根误差小于对偶集值滤波和投影-对偶集值滤波的均方根误差。图4和图5表明，本发明方法得到的状态估计值可以满足估计约束条件，但对偶集值滤波的状态估计值不总是能满足估计约束条件。可以看出，使用本发明方法提供的滤波方法在对带估计约束的非线性动态系统进行状态估计时可以确定体积更小、估计值更精确的状态定界椭球，具有更好的性能。