0 引言

非线性滤波的核心任务是对状态的后验概率密度函数(PDF)进行计算^[1], 基于“对概率分布进行近似要比对非线性函数进行近似要容易”的认识, 发展出包括无迹卡尔曼滤波(UKF)^[2]、差分滤波(DDF)^[3]、高斯-埃尔米特卡尔曼滤波(GHKF)^[1]、容积卡尔曼滤波^[4]、高阶CKF(HCKF)^[5]、广义高阶CKF(GHCKF)^[6]等在内的多种次优非线性滤波方法.其中, 文献[6]提出的GHCKF在获得更高滤波精度的同时, 进一步克服了HCKF结构复杂、高阶扩展性差的问题.

统计学意义下, 随机变量服从非高斯分布时, 包括GHCKF在内的基于l₂范数最小的估计方法的估计效果会变差.为解决随机变量的非高斯分布问题, 结合Huber方法^[7], Karlgaard等^[8]从DDF的统计线性回归观点^[9]提出了基于Huber方法的鲁棒DDF. Wang等^[10]研究了基于Huber估计的UKF (HUKF)在视频相对导航中的应用.黄玉等^[11]基于Huber估计原理推导了线性化近似回归估计和直接非线性回归的鲁棒CKF滤波算法.张文杰等^[12]通过统计线性回归模型来近似系统的非线性量测模型, 提出了基于Huber的HCKF目标跟踪算法. Lefebvre等^[9]指出, 无迹变换(UT)是在方差传递时考虑了线性化误差补偿的统计线性回归, 如果用回归得到的线性模型进行方差的传递, 而不是如UT或者容积变换(CT)一样考虑线性化误差补偿, 则会低估传递方差, 影响滤波精度.

针对上述问题, 本文从近似贝叶斯估计角度解释Huber方法作用于卡尔曼滤波的机理, 利用Huber方法对观测量进行预处理, 通过标准GHCKF的量测更新算法对非线性观测方程进行滤波, 无需对非线性观测方程进行线性化近似, 在GHCKF框架内实现算法的鲁棒化.所提出算法与基于统计线性化认识的鲁棒GHCKF相比, 滤波精度和鲁棒性都得到有效改善.单变量非平稳增长模型和再入飞行器目标跟踪问题的应用, 验证了所提出算法的良好估计性能.

1 广义高阶容积卡尔曼滤波算法

考虑如下形式的离散非线性系统:

(1)

其中: x_k∈Rⁿ和z_k∈R^m分别为系统状态向量和量测向量; ω_k-1和v_k分别为独立的零均值系统状态高斯白噪声和量测高斯白噪声, 且分别满足E[ω_kω_l^T]=Q_kδ_kl和E[v_kv_l^T]=R_kδ_kl, δ_kl是Kronecker-δ函数; 初始状态x₀与ω_k和v_k无关.

非线性滤波难点在于求解形如

(2)

的积分问题, 和P为状态变量x的估计值和方差.

传统CKF中正的容积点权值会导致估计点越界或者产生形式复杂的估计点, 而且当n足够大时, 容积点可能超出积分区域^[13]. HCKF将球面-径向积分进行高阶拓展需同时提升容积准则和高斯-拉盖尔准则的阶数, 并计算n维超球体的面积分, 计算过程复杂, 导致CKF算法的高阶扩展性较差.为克服上述问题, Zhang^[6]从数值积分的角度出发, 提出了形式简洁、计算高效、扩展性更好的GHCKF.

考虑实数域Rⁿ上积分

(3)

对式(3)进行计算, 选取第一、第二及第三G-轨迹^[13], 构成广义五阶容积积分公式^[14], 即

(4)

其中: 是相对于G-轨迹[0]、[u₁]和[u₁, u₁]的权值, n为系统状态维数.利用式(4)计算{1, x_i², x_i⁴, x_i²x_j²|i ≠j}的积分, 有

(5)

其中

方程组(5)中, 仅有u₁、四个未知, 求解可得唯一解

(6)

将式(4)变换至标准高斯分布, 可得

(7)

其中:容积点ξ_i和权值ω_i分别为

