-
摘要: 针对无迹卡尔曼滤波（Unscented Kalman fllter，UKF）在强非线性系统中估计效果差的问题，提出了双层无迹卡尔曼滤波（Double layer unscented Kalman filter，DLUKF）算法，该算法用带权值的采样点表征先验分布，而后用内层UKF算法对每个采样点进行更新，最后引入外层UKF算法的更新机制得到估计值和估计协方差.仿真结果表明，相比于传统算法，所提的DLUKF算法可以在较低计算负载下获得较高滤波估计精度.
-
关键词:
- 状态估计 /
- 采样策略 /
- 无迹卡尔曼滤波 /
- 改进的无迹卡尔曼滤波 /
- 无迹粒子滤波
Abstract: The unscented Kalman filter (UKF) has the problem of the inaccurate estimation in strong nonlinear systems. To solve this problem, the double layer unscented Kalman filter (DLUKF) algorithm is proposed. In the proposed algorithm, the weighted sampling points are used to represent the prior distribution, and then the inner layer UKF algorithm is used to update each sampling point. Finally, the state estimations are obtained by the update mechanism of the outer layer UKF algorithm. Simulation results show that the proposed algorithm not only has a low computational complexity, but also has a very good estimation accuracy, compared with the existing filtering algorithms. -
状态估计在信号处理、计算机视觉、自动控制、目标跟踪、导航、金融、通信等领域[1-6]有着广泛应用.在高斯噪声环境下, 卡尔曼滤波(Kalman fllter, KF)[7]及其次优滤波算法可以很好解决该问题.在非高斯噪声环境下, KF算法及其次优滤波算法不再适用, 因此存在着粒子滤波(Particle filter, PF)[8]及其衍生滤波算法来解决状态估计问题.
基于无迹变换(Unscented transform, UT)的无迹卡尔曼滤波(Unscented Kalman fllter, UKF)[9-11]是一种计算非线性变换均值和协方差的次优卡尔曼滤波算法.相比于扩展卡尔曼滤波(Extended Kalman fllter, EKF), UKF不需要计算雅可比矩阵, 且其可以达到非线性函数二阶泰勒展开式的精度[9].因此其在导航制导、目标跟踪、信号处理和图像跟踪等方面有着很广泛应用.但UKF算法也存在着在某些情况下估计效果差等问题.
目前, 针对UKF算法估计值不准确的问题, 有众多改进方法.为了解决UKF在工程应用中因舍入误差导致数值不稳定的问题, 提出了求根UKF (Square-root unscented Kalman fllter, SRUKF)[12]算法.在加性噪声条件下, 为了降低UKF算法的计算复杂度, 提出了简化UKF (Simplified unscented Kalman fllter, SUKF)[13]算法.在先验信息不确定性大而量测精度高的情况下, 只用一次量测值的UKF算法的估计效果较差.因此, 提出了多次利用量测值的迭代UKF (Iterated unscented Kalman fllter, IUKF)[14], 递归更新滤波器(递归更新扩展卡尔曼滤波(Recursive update extended Kalman fllter, RUEKF)[15]、递归更新容积卡尔曼滤波(Recursive update cubature Kalman fllter, RUCKF)[16])等算法.基于二阶UT变换的UKF算法滤波估计精度只能达到二阶, 为了提高滤波精度, 提出了基于高阶UT变换和高阶容积变换(Cubature transform, CT)的高阶UKF[17-18]和高阶容积卡尔曼滤波(Cubature Kalman fllter, CKF)[19-21]等算法.
UKF及其改进算法虽然可以较好处理UKF算法的估计不准确的问题, 但其仍然存在在非线性程度高的环境下估计效果差等问题, 文献[22-23]中提出将UKF算法作为PF算法建议分布, 将UKF算法估计值作为重要性密度函数, 这就是无迹粒子滤波(Unscented particle fllter, UPF)[22-23]算法.从理论上讲, 随着随机采样粒子数量提高, UPF算法的精度可以逐渐提高.但UPF算法也存在一些问题, 如其运算时间很长, 时效性较差.且UPF算法效果不总是好于UKF算法, 在量测噪声较大时, UPF算法估计精度会不如UKF算法.
为了在低计算负载的情况下获得高的滤波估计精度, 本文提出了双层无迹卡尔曼滤波器(Double layer unscented Kalman filter, DLUKF)算法.其核心思想是用带有权值的采样点表示前一个时刻的后验密度函数; 而后用内层的UKF算法对每个带权值的采样点进行更新, 并用最新的量测值对采样点的权值进行更新; 然后将各个采样点进行加权融合, 得到了初始的估计值; 最后用外层UKF算法的更新机制对初始估计值进行更新得到最终的估计值.
1. 无迹卡尔曼滤波(UKF)算法
1.1 UT变换
假设非线性函数为$\mathit{\boldsymbol{y}} = \mathit{\boldsymbol{f}}(\mathit{\boldsymbol{x}}) $, UT变换是通过近似非线性函数的概率密度分布来近似非线性函数.其在得到先验均值$\bar {\mathit{\boldsymbol{x}}}$和协方差$ {\mathit{\boldsymbol{P}}_{xx}}$的基础上, 用采样策略选取一组确定性采样点集.而后得到这些采样点集经非线性变换后的采样点集, 进而求得经非线性变换后的均值$\bar {\mathit{\boldsymbol{y}}}$和协方差${\mathit{\boldsymbol{P}}_{yy}}$.
UT变换算法可以归纳为以下三步:
1) 根据先验均值$\bar {\mathit{\boldsymbol{x}}}$和协方差${\mathit{\boldsymbol{P}}_{xx}}$, 用采样策略得到$N$个确定性采样点$\{ {\mathit{\boldsymbol{x}}_i}\} _{i = 1}^N$.定义$w_i^m$为均值加权作用的权值, $w_i^c$为协方差加权所用的权值.
2) 将确定性采样点$\{ {\mathit{\boldsymbol{x}}_i}\} _{i = 1}^N$进行非线性$\mathit{\boldsymbol{f}}(\cdot) $变换, 得到$N$个经非线性变换后的采样点集$\{ {\mathit{\boldsymbol{y}}_i}\} _{i = 1}^N = \mathit{\boldsymbol{f}}(\{ {\mathit{\boldsymbol{x}}_i}\} _{i = 1}^N) $.
3) 通过对采样点集$\{ {\mathit{\boldsymbol{y}}_i}\} _{i = 1}^N$进行加权的形式得到经非线性变换后的均值和协方差为.
1.2 UKF算法
考虑典型的非线性系统, 其状态方程和量测方程分别为:
$ \begin{equation} {\mathit{\boldsymbol{x}}_{k + 1}} = \mathit{\boldsymbol{f}}({\mathit{\boldsymbol{x}}_k}) + {\mathit{\boldsymbol{w}}_k} \end{equation} $
(1) $ \begin{equation} {\mathit{\boldsymbol{z}}_{k + 1}} = \mathit{\boldsymbol{h}}({\mathit{\boldsymbol{x}}_{k + 1}}) + {\mathit{\boldsymbol{v}}_{k + 1}} \end{equation} $
(2) ${\mathit{\boldsymbol{x}}_k}$为$k$时刻$n$维的状态向量, ${\mathit{\boldsymbol{z}}_{k + 1}}$为$k + 1$时刻的量测向量. ${\mathit{\boldsymbol{w}}_k}$为$m$维的过程噪声, 其服从均值为0方差为$\mathit{\boldsymbol{Q}}$的高斯分布. ${\mathit{\boldsymbol{v}}_{k + 1}}$为$q$维的量测噪声, 其服从均值为0方差为$\mathit{\boldsymbol{R}}$的高斯分布.滤波算法的目的就是从带有噪声的量测值${\mathit{\boldsymbol{z}}_{k + 1}}$中估计出真实值${\mathit{\boldsymbol{x}}_{k + 1}}$.
UKF[9-10]算法是基于UT变换的一种滤波算法, 其思想是在一步预测的时候, 用UT变换来进行均值和协方差传递.在UKF算法中, 因为存在噪声项, 需要对状态进行扩维.因此状态向量可以表示为. UKF算法流程为:
1) 在$k$时刻由UT变换中的采样策略得到$N$个采样点集$\{ \mathit{\boldsymbol{x}}_k^i\} _{i = 1}^N$.
2) 采样点集$\{ \mathit{\boldsymbol{x}}_k^i\} _{i = 1}^N$经非线性变换$\mathit{\boldsymbol{f}}(\cdot) $后得到采样点集$\{ \mathit{\boldsymbol{x}}_{k + 1|k}^i\} _{i = 1}^N$.
3) 由采样点集$\{ \mathit{\boldsymbol{x}}_{k + 1|k}^i\} _{i = 1}^N$加权求得预测值${\hat {\mathit{\boldsymbol{x}}}_{k + 1|k}}$和预测协方差${\hat {\mathit{\boldsymbol{P}}}_{k + 1|k}}$.
4) 采样点集$\{ \mathit{\boldsymbol{x}}_{k + 1|k}^i\} _{i = 1}^N$经非线性变换$\mathit{\boldsymbol{h}}(\cdot) $后得到采样点集$\{ \mathit{\boldsymbol{z}}_{k + 1|k}^i\} _{i = 1}^N$.
5) 由采样点集$\{ \mathit{\boldsymbol{z}}_{k + 1|k}^i\} _{i = 1}^N$加权求得预测的量测值${\hat {\mathit{\boldsymbol{z}}}_{k + 1|k}}$及其协方差${\mathit{\boldsymbol{P}}_{zz}}$和互协方差${\mathit{\boldsymbol{P}}_{xz}}$.
6) 求得$k + 1$时刻的估计值${\hat {\mathit{\boldsymbol{x}}}_{k + 1}}$及和协方差${\hat {\mathit{\boldsymbol{P}}}_{k + 1}}$.
在实际应用中, 受初始误差的影响, UKF算法存在着收敛速度慢, 精度不高等问题.基于此, 文献[14]提出了IUKF算法, 文献[15]提出了RUEKF算法, 文献[16]出了RUCKF算法.这三种算法的核心思想都是多次利用量测值对估计值进行更形, 以获得更好的滤波估计效果.
2. 无迹粒子滤波(UPF)算法
UPF[22-23]算法是在PF算法的基础上, 用UKF算法的滤波估计值作为PF算法的建议密度函数.这虽然可以解决UKF算法不适用于非高斯环境等问题, 但其由于要选取大量的随机性采样点来逼近密度函数, 所以UPF算法会临着计算量大的问题. UPF算法具体步骤如下:
1) 由$p({\mathit{\boldsymbol{x}}_0}) $得到$N$个粒子点$\{ \mathit{\boldsymbol{x}}_0^{(i)}\} _{i = 1}^N$, 初始权值为$\mathit{\boldsymbol{w}}_0^{(i)} = 1/N$.
2) 用UKF算法对每一粒子进行状态更新.
3) 计算粒子点对应的权值$\mathit{\boldsymbol{w}}_k^{(i)} = \mathit{\boldsymbol{w}}_{k - 1}^{(i)}\frac{{p({\mathit{\boldsymbol{z}}_k}|\mathit{\boldsymbol{x}}_k^{(i)})p(\mathit{\boldsymbol{x}}_k^{(i)}|\mathit{\boldsymbol{x}}_{k - 1}^{(i)})}}{{q(\mathit{\boldsymbol{x}}_k^{(i)}|{\mathit{\boldsymbol{z}}_{1:k}})}}$并对其归一化.
4) 当粒子退化严重时, 对粒子进行重采样.
5) 计算每个粒子点${\mathit{\boldsymbol{x}}^{(i)}}$对应的协方差.
6) 重复步骤2)~5).
最后得到$k$时刻状态量的估计为${\hat {\mathit{\boldsymbol{x}}}_k} = \sum\nolimits_{i = 1}^N {\tilde {\mathit{\boldsymbol{w}}}_k^{(i)}\mathit{\boldsymbol{x}}_k^{(i)}} $.
3. 双层无迹卡尔曼滤波(DLUKF)算法
UPF需要用大量的粒子点去逼近状态的后验密度函数, 因此其有着运算量大的问题.本文所提的DLUKF算法用带权值的采样点去表征状态的后验密度函数, 其核心思想为用内层的UKF对每个带权值的采样点进行更新, 而后用最新的量测值对每个采样点的权值进行更新, 并对更新后的采样点进行加权求和得到下一时刻初始估计值, 然后将该初始估计值作为预测值运行外层UKF算法, 从而得到最终估计值.
3.1 DLUKF算法框架
DLUKF算法由外层UKF算法和内层UKF算法组成, 其算法流程如下:
状态初始条件为初始值${\hat {\mathit{\boldsymbol{x}}}_0} = {\rm E}({\mathit{\boldsymbol{x}}_0}) $, 初始协方差${\hat {\mathit{\boldsymbol{P}}}_0} = {\rm E}(({\mathit{\boldsymbol{x}}_0} - {\hat {\mathit{\boldsymbol{x}}}_0}){({\mathit{\boldsymbol{x}}_0} - {\hat {\mathit{\boldsymbol{x}}}_0})^{\rm T}}) $.因为存在噪声项, 需要对初始的状态进行扩维处理.其可以表示为
$ \begin{equation} \hat {\mathit{\boldsymbol{x}}}_0^a = {\left[ {\begin{array}{*{20}{c}} {{{\hat {\mathit{\boldsymbol{x}}}}_0}}&0&0 \end{array}} \right]^{\rm T}} \end{equation} $
(3) $ \begin{equation} \mathit{\boldsymbol{P}}_0^a = {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{P}}_0}}&0&0\\ 0&\mathit{\boldsymbol{Q}}&0\\ 0&0&\mathit{\boldsymbol{R}} \end{array}} \right]^{\rm T}} \end{equation} $
(4) 内层UKF算法:
在$k$时刻, 用采样策略选取$N$个采样点$\{ {\hat {\mathit{\boldsymbol{x}}}_{i, k}}\} _{i = 1}^N$, 并求取其权值对应的一阶矩$w_{i, k}^m$和二阶矩$w_{i, k}^c$.而后用内层UKF算法对每个采样点进行更新.
对每个采样点, 用采样策略选取$M$个采样点$ \{ {\hat {\mathit{\boldsymbol{x}}}_{j, i, k}}\} _{j = 1}^M$, 并取其对应的一阶矩$w_{j, i, k}^m$和二阶矩$w_{j, i, k}^c$.
时间更新:
$ \begin{equation} \hat {\mathit{\boldsymbol{x}}}_{j, i, k + 1|k}^x = \mathit{\boldsymbol{f}}(\hat {\mathit{\boldsymbol{x}}}_{j, i, k}^x, \hat {\mathit{\boldsymbol{x}}}_{j, i, k}^w) \end{equation} $
(5) $ \begin{equation} {\hat {\mathit{\boldsymbol{x}}}_{i, k + 1|k}} = \sum\limits_{j = 1}^M { w_{j, i, k}^m\hat {\mathit{\boldsymbol{x}}}_{j, i, k + 1|k}^x} \end{equation} $
(6) $ \begin{align} {{\hat {\mathit{\boldsymbol{P}}}}_{i, k + 1|k}} = &\sum\limits_{j = 1}^M {w_{j, i}^c(\hat {\mathit{\boldsymbol{x}}}_{j, i, k + 1|k}^x - {{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1|k}})}\times\nonumber\\& {(\hat {\mathit{\boldsymbol{x}}}_{j, i, k + 1|k}^x - {{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1|k}})^{\rm T}} + \mathit{\boldsymbol{Q}} \end{align} $
(7) 量测更新:
基于预测值${\hat {\mathit{\boldsymbol{x}}}_{i, k + 1|k}}$和预测协方差${\hat {\mathit{\boldsymbol{P}}}_{i, k + 1|k}}$产生新的$M$个带权值的采样点.
$ \begin{equation} {\mathit{\boldsymbol{z}}_{j, i, k + 1|k}} = \mathit{\boldsymbol{h}}(\mathit{\boldsymbol{x}}_{j, i, k + 1|k}^x, \mathit{\boldsymbol{x}}_{j, i, k + 1|k}^v) \end{equation} $
(8) $ \begin{equation} {\hat {\mathit{\boldsymbol{z}}}_{i, k + 1|k}} = \sum\limits_{j = 1}^M {w_{j, i}^m{\mathit{\boldsymbol{z}}_{j, i, k + 1|k}}} \end{equation} $
(9) $ \begin{align} {\mathit{\boldsymbol{P}}_{i, zz}} = & \sum\limits_{j = 1}^M {w_{j, i}^c({\mathit{\boldsymbol{z}}_{j, i, k + 1|k}} - {{\hat {\mathit{\boldsymbol{z}}}}_{i, k + 1|k}})}\times \nonumber\\& {({\mathit{\boldsymbol{z}}_{j, i, k + 1|k}} - {{\hat {\mathit{\boldsymbol{z}}}}_{i, k + 1|k}})^{\rm T}} + \mathit{\boldsymbol{R}} \end{align} $
(10) $ \begin{align} {\mathit{\boldsymbol{P}}_{i, xz}} = &\sum\limits_{j = 1}^M w_{j, i}^c(\hat {\mathit{\boldsymbol{x}}}_{j, i, k + 1|k}^x - {{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1|k}})\times \nonumber\\&{{({\mathit{\boldsymbol{z}}_{j, i, k + 1|k}} - {{\hat {\mathit{\boldsymbol{z}}}}_{i, k + 1|k}})}^{\rm T}} \end{align} $
(11) $ \begin{equation} {\mathit{\boldsymbol{K}}_{i, k + 1}} = {\mathit{\boldsymbol{P}}_{i, xz}}\mathit{\boldsymbol{P}}_{i, zz}^{ - 1} \end{equation} $
(12) $ \begin{equation} {\hat {\mathit{\boldsymbol{x}}}_{i, k + 1}} = {\hat {\mathit{\boldsymbol{x}}}_{i, k + 1|k}} + {\mathit{\boldsymbol{K}}_{i, k + 1}}({\mathit{\boldsymbol{z}}_{k + 1}} - {\hat {\mathit{\boldsymbol{z}}}_{i, k + 1|k}}) \end{equation} $
(13) $ \begin{equation} {\hat {\mathit{\boldsymbol{P}}}_{i, k + 1}} = {\hat {\mathit{\boldsymbol{P}}}_{i, k + 1|k}} - {\mathit{\boldsymbol{K}}_{i, k + 1}}{\mathit{\boldsymbol{P}}_{i, zz}}\mathit{\boldsymbol{K}}_{i, k + 1}^{\rm T} \end{equation} $
(14) 在采样点用内层UKF算法更新后, 类似于UPF算法, 表示一阶矩的权值和表示二阶矩的权值的更新可以表示为:
$ \begin{equation} \left\{ {\begin{array}{*{20}{c}} {w_i^m = w_i^m\frac{{p({\mathit{\boldsymbol{z}}_{k + 1}}|{{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}})p({{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}}|{{\hat {\mathit{\boldsymbol{x}}}}_{i, k}})}}{{q({{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}}|{\mathit{\boldsymbol{z}}_{1:k}})}}}\\ {w_i^c = w_i^c\frac{{p({\mathit{\boldsymbol{z}}_{k + 1}}|{{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}})p({{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}}|{{\hat {\mathit{\boldsymbol{x}}}}_{i, k}})}}{{q({{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}}|{\mathit{\boldsymbol{z}}_{1:k}})}}} \end{array}} \right. \end{equation} $
(15) 在得到权值更新的基础上, 对权值进行归一化处理, 有
$ \begin{equation} \left\{ {\begin{array}{*{20}{c}} {w_i^m = \frac{{w_i^m}}{{\sum\limits_{i = 1}^N {w_i^m} }}}\\ {w_i^c = \frac{{w_i^c}}{{\sum\limits_{i = 1}^N {w_i^c} }}} \end{array}} \right. \end{equation} $
(16) $k + 1$时刻的初始估计值及其协方差可以表示为
$ \begin{equation} \hat {\mathit{\boldsymbol{x}}}_{k + 1}^I = \sum\limits_{i = 1}^N {w_i^m{{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}}} \end{equation} $
(17) $ \begin{equation} \hat {\mathit{\boldsymbol{P}}}_{k + 1}^I = \sum\limits_{i = 1}^N {w_i^c({{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}} - \hat {\mathit{\boldsymbol{x}}}_{k + 1}^I){{({{\hat {\mathit{\boldsymbol{x}}}}_{i, k + 1}} - \hat {\mathit{\boldsymbol{x}}}_{k + 1}^I)}^{\rm T}}} + \mathit{\boldsymbol{Q}} \end{equation} $
(18) 外层UKF算法:
基于$\hat {\mathit{\boldsymbol{x}}}_{k + 1}^I$和$\hat {\mathit{\boldsymbol{P}}}_{k + 1}^I$, 用采样策略选取$N$个带权值的采样点$ \{ \mathit{\boldsymbol{x}}_{i, k + 1}^I\} _{i = 1}^N$.而后再次对粒子点进行量测更新, 可以表示为:
$ \begin{equation} \mathit{\boldsymbol{z}}_{i, k + 1}^I = \mathit{\boldsymbol{h}}(\mathit{\boldsymbol{x}}_{i, k + 1}^{I, x}, \mathit{\boldsymbol{x}}_{i, k + 1}^{I, v}) \end{equation} $
(19) $ \begin{equation} \hat {\mathit{\boldsymbol{z}}}_{k + 1}^I = \sum\limits_{j = 1}^N {w_i^m\mathit{\boldsymbol{z}}_{i, k + 1}^I} \end{equation} $
(20) $ \begin{equation} \mathit{\boldsymbol{P}}_{zz}^I = \sum\limits_{i = 1}^N {w_i^c(\mathit{\boldsymbol{z}}_{i, k + 1}^I - \hat {\mathit{\boldsymbol{z}}}_{k + 1}^I){{(\mathit{\boldsymbol{z}}_{i, k + 1}^I - \hat {\mathit{\boldsymbol{z}}}_{k + 1}^I)}^{\rm T}}} + \mathit{\boldsymbol{R}} \end{equation} $
(21) $ \begin{equation} \mathit{\boldsymbol{P}}_{xz}^I = \sum\limits_{i = 1}^N {w_i^c(\mathit{\boldsymbol{x}}_{i, k + 1}^{I, x} - \hat {\mathit{\boldsymbol{x}}}_{k + 1}^I){{(\mathit{\boldsymbol{z}}_{i, k + 1}^I - \hat {\mathit{\boldsymbol{z}}}_{k + 1}^I)}^{\rm T}}} \end{equation} $
(22) $ \begin{equation} \mathit{\boldsymbol{K}}_{k + 1}^I = \frac{{\mathit{\boldsymbol{P}}_{xz}^I}}{{\mathit{\boldsymbol{P}}_{zz}^I}} \end{equation} $
(23) $ \begin{equation} {\hat {\mathit{\boldsymbol{x}}}_{k + 1}} = \hat {\mathit{\boldsymbol{x}}}_{k + 1}^I + \mathit{\boldsymbol{K}}_{k + 1}^I({\mathit{\boldsymbol{z}}_{k + 1}} - \hat {\mathit{\boldsymbol{z}}}_{k + 1}^I) \end{equation} $
(24) $ \begin{equation} {\hat {\mathit{\boldsymbol{P}}}_{k + 1}} = \hat {\mathit{\boldsymbol{P}}}_{k + 1}^I - \mathit{\boldsymbol{K}}_{k + 1}^I\mathit{\boldsymbol{P}}_{zz}^I{(\mathit{\boldsymbol{K}}_{k + 1}^I)^{\rm T}} \end{equation} $
(25) 不断重复方程(5)~(25), 即可求得DLUKF算法在每个时刻的估计值${\hat x_k}$.
DLUKF算法的流程图如图 1所示.
3.2 基于对称采样策略的DLUKF算法
根据选取粒子点的采样策略不同, 又可以得到多种DLUKF算法.在UT变换中, 目前的采样策略方法包括对称采样、单形采样、3阶矩偏度采样和高斯分布4阶矩对称采样[8]等.还有为了保证经过非线性变换后协方差${\mathit{\boldsymbol{P}}_{yy}}$的正定性而提出的对基本采样策略进行比例修正的算法框架.
下面主要详细介绍对称采样策略.
考虑均值$\bar {\mathit{\boldsymbol{x}}}$和协方差${\mathit{\boldsymbol{P}}_{xx}}$的情况下, 通过对称采样的策略选取$N = 2n + 1$个采样点.采样点及其权值可以表示为:
$ \begin{equation} \left\{ \begin{array}{l} {\mathit{\boldsymbol{x}}^{(1)}} = \bar {\mathit{\boldsymbol{x}}}\\ \{ {\mathit{\boldsymbol{x}}^{(i)}}\} _{i = 2}^{N - n} = \bar {\mathit{\boldsymbol{x}}} + \sqrt {(n + \kappa )} {(\sqrt {{\mathit{\boldsymbol{P}}_{xx}}} )_{i - 1}}\\ \{ {\mathit{\boldsymbol{x}}^{(i)}}\} _{i = N - n + 1}^N = \bar {\mathit{\boldsymbol{x}}} - \sqrt {(n + \kappa )} {(\sqrt {{\mathit{\boldsymbol{P}}_{xx}}} )_{i - n + 1}} \end{array} \right. \end{equation} $
(26) $ \begin{equation} \left\{ \begin{array}{l} w_1^m = w_1^c = \frac{{\kappa }}{{n + \kappa }}\\ \{ w_i^m\} _{i = 2}^N = \{ w_i^c\} _{i = 2}^N = \frac{{1}}{{2n + 2\kappa }} \end{array} \right. \end{equation} $
(27) 式(26)中的$n$表示均值$\bar {\mathit{\boldsymbol{x}}}$维数. $\kappa $为比例参数, 可调节采样点与均值$\bar x$间的距离, 仅影响二阶以后高阶矩带来的误差. $ {(\sqrt {{\mathit{\boldsymbol{P}}_{xx}}})_i}$表示平方根矩阵的第$i$列或行.
在对称采样策略中, 采样点除了中心点外, 其他的采样点的权值是相同的.这说明除中心点外, 其他采样点的重要性是相同的.从采样点的分布可以看出, 采样点是关于中心点呈中心对称的.
基于对称采样的DLUKF算法就是在产生粒子点时用对称采样策略产生粒子点, 其具体的算法流程如下:
1) $k$时刻的估计值为${\hat {\mathit{\boldsymbol{x}}}_k}$, 协方差为${\hat {\mathit{\boldsymbol{P}}}_k}$.
2) 基于${\hat {\mathit{\boldsymbol{x}}}_k}$和${\hat {\mathit{\boldsymbol{P}}}_k}$, 通过式(26)和式(27)求得外层UKF算法$N$个采样点$\{ {\hat {\mathit{\boldsymbol{x}}}_{i, k}}\} _{i = 1}^N$, 及其权值对应的一阶矩$w_{i, k}^m$和二阶矩$w_{i, k}^c$.
3) 通过方程(5) $ \sim $ (14)得到每个粒子经内层UKF更新后的粒子点$\{ {\hat {\mathit{\boldsymbol{x}}}_{i, k + 1}}\} _{i = 1}^N$及其协方差$\{ {\hat {\mathit{\boldsymbol{P}}}_{i, k + 1}}\} _{i = 1}^N$.
4) 通过方程(15) $ \sim $ (16)得到外层UKF更新后的权值$\{ w_i^m\} _{i = 1}^N$和$\{ w_i^c\} _{i = 1}^N$.
5) 通过方程(17) $ \sim $ (18), 得到$k + 1$时刻的初始估计值$\hat {\mathit{\boldsymbol{x}}}_{k + 1}^I$及其协方差$\hat {\mathit{\boldsymbol{P}}}_{k + 1}^I$.
6) 基于$\hat {\mathit{\boldsymbol{x}}}_{k + 1}^I$和$\hat {\mathit{\boldsymbol{P}}}_{k + 1}^I$, 通过式(26)和式(27)求得$N$个采样点$\{ \mathit{\boldsymbol{x}}_{i, k + 1}^{I}\} _{i = 1}^N$.
7) 通过方程(19) $ \sim $ (25), 得到$k + 1$时刻的估计值为${\hat {\mathit{\boldsymbol{x}}}_{k + 1}}$, 协方差为${\hat {\mathit{\boldsymbol{P}}}_{k + 1}}$.
4. 仿真分析
将本文所提的基于对称采样策略DLUKF算法与UKF算法、IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和UPF算法分别在一维和二维仿真场景下进行仿真对比分析, 用滤波算法估计值与真实值间的均方根误差(Root mean square error, RMSE)来表示滤波算法估计效果.
4.1 单维仿真
假设有下述状态空间模型, 其状态方程和量测方程分别可以表示为:
$ \begin{equation} {x_{k + 1}} = 0.5{x_k} + \sin (0.04\pi k) + 1 + {w_k} \end{equation} $
(28) $ \begin{equation} {z_{k + 1}} = 0.2x_{k + 1}^2 + {v_{k + 1}} \end{equation} $
(29) 式(28)中${w_k}$表示过程噪声, 其服从$Ga(3, 2) $的伽马分布.式(29)中的${v_{k + 1}}$表示量测噪声, 其服从均值为0, 方差为的高斯分布.初始位置为${x_0} = 3$, IUKF算法, RUEKF算法和RUCKF算法的迭代次数都为10次. UPF算法粒子数量为100个, DLUKF算法产生粒子的方法是对称采样策略.仿真时间为30 s, 蒙特卡洛仿真次数为100次.其仿真结果如图 2所示.
通过图 2可以看出, IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和UPF算法滤波估计效果都略好于UKF算法.这是因为IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和UPF算法都对UKF算法进行了改进, 所以其效果是好于UKF算法的.本文所提的DLUKF算法在每个时刻的估计效果都好于其他的滤波算法.这说明, DLUKF算法对于UKF算法的改进效果比其他经典算法更加显著.且因为DLUKF算法用两层UKF算法对状态进行估计, 所以可以有着很好的滤波估计效果.
将UPF算法的粒子数由100逐渐增加到500, 其与UKF算法、IUKF算法、RUEKF算法、RUCKF算法、高阶UKF算法、高阶UKF算法和DLUKF算法的单次运行时间以及平均RMSE如表 1所示.
表 1 各算法计算时间及RMSE对比分析表Table 1 The calculation time and RMSE of each algorithm算法 运行时间(s) 平均RMSE UKF 0.0002 0.1566 IUKF 0.0014 0.0881 RUEKF 0.0006 0.0378 RUCKF 0.0031 0.0337 高阶UKF 0.0006 0.1434 高阶CKF 0.0006 0.1437 UPF (100) 0.1032 0.1153 UPF (200) 0.2097 0.0714 UPF (300) 0.3200 0.0626 UPF (400) 0.4296 0.0564 UPF (500) 0.5416 0.0476 DLUKF 0.0016 0.0297 通过表 1可以看出, UKF算法、RUEKF算法、高阶UKF算法和高阶CKF算法的用时都很少. IUKF算法、RUCKF和DLUKF算法的用时略长, 这是由于这三种算法都进行了多次滤波计算. UPF算法用时最长.在UPF算法中, 随着粒子数目的增多, 用时也是逐渐增加.在RMSE方面, DLUKF算法比另外7种方法小很多.在UPF算法中, 随着粒子数增多, RMSE也是逐渐变小的.但当500个粒子点时, UPF算法RMSE依然是DLUKF算法的两倍.这说明了基于带权值的采样点表征后验分布的方法是优于随机点表征后验分布的.
4.2 多维仿真
考虑一个二维匀速直线运动的例子, 其状态方程和量测方程分别为:
$ \begin{equation} {\mathit{\boldsymbol{{\rm{X}}}}_{k + 1}} = \mathit{\boldsymbol{F}}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{w}}_k} \end{equation} $
(30) $ \begin{equation} {\mathit{\boldsymbol{Z}}_{k + 1}} = \mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}_{k + 1}}) + {\mathit{\boldsymbol{v}}_{k + 1}} \end{equation} $
(31) 式(30)中, ${{\mathit{\boldsymbol{X}}_k} = [{x_k}, {\dot x_k}, {y_k}, {\dot y_k}]^{\rm T}}$是状态变量, 分别表示$x$轴和$y$轴方向的位置和速度. ${\mathit{\boldsymbol{w}}_k}$为过程噪声, 其服从均值为0, 方差为$\mathit{\boldsymbol{Q}}$的高斯分布.其中, $\mathit{\boldsymbol{F}}$和$\mathit{\boldsymbol{Q}}$分别可以表示为
$ \begin{equation} \mathit{\boldsymbol{F}} = \left[ {\begin{array}{*{20}{c}} 1&T&0&0\\ 0&1&0&0\\ 0&0&1&T\\ 0&0&0&1 \end{array}} \right] \end{equation} $
(32) $ \begin{equation} \mathit{\boldsymbol{Q}} = {q^2}\left[ {\begin{array}{*{20}{c}} {\frac{T^3}{3}}&{\frac{T^2}{2}}&0&0\\ {\frac{T^2}{2}}&T&0&0\\ 0&0&{\frac{T^3}{3}}&{\frac{T^2}{2}}\\ 0&0&{\frac{T^2}{2}}&T \end{array}} \right] \end{equation} $
(33) 式(31)中, ${\mathit{\boldsymbol{Z}}_{k + 1}} = {[{r_{k + 1}}, {\theta _{k + 1}}]^{\rm T}}$为观测变量, 分别表示对目标的径向距和方位角. ${\mathit{\boldsymbol{v}}_{k + 1}}$为量测噪声, 其为闪烁噪声, 可以表示为:
$ \begin{align} p({\mathit{\boldsymbol{v}}_{k + 1}}) = &(1 - \varepsilon ){p_1}({\mathit{\boldsymbol{v}}_{k + 1}}) + \varepsilon {p_2}({\mathit{\boldsymbol{v}}_{k + 1}}) = \nonumber\\& (1 - \varepsilon )N({\mathit{\boldsymbol{v}}_{k + 1}};0, {\mathit{\boldsymbol{R}}_1}) + \varepsilon N({\mathit{\boldsymbol{v}}_{k + 1}};0, {\mathit{\boldsymbol{R}}_2}) \end{align} $
(34) 量测方程$\mathit{\boldsymbol{h}}(\cdot) $可以表示为:
$ \begin{equation} \mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}_{k + 1}}) = {\left[ {\begin{array}{*{20}{c}} {\sqrt {x_{k + 1}^2 + y_{k + 1}^2} }&{\arctan (\frac{{{y_{k + 1}}}}{{{x_{k + 1}}}})} \end{array}} \right]^{\rm{T}}} \end{equation} $
(35) 式(34)中, ${\mathit{\boldsymbol{R}}_1}$和${\mathit{\boldsymbol{R}}_2}$分别可以表示为
$ \begin{equation} {\mathit{\boldsymbol{R}}_1} = \left[ {\begin{array}{*{20}{c}} {\sigma _{1r}^2}&0\\ 0&{\sigma _{1\varepsilon }^2} \end{array}} \right] \end{equation} $
(36) $ \begin{equation} {\mathit{\boldsymbol{R}}_2} = \left[ {\begin{array}{*{20}{c}} {\sigma _{2r}^2}&0\\ 0&{\sigma _{2\varepsilon }^2} \end{array}} \right] \end{equation} $
(37) 仿真中, 仿真时间为100 s, 蒙特卡洛仿真次数为300次.目标初始位置为(20 000 m, 40 000 m), 初始速度为(-160 m/s, -150 m/s). IUKF算法、RUEKF算法和RUCKF算法的迭代次数都为10次. UPF算法粒子数量为300个, DLUKF算法产生粒子的方法是对称采样策略.
其他的参数设置为:
表 2 仿真参数设置Table 2 The Simulation parameters参数 $T$ $q$ ${\sigma _{1r}}$ ${\sigma _{1\varepsilon }}$ ${\sigma _{2r}}$ ${\sigma _{2\varepsilon }}$ $\varepsilon $ 数值 1 1 20 m 0.2$^{o}$ 200 m 0.2$^{o}$ 0.1 位置的RMSE公式可以表示为$RMS{E_{}} = \sqrt {RMSE_{x}^2 + RMSE_{y}^2} $, 进行仿真分析, 其效果如图 3所示.
图 3是各个算法在位置方面的RMSE, 可以看出, RUEKF算法、RUCKF算法、高阶UKF算法、高阶CKF算法和UKF算法的估计效果基本相同, 而IUKF算法和UPF算法的估计效果优于UKF算法.而本文算法性能是最好的, 这是因为本文算法用带权值的采样点表征后验分布, 这比随机的粒子点表征后验分布更有优势, 故DLUKF算法的RMSE是好于其他滤波算法的.这也说明所提的DLIKF算法对匀速直线运动可以有着很好的滤波估计效果.
在匀速直线运动中, 将UPF算法的粒子数由300逐渐增加到1 000, 其与其他算法的单次运行时间以及位置和速度的平均RMSE如表 3所示.
表 3 各个算法的性能Table 3 The performance of each algorithm算法 运行时间(s) 平均RMSE UKF 0.0059 99.8709 IUKF 0.0424 85.0107 RUEKF 0.0150 100.2616 RUCKF 0.0397 99.8704 高阶UKF 0.0193 100.4763 高阶CKF 0.0191 99.7558 UPF (300) 3.5953 88.2638 UPF (400) 4.8406 86.5004 UPF (500) 6.0552 85.8206 UPF (600) 7.2596 85.1056 UPF (700) 8.4211 84.6700 UPF (800) 9.6178 83.2706 UPF (900) 10.8389 82.9057 UPF (1 000) 12.0105 82.4258 DLUKF 0.0757 78.5559 从表 3可以看出, 本文算法的运算时间虽然略长于UKF算法、IUKF算法、REUKF算法、RUCKF算法, 高阶UKF算法和高阶CKF算法, 却远远小于UPF算法.且由于DLUKF算法的外层UKF算法选取了9个确定性采样点, 所以其运算时间是大约是UKF算法的9倍.在UPF算法中, 随着粒子数目的增多, 运算时间也是逐渐增加的.在各个方面的RMSE、DLUKF算法都是最好的.在UPF算法中, 随着粒子数目的增多, RMSE也是逐渐减小的, 但比起DLUKF算法、UPF算法的RMSE依然是很大的.这说明基于双层采样的的DLUKF算法在多维目标跟踪中有着很好的滤波估计效果.
5. 结论
本文所提的DLUKF算法是在双层UKF算法的基础上, 用采样策略选取带权值的采样点, 而后用内层UKF算法对每个采样点进行更新, 同时用最新的量测对采样点的权值进行更新, 最后通过外层UKF算法的更新机制得到每个时刻的滤波估计值.仿真结果表明, 在一维和二维的仿真场景中, 相比于存在的经典算法, 本文所提的DLUKF算法可以在较短的时间内获得很好的滤波估计效果.
-
表 1 各算法计算时间及RMSE对比分析表
Table 1 The calculation time and RMSE of each algorithm
算法 运行时间(s) 平均RMSE UKF 0.0002 0.1566 IUKF 0.0014 0.0881 RUEKF 0.0006 0.0378 RUCKF 0.0031 0.0337 高阶UKF 0.0006 0.1434 高阶CKF 0.0006 0.1437 UPF (100) 0.1032 0.1153 UPF (200) 0.2097 0.0714 UPF (300) 0.3200 0.0626 UPF (400) 0.4296 0.0564 UPF (500) 0.5416 0.0476 DLUKF 0.0016 0.0297 
下载: 导出CSV
表 2 仿真参数设置
Table 2 The Simulation parameters
参数 $T$ $q$ ${\sigma _{1r}}$ ${\sigma _{1\varepsilon }}$ ${\sigma _{2r}}$ ${\sigma _{2\varepsilon }}$ $\varepsilon $ 数值 1 1 20 m 0.2$^{o}$ 200 m 0.2$^{o}$ 0.1
下载: 导出CSV
表 3 各个算法的性能
Table 3 The performance of each algorithm
算法 运行时间(s) 平均RMSE UKF 0.0059 99.8709 IUKF 0.0424 85.0107 RUEKF 0.0150 100.2616 RUCKF 0.0397 99.8704 高阶UKF 0.0193 100.4763 高阶CKF 0.0191 99.7558 UPF (300) 3.5953 88.2638 UPF (400) 4.8406 86.5004 UPF (500) 6.0552 85.8206 UPF (600) 7.2596 85.1056 UPF (700) 8.4211 84.6700 UPF (800) 9.6178 83.2706 UPF (900) 10.8389 82.9057 UPF (1 000) 12.0105 82.4258 DLUKF 0.0757 78.5559
下载: 导出CSV
-
[1] Song W J, Yang Y, Fu M Y, Kornhauser A, Wang M L. Critical rays self-adaptive particle filtering SLAM. Journal of Intelligent and Robotic Systems, 2018, 92(1):107-124 [2] Zhang T, Zhu Y Y, Zhou F, Yan Y X, Tong J W. Coarse alignment technology on moving base for SINS based on the improved quaternion filter algorithm. Sensors, 2017, 17(6):1424 doi: 10.3390/s17061424 [3] 朱书军, 刘伟峰, 崔海龙.基于广义标签多伯努利滤波的可分辨群目标跟踪算法.自动化学报, 2017, 43(12):2178-2189 http://www.aas.net.cn/CN/abstract/abstract19191.shtmlZhu Shu-Jun, Liu Wei-Feng, Cui Hai-Long. Multiple resolvable groups tracking using the GLMB filter. Acta Automatica Sinica, 2017, 43(12):2178-2189 http://www.aas.net.cn/CN/abstract/abstract19191.shtml [4] 胡艳艳, 金增旺, 薛晓玲, 孙长银.基于异步IMM融合滤波的网络化系统故障诊断.自动化学报, 2017, 43(8):1329-1338 http://www.aas.net.cn/CN/abstract/abstract19107.shtmlHu Yan-Yan, Jin Zeng-Wang, Xue Xiao-Ling, Sun Chang-Yin. Fault diagnosis for networked systems by asynchronous IMM fusion filtering. Acta Automatica Sinica, 2017, 43(8):1329-1338 http://www.aas.net.cn/CN/abstract/abstract19107.shtml [5] 李军, 万文军, 胡康涛.一种基于点频滤波器的微分信号提取方法.自动化学报, 2017, 43(3):478-486 http://www.aas.net.cn/CN/abstract/abstract19026.shtmlLi Jun, Wan Wen-Jun, Hu Kang-Tao. A new method for extraction of process difierential signal based on single-frequency-pass fllter. Acta Automatica Sinica, 2017, 43(3):478-486 http://www.aas.net.cn/CN/abstract/abstract19026.shtml [6] 申屠晗, 薛安克, 周治利.多传感器高斯混合PHD融合多目标跟踪方法.自动化学报, 2017, 43(6):1028-1037 http://www.aas.net.cn/CN/abstract/abstract19078.shtmlShen Tu-Han, Xue An-Ke, Zhou Zhi-Li. Multi-sensor Gaussian mixture PHD fusion for multi-target tracking, Acta Automatica Sinica, 2017, 43(6):1028-1037 http://www.aas.net.cn/CN/abstract/abstract19078.shtml [7] Kalman R E. A new approach to linear filtering and prediction problems. Journal of Basic Engineering Transactions, 1960, 82:35-45 doi: 10.1115/1.3662552 [8] Arulampalam M S, Maskell S, Gordon N, Clapp, T. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing, 2002, 50(2):174-188 doi: 10.1109/78.978374 [9] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 2004, 92(3):401-422 doi: 10.1109/JPROC.2003.823141 [10] 潘泉, 杨峰, 叶亮, 梁彦, 程咏梅.一类非线性滤波器——UKF综述.控制与决策, 2005, 20(5):481-489 doi: 10.3321/j.issn:1001-0920.2005.05.001Pan Quan, Yang Feng, Ye Liang, Liang Yan, Cheng Yong-Mei. Survey of a kind of nonlinear filters——UKF. Control and Decision, 2005, 20(5):481-489 doi: 10.3321/j.issn:1001-0920.2005.05.001 [11] Peng S, Chen C, Shi H, Yao Z. State of charge estimation of battery energy storage systems based on adaptive unscented Kalman filter with a noise statistics estimator. IEEE Access, 2017, 13202-13212 [12] Merwe R V D, Wan E A. The square-root unscented Kalman filter for state and parameter-estimation. IEEE International Conference on Acoustics, Speech, and Signal Processing, 2001. 3461-3464 doi: 10.1111-j.1365-2559.2010.03597.x/ [13] 占荣辉, 郁春来, 万建伟.简化UKF算法在单站无源目标跟踪中的应用.现代雷达, 2007, 29(3):42-46 doi: 10.3969/j.issn.1004-7859.2007.03.012Zhan Rong-Hui, Yu Chun-Lai, Wan Jian-Wei. Aplication of simplified UKF algorithm to single observer passive target tracking. Modern Radar, 2007, 29(3):42-46 doi: 10.3969/j.issn.1004-7859.2007.03.012 [14] Zhan R, Wan J. Iterated unscented Kalman filter for passive target tracking. Aerospace and Electronic Systems IEEE Transactions on, 2007, 43(3):1155-1163 doi: 10.1109/TAES.2007.4383605 [15] Zanetti. Recursive update filtering for nonlinear estimation. IEEE Transactions on Automatic Control, 2012, 57(6):1481-1490 doi: 10.1109/TAC.2011.2178334 [16] Huang Y L, Zhang Y G, Li N, Zhao L. Design of sigma-point Kalman filter with recursive updated measurement. Circuits Systems and Signal Processing, 2016, 35(5):1767-1782 doi: 10.1007/s00034-015-0137-y [17] Lerner U N. Hybrid Bayesian Networks for Reasoning about Complex Systems.[Ph.D. thesis], Stanford University, USA, 2002. [18] 张勇刚, 黄玉龙, 武哲民, 李宁.一种高阶无迹卡尔曼滤波方法.自动化学报, 2014, 40(5):838-848 http://www.aas.net.cn/CN/abstract/abstract18352.shtmlZhang Yong-Gang, Huang Yu-Long, Wu Zhe-Min, Li Ning. A high order unscented Kalman filtering method. Acta Automatica Sinica, 2014, 40(5):838-848 http://www.aas.net.cn/CN/abstract/abstract18352.shtml [19] Jia B, Xin M, Cheng Y. High-degree cubature Kalman filter. Automatica, 2013, 49(2):510-518 doi: 10.1016/j.automatica.2012.11.014 [20] Zhang Y, Huang Y, Li N, Zhao L. Embedded cubature Kalman filter with adaptive setting of free parameter. Signal Processing, 2015, 114(C):112-116 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=1baebea928940e67256ccd3de6879aed [21] Zhang Y, Huang Y, Li N, Zhao L. Interpolatory cubature Kalman filters. Control Theory and Applications Iet, 2015, 9(11):1731-1739 doi: 10.1049/iet-cta.2014.0873 [22] Merwe R V D, Doucet A, Freitas N D, Wan E. The unscented particle filter. International Conference on Neural Information Processing Systems. MIT Press, 2000: 563-569 [23] Wang X, Li T, Sun S, Corchado J M. A survey of recent advances in particle filters and remaining challenges for multi-target tracking, Sensors, 2017, 17(12):2707 doi: 10.3390/s17122707 -
下载:
计量
- 文章访问数: 3342
- HTML全文浏览量: 476
- PDF下载量: 714
- 被引次数: 0
