Professional Documents
Culture Documents
非线性自适应平方根无迹卡尔曼滤波方法研究 张玉峰
非线性自适应平方根无迹卡尔曼滤波方法研究 张玉峰
非线性自适应平方根无迹卡尔曼滤波方法研究
张玉峰 1,周奇勋 1,周 勇 2,张举中 3
ZHANG Yufeng1, ZHOU Qixun1, ZHOU Yong2, ZHANG Juzhong3
ZHANG Yufeng, ZHOU Qixun, ZHOU Yong, et al. Research on adaptive square-root unsented Kalman filter for
nonlinear system. Computer Engineering and Applications, 2016, 52(16):36-40.
Abstract:In this paper, a Nonlinear Adaptive Square-Root Unsented Kalman Filtering(NASRUKF)approach is described
for nonlinear systems with additive noise which have unknown statistical characteristics. Based on the square-root algo-
rithm, the traditional Sage-Husa adaptive filter’s estimator is modified and combinated with the Square Root Unscented
Kalman Filtering(SRUKF)for nonlinear filtering. The process noise covariance matrix Q or the measurement noise cova-
riance matrix R is estimated straightforwardly in proposed NASRUKF. Thus, the positive semidefiniteness and symmetri-
cal properties of the filter are improved. Simulation results show that NASRUKF performs better than SRUKF in the
aspects of the accuracy, stability and self-adaptability.
Key words:Nonlinear Adaptive Square-Root Unsented Kalman Filtering(NASRUKF); Kalman filtering; Square Root
Unscented Kalman Filtering(SRUKF); Sage-Husa filtering; nonlinear filtering; estimating
摘 要:针对带有附加噪声且噪声特性未知的系统,提出了一种非线性卡尔曼滤波方法——自适应平方根无迹卡尔
曼滤波(NASRUKF)方法,该方法基于平方根滤波的思想,对传统的 Sage-Husa 自适应滤波算法进行了改进,并与平
方根无迹卡尔曼滤波(SRUKF)算法相结合用来进行非线性滤波。该算法能直接对非线性系统的状态方差阵和噪声
方差阵的平方根进行递推与估算,确保状态和噪声方差阵的对称性和非负定性。将所提方法通过计算机仿真技术
与 SRUKF 算法进行对比,结果表明 NASRUKF 方法在滤波精度、稳定性和自适应能力方面均优于 SRUKF 方法。
关键词:非线性自适应平方根无迹卡尔曼滤波方法(NASRUKF);卡尔曼滤波;平方根无迹卡尔曼滤波(SRUKF);
Sage-Husa 滤波;非线性滤波;预估
文献标志码:A 中图分类号:TP274 doi:10.3778/j.issn.1002-8331.1603-0144
1 引言 略去二阶以上高阶项来得到非线性系统的线性化模型,
目前在伺服系统控制、飞行器定姿等领域应用广泛 它是一种次优滤波,并且只能在滤波误差及一步预测误
的非线性滤波方法有扩展卡尔曼滤波(Expended Kalman 差较小时才能适用 [1-3]。UKF 与 DKF 同属于 Sigma 类滤
Filter,EKF)、无迹卡尔曼滤波(Unscented Kalman Filter, 波器,是构造一组可调权重的采样点,并通过非线性函
UKF)、
差分卡尔曼滤波(Difference Kalman Filter,
DKF) 数传播来估计系统状态的均值和协方差。UKF、DKF 滤
等。其中 EKF 滤波是将非线性函数用泰勒级数展开,并 波在实际应用时需要精心调节自由参数才能获得较好
基金项目:国家自然科学基金(No.51307137);西安科技大学培育基金项目(No.201317)。
作者简介:张玉峰(1977—),男,博士,讲师,研究领域为伺服控制技术,E-mail:xkdzhangyufeng@xust.edu.cn;周奇勋(1979—),
男,博士,副教授,研究领域为永磁电机及其控制技术;周勇(1978—),男,博士,副教授,研究领域为故障诊断、容错技术;
张举中(1978—),男,博士,高级工程师,研究领域为伺服控制技术。
收稿日期:2016-03-09 修回日期:2016-04-25 文章编号:1002-8331(2016)16-0036-05
张玉峰,周奇勋,周 勇,等:非线性自适应平方根无迹卡尔曼滤波方法研究 2016,52(16) 37
线性系统的状态方差阵、过程噪声方差阵和测量噪声方 估测过程噪声统计特性:
差阵的平方根进行推算,确保非负性和对称性,提高非 Q̂ (k) = (1 - d(k))Q̂ (k - 1) + d(k)(K (k)z͂ (k)z͂ (k)T K (k)T +
线性系统滤波的精确性、稳定性、实时性和自适应性。
P(k) - Φ k k - 1 P(k - 1)Φ Tk k - 1) (8)
k+1
其中 d(k) = (1 - b)/(1 - b 通常 0 < b < 1 。
) ,b 为遗忘因子,
2 算法原理
由式(4)可知,Sage-Husa 自适应算法在利用测量数据进
假定随机线性离散系统的状态方程和测量方程分
行递推滤波的同时,实时估计和修正系统过程噪声和测
别为:
量噪声的统计特性,从而达到自适应滤波的目的。该方
x(k) = Φ k k - 1 x(k - 1) + B k k - 1u(k - 1) + w(k - 1) (1)
法原理简单,实时性好,因此在许多领域得到推广应用。
z(k) = H (k)x(k) + v(k) (2) 然而,Sage-Husa 自适应滤波器也存在一些缺陷,主
其中 x 是系统的 L 维状态向量,Φ k k - 1 是系统的 L ´ L 要表现为以下两点:
阶状态转移矩阵,u 是系统的 r 维控制输入向量,B k k - 1 (1)对系统不能完全保证收敛和稳定,有可能随着
是 L ´ r 阶控制增益矩阵,w 是 L 维系统过程噪声序列, 噪声方差阵 Q 和 R 失去半正定性和正定性而出现滤波
z 是系统的 m 维观测向量,H (k) 是 m ´ L 阶观测矩阵,v 发散现象;
是 m 维观测噪声序列。此外,w 和 v 满足均值为零、相 (2)不能同时估计出真实的系统过程噪声方差阵和
互独立和服从高斯分布的统计特性,即 测量噪声方差阵,只能在 Q(或 R )已知的条件下,估计
T
E[w(k)] = 0 E[w(k)w( j) ] = Q(k)δ kj 出 R(或 Q )。
2.2 NASRUKF 算法
E[v(k)] = 0 E[v(k)v( j)T] = R(k)δ kj
传统的 Sage-Husa 自适应滤波算法是针对线性离散
E[w(k)v( j)T] = 0 (3)
系统而设计的,目前关于其在非线性系统滤波中的应用
式中 Q 是系统过程噪声 w 的 L ´ L 阶对称非负定方差
研究还相对较少。结合平方根无迹卡尔曼滤波(SRUKF)
矩阵,R 是观测噪声 v 的 m ´ m 阶对称正定方差矩阵, 算 法 [12] ,本 文 对 Sage-Husa 自 适 应 滤 波 算 法 进 行 了 改
δ kj 是 Kronecker- δ 函数。 进 ,将 其 推 广 到 非 线 性 系 统 ,提 出 了 一 种 新 型 的 非 线
2.1 Sage-Husa 自适应滤波算法 性 自 适 应 平 方 根 无 迹 卡 尔 曼 滤 波(Nonlinear Adaptive
对 于 式(1)和(2)所 描 述 的 线 性 离 散 系 统 ,Sage 和 Square-Root Unscented Kalman Filtering,NASRUKF)
Husa 提出了基于极大后验估计的自适应滤波算法,邓 算法。
38 2016,52(16) Computer Engineering and Applications 计算机工程与应用
ïï {
ì R** = cholupdate 1 - d(k) R̂ (k - 1) z͂ (k) d(k)
| | } ①将公式(12)和(13)用公式(24)代替;
z͂ (k) = z(k) - H (k)x̂ - (k) (24)
ïï * **
{
í R = cholupdate R Ẑ 0:2L (k) - ẑ (k) -d(k)W i
- - (c)
(14) } ②将公式(14)用公式(25)代替;
ïï
ïï ̂
{ *
î R(k) = diag diag( R R )
*
T
} ï {
ì R** = cholupdate 1 - d(k) R̂ (k - 1) z͂ (k) d(k)
| | }
计算滤波增益:
ï * **
{ -
í R = cholupdate R H (k)S (k) -d(k) } (25)
{ }
2L
ï T
P xz (k) = åW i ( χ̂ i- (k) - x̂ - (k))(Ẑ i- (k) - ẑ - (k))T
(c)
(15) ï R̂ (k) = diag diag( R* R* )
î
i=0
{ }
③将公式(15)用公式(26)代替;
ìS (k) = qr é (c) ̂ - - ̂ ù
ïï z ë W1 (Z1:2L (k) - ẑ (k)) R(k)û P xz (k) = S - (k)S - (k)T H (k)T (26)
í (16)
î z z {[
ïïS (k) = cholupdate S (k) Ẑ - (k) - ẑ - (k) W (c)
0 0 ]} ④将公式(16)用公式(27)代替。
利 用 SRUKF 和 NASRUKF 算 法 分 别 对 状 态 x 进 行 估 5
0
迭代运行 100 次,
仿真用时分别为 0.013 6 s 和 0.142 s,
x
计,
-5
仿真结果如图 1 和图 2 所示。
-10
15 -15
real SRUKF NASRUKF 0 10 20 30 40 50 60 70 80 90 100
10 k
5 5.0
2.5 SRUKF NASRUKF
0
x
error
-5 0
-10 -2.5
-15 -5.0
0 10 20 30 40 50 60 70 80 90 100 0 10 20 30 40 50 60 70 80 90 100
k k
10 图3 状态跟踪波形图( σ w 已知,σ v 未知)
5
error
0 1.4
estimate
-5 1.2 real
standard deviation of
SRUKF NASRUKF
measurement noise
-10 1.0
0 10 20 30 40 50 60 70 80 90 100
k 0.8
图1 状态跟踪波形图( σ w 未知,σ v 已知) 0.6
4 0.4
estimate 0.2
standard deviation of
real
3 0
process noise
10 20 30 40 50 60 70 80 90 100
k
2
图4 σ w 估计曲线图( σ w 已知,σ v 未知)