(8)

(9)

将式(7)和(8)置于高斯滤波框架下, 即可得到标准GHCKF算法.通过选取更高阶的G-轨迹, 可构造出任意阶的CKF算法.与其他高斯滤波方法相似, GHCKF由时间更新和量测更新两部分组成, 区别在于采样点以及采样点权值不同.对比文献[5]和文献[6]可以发现, HCKF和GHCKF的采样点相同, 但HCKF采样点的系数为在高维问题中易产生非局部采样问题, 而GHKCF采样点的系数为常值避免了上述问题.

2 基于Huber方法的鲁棒GHCKF 2.1 卡尔曼滤波的近似贝叶斯估计解释

考虑线性统计状态空间模型

(10)

其中: F_k-1和H_k为状态转移矩阵和观测矩阵, w_k-1 ~ N(w; 0, Q_k−1)和v_k~ N(v; 0, R_k)为状态噪声和观测噪声, x_k∈Rⁿ和y_k∈R^m为k时刻的状态和观测.令表示{y_j, j < l}观测信息下x_k的估计, P_k|l是对应的方差.初始状态x₀与w_k和v_k无关.

由贝叶斯准则及状态概率分布模型的Markov特性可知

(11)

(12)

其中: p(x_k|y_1:k)为k时刻状态的后验概率密度, p(x_k|y_1:k-1)由状态的先验概率密度p(x_k|y_1:k-1)、似然概率密度p(y_k|x_k)以及归一化常数p(y_k|y_1:k-1)组成.令p(x_k|y_1:k-1)=

将式(12)代入(11), 得

(13)

由于卡尔曼滤波中状态预测符合高斯分布, 即

(14)

对式(14)两边求导, 可得

(15)

将式(15)代入(13), 可得

(16)

似然概率密度为观测值与其预测值之差的函数, 即

(17)

将式(17)代入(16), 可得

(18)

因观测是独立同分布的, 故

(19)

综合式(18)和(19), 可得

(20)

式(20)可写为如下形式:

(21)

定义新息及其方差S_k, 定义变换新息η_k=S_k^-1/2s_k, 得到

(22)

综合式(21)和(22), 可得

(23)

令

(24)

将式(24)代入(23), 可得

(25)

由于观测和新息独立同分布, 有

(26)

其中η_{k, i}为η_k的第i项.将式(26)代入(24), 可得

(27)

定义如下函数:

(28)

(29)

令Θ_k=diag[(φ(η_{k, i}))], 则有

(30)

假设变换新息符合高斯分布, 则式(30)可简化为

(31)

在高斯分布假设下

(32)

将式(32)代入(25), 得

(33)

可以发现, 式(33)是卡尔曼滤波中的状态更新方程.在上述推导过程中假设新息符合高斯分布, 则当观测信息含有干扰噪声或野值时, 新息符合高斯分布的假设不再满足, 从而导致滤波效果降低.

2.2 基于近似贝叶斯估计的卡尔曼滤波鲁棒化

Huber提出了一种代价函数

(34)

其中γ为调节因子.

Aravkin等指出, 存在变量 ς∈R^m和 ζ∈R^m使得exp[-ρ(ζ_it)]是一概率密度.变量ζ可由下式计算:

(35)

其中: erf表示误差函数, 其公式为

(36)

变量 ς不影响后续讨论.

基于Huber代价函数的变换新息概率密度为

(37)

则有

(38)

定义如下函数:

(39)

(40)

则有

(41)

其中sgn(n)表示符号函数.令Θ_k=diag[φ(η_{k, i})], 并将式(27)重写为

(42)

将式(42)代入(25), 可得

(43)

Masreliez等已经研究过此类估计问题的方差求解方法, 并指出后验状态方差可表示为

(44)

其中: ℓ(k)是一标量, 可由下式计算得到:

(45)

0≤ε≤1, Φ是标准高斯分布函数.

对比式(33)和(43)可以发现, 鲁棒化的状态估计方程只在新息前增加一个权矩阵, Huber方法对卡尔曼滤波进行鲁棒化的本质是对新息进行截断平均.基于这一认识, 将Huber方法引入GHCKF算法中, 构造基于Huber的鲁棒GHCKF.

