一种交互式多调制因子无迹Kalman滤波方法

一种交互式多调制因子无迹kalman滤波方法
技术领域
1.本发明属于状态估计技术领域，特别涉及一种无迹kalman滤波方法。


背景技术：

2.kalman滤波器广泛应用于状态估计领域，但常规的kalman滤波器仅能用于线性状态空 间模型，无法用于非线性系统。针对于此，研究人员设计了多种非线性kalman滤波器，包 括扩展kalman滤波、无迹kalman滤波、粒子滤波等。在这其中，无迹kalman滤波器以其 较高的估计精度、较低的运算量和简单易用等优点而在非线性状态估计领域得到了广泛的应 用。然而，无迹kalman滤波器需要用户预先设置一个标量调制因子。研究发现，调制因子 的设置对无迹kalman滤波器估计性能有较大的影响，而实际应用中没有统一合理的标准来 得到该调制因子的最优值。尽管对于低维系统(状态向量维度不大于3)，有研究人员基于 gauss分布的矩匹配从理论上分析得到了调制因子最优值，但一些数值仿真表明该最优值难 以保证无迹kalman滤波器获得最优的状态估计性能，且该最优值无法用于高维系统。


技术实现要素：

3.本发明的目的是：针对于无迹kalman滤波器调制因子对状态估计精度影响较大且最优 调制因子选择困难的问题，提出一种交互式多调制因子无迹kalman滤波方法。
4.本发明的技术方案是：对于离散形式非线性状态空间模型，基于无迹kalman滤波器进 行状态估计。首先设定n个无迹kalman滤波的调制因子、各个调制因子之间的转移概率以 及n个初始状态高斯分布的均值、方差矩阵以及权重。对于每一个时间历元，假设上一个时 间历元的状态后验分布为n个gauss分布的加权混合，执行如下步骤：1)计算上一时间历 元观测已知情况下的混合概率密度；2)进行状态混合，将每一个调制因子对应的上一时间历 元后验概率密度混合为单个的gauss分布，计算对应的均值向量与方差矩阵；3)基于每一个 调制因子的上一时间历元后验gauss分布以及调制因子值运行无迹kalman滤波，得到每一个 调制因子当前时间历元的后验gauss分布参数以及对应的似然概率密度；4)基于每一个调制 因子的似然概率密度、各个调制因子之间的转移概率以及上一个时间历元各个调制因子的权 重，计算当前时间历元每一个调制因子的权重；5)根据每一个调制因子的权重及对应的当前 时间历元后验均值及方差矩阵，计算得到当前时间历元的后验状态估计及方差矩阵。
5.本发明包括以下步骤：
6.a.设定n个无迹kalman滤波的调制因子，设定各个调制因子之间的转移概率，设定n 个初始状态高斯分布的均值、方差矩阵以及权重。
7.b.对于每一个时间历元，假设上一个时间历元的状态后验分布为n个gauss分布的加权 混合，执行如下步骤：
8.b1.计算上一时间历元观测已知情况下的混合概率密度。
9.b2.进行状态混合，将每一个调制因子对应的上一时间历元后验概率密度混合为
单个的 gauss分布，计算对应的均值与方差矩阵；
10.b3.基于每一个调制因子的上一时间历元后验gauss分布以及调制因子值运行无迹 kalman滤波，得到每一个调制因子当前时间历元的后验gauss分布以及对应的似然概率密度。
11.b4.基于每一个调制因子的似然概率密度、各个调制因子之间的转移概率以及上一个时 间历元各个调制因子的权重，计算当前时间历元每一个调制因子的权重。
12.b5.根据每一个调制因子的权重及对应的当前时间历元后验均值及方差矩阵，计算得到 当前时间历元的后验状态估计及方差矩阵。
13.上述方案中，所述步骤a采用以下方法：
14.对于非线性状态空间模型：
15.x
k
＝f
k
(x
k
‑1,u
k
‑1)+w
k
‑116.z
k
＝h
k
(x
k
)+v
k
17.其中：下标k表示第k个时间历元的变量；为系统状态向量，为输入 向量，为系统观测向量，为状态转移函数，为 系统观测函数，为过程噪声，为观测噪声；
18.过程噪声与观测噪声均满足均值为0的gauss分布，即：
[0019][0020][0021]
其中：n(x；a,a)表示以a为均值向量，a为方差矩阵的满足gauss分布的随机变量x； 0
m
×
n
为维度为m行n列，每一个元素均为0的矩阵；为过程噪声方差矩阵，为观测噪声方差矩阵；q
k
与r
k
均已知；
[0022]
为保证状态估计的正定性，无迹kalman滤波器需要保持其调制因子κ非负，即κ≥0； 设定调制因子的上下界分别为κ
max
与κ
min
≥0，n个调制因子均匀分布在区间[κ
min
,κ
max
]当中， 即：
[0023][0024]
各个调制因子之间的转移概率设置为：
[0025][0026]
其中：p为调制参数，通常选择0＜＜p＜1；
[0027]
对于每一个调制因子，设置其初始状态高斯分布的均值、方差矩阵以及权重均相等，即：
[0028][0029]
[0030][0031]
其中：下标k|i表示以第i个及第i个时刻之前的系统观测为条件的第k个时刻变量估计 值，上标j表示第j个调制因子。
[0032]
上述方案中，所述步骤b1采用以下方法：
[0033]
在调制因子转移概率以及前一个时间历元每一个调制因子权重已知的 情况下，按照以下方法计算混合概率密度：
[0034][0035][0036]
上述方案中，所述步骤b2采用以下方法：
[0037]
在混合概率密度以及前一个时间历元每一个调制因子对应的后验状态估计与每一个调制因子对应的后验状态方差已知的情况下，按照以下方法进 行状态与方差混合：
[0038][0039][0040]
上述方案中，所述步骤b3采用以下方法：
[0041]
在每一个调制因子及其对应的上一时间历元混合后后验gauss分布参数 已知的情况下，采用如下方法计算每一个调制因子当前时间历元的后验分布 参数：
[0042]
对于每一个调制因子，记其序号为上标j，执行如下步骤：
[0043]
1)预测：
[0044]
计算sigma点及相应的权重：
[0045][0046][0047][0048]
[0049]
其中：[a]
i
表示矩阵a的第i列；
[0050]
计算经过一步预测之后的sigma点：
[0051][0052]
计算先验估计及先验方差：
[0053][0054][0055]
2)更新：
[0056]
计算sigma点及相应的权重：
[0057][0058][0059][0060][0061]
计算各个sigma点的一步观测：
[0062][0063]
计算辅助参数：
[0064][0065][0066][0067]
计算kalman增益：
[0068][0069]
计算后验估计与后验方差：
[0070][0071][0072]
3)计算似然概率密度为：
[0073]
[0074]
上述方案中，所述步骤b4采用以下方法：
[0075]
在当前时间历元每一个调制因子的似然概率密度各个调制因子之间的转移概率 以及上一个时间历元各个调制因子的权重已知情况下，采用如下方法计算 当前时间历元每一个调制因子的权重：
[0076][0077][0078][0079]
上述方案中，所述步骤b5采用以下方法：
[0080]
在每一个调制因子的权重及对应的当前时间历元后验分布参数已知情 况下，采用如下方法计算得到当前时间历元的后验状态估计及方差矩阵：
[0081][0082][0083]
有益效果：本发明通过引入多个调制因子以及交互式机理，设计了交互式多调制因子无 迹kalman滤波方法。该方法无需设置无迹kalman滤波器的调制因子，避免了常规无迹kalman 滤波器最优调制因子选择困难的问题，且可以得到比常规无迹kalman滤波方法更好的状态 估计性能。
附图说明
[0084]
图1为本发明的流程图；
[0085]
图2为实施例3中七个滤波器估计均方误差比较。
具体实施方式
[0086]
实施例1，参见附图1，一种交互式多调制因子无迹kalman滤波方法，包括以下步骤：
[0087]
a.设定n个无迹kalman滤波的调制因子，设定各个调制因子之间的转移概率，设定n 个初始状态高斯分布的均值、方差矩阵以及权重。
[0088]
对于非线性状态空间模型：
[0089]
x
k
＝f
k
(x
k
‑1,u
k
‑1)+w
k
‑1[0090]
z
k
＝h
k
(x
k
)+v
k
[0091]
其中：下标k表示第k个时间历元的变量；为系统状态向量，为输入 向量，为系统观测向量，为状态转移函数，为 系统观测函数，为过程噪声，为观测噪声；
[0092]
过程噪声与观测噪声均满足均值为0的gauss分布，即：
[0093][0094][0095]
其中：n(x；a,a)表示以a为均值向量，a为方差矩阵的满足gauss分布的随机变量x； 0
m
×
n
为维度为m行n列，每一个元素均为0的矩阵；为过程噪声方差矩阵，为观测噪声方差矩阵；q
k
与r
k
均已知；
[0096]
为保证状态估计的正定性，无迹kalman滤波器需要保持其调制因子κ非负，即κ≥0； 设定调制因子的上下界分别为κ
max
与κ
min
≥0，n个调制因子均匀分布在区间[κ
min
,κ
max
]当中， 即：
[0097][0098]
各个调制因子之间的转移概率设置为：
[0099][0100]
其中：p为调制参数，通常选择0＜＜p＜1；
[0101]
对于每一个调制因子，设置其初始状态高斯分布的均值、方差矩阵以及权重均相等，即：
[0102][0103][0104][0105]
其中：下标k|i表示以第i个及第i个时刻之前的系统观测为条件的第k个时刻变量估计 值，上标j表示第j个调制因子。
[0106]
b.对于每一个时间历元，假设上一个时间历元的状态后验分布为n个gauss分布的加权 混合，执行如下步骤：
[0107]
b1.计算上一时间历元观测已知情况下的混合概率密度。
[0108]
在调制因子转移概率以及前一个时间历元每一个调制因子权重已知的 情况下，按照以下方法计算混合概率密度：
[0109][0110][0111]
b2.进行状态混合，将每一个调制因子对应的上一时间历元后验概率密度混合为单个的 gauss分布，计算对应的均值与方差矩阵。
[0112]
在混合概率密度以及前一个时间历元每一个调制因子对应的后验状态估计与每一个调制因子对应的后验状态方差已知的情况下，按照以下方法进 行状态与方差混合：
[0113][0114][0115]
b3.基于每一个调制因子的上一时间历元后验gauss分布以及调制因子值运行无迹 kalman滤波，得到每一个调制因子当前时间历元的后验gauss分布以及对应的似然概率密度。 在每一个调制因子及其对应的上一时间历元混合后后验gauss分布参数已知的情况下，采用如下方法计算每一个调制因子当前时间历元的后验分布 参数：
[0116]
对于每一个调制因子，记其序号为上标j，执行如下步骤：
[0117]
1)预测：
[0118]
计算sigma点及相应的权重：
[0119][0120][0121][0122][0123]
其中：[a]
i
表示矩阵a的第i列；
[0124]
计算经过一步预测之后的sigma点：
[0125][0126]
计算先验估计及先验方差：
[0127]
[0128][0129]
2)更新：
[0130]
计算sigma点及相应的权重：
[0131][0132][0133][0134][0135]
计算各个sigma点的一步观测：
[0136][0137]
计算辅助参数：
[0138][0139][0140][0141]
计算kalman增益：
[0142][0143]
计算后验估计与后验方差：
[0144][0145][0146]
3)计算似然概率密度为：
[0147][0148]
b4.基于每一个调制因子的似然概率密度、各个调制因子之间的转移概率以及上一个时 间历元各个调制因子的权重，计算当前时间历元每一个调制因子的权重。
[0149]
在当前时间历元每一个调制因子的似然概率密度各个调制因子之间的转移概率 以及上一个时间历元各个调制因子的权重已知情况下，采用如下方法计算 当前时间历元每一个调制因子的权重：
[0150][0151][0152][0153]
b5.根据每一个调制因子的权重及对应的当前时间历元后验均值及方差矩阵，计算得到 当前时间历元的后验状态估计及方差矩阵。
[0154]
在每一个调制因子的权重及对应的当前时间历元后验分布参数已知情 况下，采用如下方法计算得到当前时间历元的后验状态估计及方差矩阵：
[0155][0156][0157]
实施例2，本发明实现的伪代码为：
[0158][0159]
实施例3，利用实施例1所述的交互式多调制因子无迹kalman滤波器通过仿真数据进行 验证。
[0160]
仿真案例为广泛用于非线性状态估计算法验证的单变量非静态增长模型，状态方程及观 测方程分别为
[0161][0162][0163]
总仿真时间历元数t选择为100。过程噪声以及观测噪声方差设置为：q
k
＝1,r
k
＝1。状 态初始值及初始方差设置为x0＝0,p0＝2。对于本发明所提出的方法，调制参数设置为： κ
min
＝0,κ
max
＝5,n＝6,p＝0.8。
[0164]
作为比较，本实施例同时展示了常规无迹kalman滤波器的估计结果，无迹kalman滤波 器调制因子分别选择为κ＝0,κ＝1,κ＝2,κ＝3,κ＝4,κ＝5(分别简记作ukf0、ukf1、 ukf2、ukf3、ukf4及ukf5)。本发明的方法简记作“imm
‑
ukf”。
[0165]
20000独立的次monte carlo仿真的结果被用来验证本发明所提出的方法。状态变量的均 方根误差(rmse)及平均均方根误差(armse)被用来评价不同方法的性能。评价指标的 计算方式如下：
[0166][0167][0168]
其中：为第i次monte carlo仿真中状态变量在k时刻真实值；为第i次monte carlo 仿真中状态变量在k时刻的估计值；m＝20000表示总的仿真次数。
[0169]
附图2为七个滤波器估计均方误差比较，对应的平均均方根误差如表1所示。
[0170] ukf0ukf1ukf2ukf3ukf4ukf5imm
‑
ukfarmse8.5946.4497.1157.1917.2337.2885.146
[0171]
表1
[0172]
根据图2以及表1，可以看出相比于基于单个调制因子的无迹kalman滤波器，本发明所 提出的imm
‑
ukf可以获得更好的状态估计精度。本发明所提出的方案无需设置调制因子， 其估计性能远优于根据gauss分布矩匹配得出的一维无迹kalman滤波器最优调制参数(κ＝2)下的估计性能，也优于基于单个调制因子的无迹kalman滤波器所能得到的最好估 计性能(κ＝1)。有较好的实际应用潜力。
[0173]
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基 础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏 离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。