2.3 基于新息修正的鲁棒GHCKF

记以及, 则式(43)和(44)可以表示为

(46)

(47)

由上式可见, Θ_k和ℓ(k)并未影响观测信息的统计量基于新息修正的鲁棒化方法无需对GHCKF中的CT过程作任何近似, 只需修正相应的信息即可.

下面针对利用观测信息设计鲁棒化的GHCKF算法, 对于非线性统计状态空间模型(1), 基于新息修正的鲁棒GHCKF滤波过程如下.

假设k-1时刻的状态向量x满足统计特性, 则基于新息修正的GHCKF具体算法如下:

1) 基于GHCKF时间更新获得的状态均值和方差进行sigma点重采样, 有

(48)

2) 将X_{i, k|k-1}在非线性观测方程中传递, 即

(49)

3) 根据CT方法计算观测预测均值、方差及其与状态的协方差, 即

(50)

(51)

(52)

4) 计算变换新息η_k, 即

(53)

5) 计算变换新息φ(η_{k, i}), 有

(54)

(55)

(56)

其中: Huber代价函数中的调节因子γ设置为γ=1.345^[12], 令Θ_k=diag[φ(η_{k, i})].

6) 计算ℓ(k), 即

(57)

7) 进行均值和方差估计, 即

(58)

(59)

可以看出, 本文提出的鲁棒GHCKF算法, 没有如文献[12]一样对观测方程进行线性化近似, 从而保持了CT在方差传递中原有的精度优势.

3 仿真验证

分别比较以下3种算法:传统的GHCKF算法^[6]、基于统计线性化认识的Huber鲁棒GHCKF算法(记为HGHCKF1)和本文提出的基于近似贝叶斯估计认识的Huber鲁棒GHCKF算法(记为HGHCKF2).

3.1 单变量非平稳增长模型

单变量非平稳增长模型的系统方程如下:

其中:系统噪声w_k-1~N(w; 0, 1), 仿真时间K=500, 仿真时用于产生仿真数据的初始真值x₀=0.1.观测噪声服从如下形式的干扰高斯分布:

其中: α为干扰因子, 设为0.3; λ是干扰高斯分布方差相对于主高斯分布方差的比重.仿真实验中, 令σ₁=1, λ依次设置为1 ~ 20之间的整数.通过以下指标比较3种滤波算法的性能:

(60)

(61)

M=50表示Monte Carlo仿真次数.初始滤波条件设为 Huber代价函数的调节因子设为γ=1.345. 3种滤波算法的MMSE如图 1所示.

由图 1可以看出:当λ=1时(观测噪声服从高斯分布)GHCKF精度最高, 说明高斯分布条件下基于l₂范数最小的估计方法具有更高的精度, 而基于l₁/l₂混合代价函数最小的估计方法精度降低.随着λ的增大(干扰高斯分布的影响增大), GHCKF估计精度明显下降, 说明统计学意义下基于l₂范数最小的估计方法不具有鲁棒性.而两种基于Huber方法的鲁棒GHCKF算法的精度基本上不受干扰高斯分布的影响, 具有良好的鲁棒性.同时HGHCKF2相对于HGHCKF1具有明显的精度优势, 这是因为HGHCKF1在利用GHCKF的统计线性化方法时忽略了线性化误差补偿量, 丢失了部分滤波精度, 而HGHCKF2充分利用了GHCKF的精度优势, 算法精度更高.

为了进一步说明干扰高斯分布下HGHCKF2相对于GHCKF和HGHCKF1的精度优势, 令σ₁=1, λ=15, 并使用式(61)的指标比较3种滤波算法的估计精度, 3种滤波算法的MSE如图 2所示.

可以看出, 在干扰高斯分布条件下, HGHCKF2相对于GHCKF和HGHCKF1具有更高的估计精度, 进一步表明了本文所提出算法的有效性.

3.2 再入飞行器目标跟踪

再入飞行器目标跟踪问题是一个强非线性状态估计问题.状态向量x∈R⁵由位置(x₁, x₂)、速度(x₃, x₄)以及一个飞行器系数(x₅)组成, 目标的动力学方程表示如下:

其中: D(t)是与拖力相关的项, G(t)是与引力相关的项, ω(t)=[ω₁(t) ω₂(t) ω₃(t)]^T为系统噪声.拖力项和引力项可以表示如下:

其中: 为飞行器与地球中心之间的距离, 为飞行器的飞行速率.仿真实验中, 参数设置R₀=6 374 km, β(0) =-0.597 83, G_m0=3.986 0×10⁵ km³/s², H₀=13.406 km.系统噪声ω(t)为零均值白噪声, 其协方差矩阵Q=diag([2.406 4× 10⁻⁵, 2.4064×10⁻⁵, 0].

位于(x_r, y_r)的雷达每隔1 s输出一次飞行器相对于雷达的距离和方位, 即

其中: x_r=6 374 km, y_r=0 km, v(t)=[v₁(t), v₂(t)]^T为量测高斯噪声, 方差R= diag([10^-3 km², 0.17 m · rad²]), 初始协方差P₀=diag([10^-6, 10^-6, 10^-6, 10^-6, 0)]; 真实状态x₀=[6 500.4, 349.14, -1.809 3, -6.796 7, 0.693 2]^T, 协方差矩阵P_0|0= diag ([10^-6, 10^-6, 10^-6, 10^-6, 1]).仿真中滤波初始值设为x₀=[6 500.4, 349.14, -1.809 3, -6.796 7, 0.693 2]^T, 假设观测噪声服从如下形式的干扰高斯分布:

其中: unidrnd(20)表示1 ~ 20之间的随机整数, 仿真实验中, 令α=0.3.

使用位置、速度及飞行器系数RMSE和MMSE比较不同滤波器的估计性能.定义第t时刻位置的RMSE和MMSE为

(62)

(63)

(64)

其中: M=50为Monte Carlo次数; xⁿ(t)和分别表示在第n次Monte Carlo运行时, 第t时刻真实的状态和估计的状态; k为离散时刻.同理, 可以得到速度和飞行器系数的RRMS和MMSE.

图 3 ~图 5给出了50次Monte Carlo仿真实验中3种算法的平均绝对值误差.由于干扰噪声的存在, 基于Huber方法的HGHCKF2和HGHCKF1较传统GHCKF具有明显的精度优势; 同时, 由于GHHCKF2算法充分利用了GHCKF算法中CT变换的优势, 其精度也高于忽略统计线性化误差方差补偿的HGHCKF1算法.

结合文献[14]可以发现, HGHCKF1在k时刻完成一次GHCKF估计后, 进行一次基于Huber方法的线性回归估计, 该过程具有以下两个相反的作用: 1)当观测噪声的实际分布与假设存在一定偏差时, 这一过程起到减小偏差影响的作用, 提高GHCKF的滤波精度. 2)这种统计线性化近似方法会对已有的滤波结果产生污染, 降低了算法的估计精度.即:分布偏差较大时, HGHCKF1主要起到提高滤波精度的作用; 分布偏差较小时, HGHCKF1主要起到降低滤波精度的作用. 3.1节实验证实了上述结论.而GHHCKF2在观测噪声分布存在偏差时, 只起到提高滤波精度的作用, 避免了因线性化近似而带来的对滤波精度的降低作用, 3.2节实验证实了上述结论.

为了进一步比较干扰高斯分布下HGHCKF2相对于GHCKF和HGHCKF1的精度优势, 使用式(64)的指标比较3种滤波算法的精度. 3种滤波算法的MMSE如图 6 ~图 8所示.该结果进一步表明了HGHCKF2的良好估计性能.

4 结论

本文从近似贝叶斯估计角度解释了Huber方法作用于卡尔曼滤波的本质是对新息进行截断平均, 推导了无需对观测方程进行线性化近似的鲁棒GHCKF.所提出方法兼具Huber方法鲁棒性强以及GHCKF方法滤波精度高的特点, 滤波性能优于标准的GHCKF算法以及基于观测方程线性化近似的鲁棒GHCKF.