2013 年 2 月 西北工业大学学报 Feb． 2013

第 31 卷第 1 期 Journal of Northwestern Polytechnical University Vol． 31 No． 1

基于 Sage-Husa 的线性自适应平方根
卡尔曼滤波算法
1 2 1 3
周 勇 ，张玉峰 ，张 超 ，张举中

( 1． 西北工业大学 航空学院，陕西 西安 710072； 2． 西北工业大学 机电学院，陕西 西安 710072

3． 中船重工第 713 研究所，河南 郑州 450000 )
摘 要： 针对标准卡尔曼滤波和扩展卡尔曼滤波存在的局限性，结合平方根滤波的思想，对传统 Sage-
Husa 估计器进行改进，提出了一种新的线性自适应平方根卡尔曼滤波 （ Linear Adaptive Square-Root
LASRKF） 算法。该算法直接对系统状态方差阵和噪声方差阵的平方根进行递推与
Kalman Filtering，
估算，确保了状态和噪声方差阵的对称性和非负定性； 算法还增添了对系统噪声统计特性估计的计
算，
强化了滤波器的稳定性和自适应能力； 与传统 Sage-Husa 自适应滤波算法相比 LASRKF 可提高滤
波器抗发散的能力。仿真实验表明，
LASRKF 可有效提高滤波器的精确性、
稳定性和自适应能力。

关 词： 算法，协方差矩阵，预估，卡尔曼滤波，线性系统，稳定性，LASRKF 算法
键
中图分类号： TP13 文献标识码： A 文章编号： 1000-2758（ 2013） 01-0089-05

目前卡尔曼滤波理论广泛应用于自动控制伺服 1 标准 KF 和 EKF 概述
系统等领域，其中标准卡尔曼滤波（ KF） 和扩展卡尔
曼滤波（ EKF） 都是以已知系统过程噪声和测量噪声 1. 1 标准 KF
的统计特性为应用前提。 然而在实际中，有些参数 假定随机线性离散系统的状态方程和测量方程
事先无法获得，特别是无刷直流伺服系统的过程噪 分别为
声，常伴有执行机构的噪声，有时甚至是时变且具有 x（ k） = Φk，k －1 x（ k － 1） +
很强的不确定性，通常只能通过经验进行估计，但错 B k，k －1 u（ k － 1） + w（ k － 1） （ 1）
［1，
2］
误的参数估计又常常会导致滤波的发散 。
z（ k） = H（ k） x（ k） + v（ k） （ 2）
为了克服这些缺点，一些自适应卡尔曼滤波器
式中 x 是系统的 L 维状态向量，Φk，k －1 是系统的 L × L
（ Adaptive Kalman Filter，AKF） 发展了起来，如基于
［3］ ［4］
阶状态转移矩阵，u 是系统的 r 维控 制 输 入 向 量，
鲁棒的 AKF 、基于神经网络的 AKF 和基于模糊
［5］
B k，k －1 是 L × r 阶控制增益矩阵，w 是 L 维系统过程噪
逻辑的 AKF 等。 在此基础上，本文提出了一种新
声序列，z 是系统的 m 维观测向量，H（ k） 是 m × L 阶
的思路，将采用平方根滤波的思想，对原理简单且实
观测矩阵，v 是 m 维观测噪声序列。
时性好的 Sage-Husa 自适应滤波算法进行改进，提 －

出一种新的线性自适应平方根卡尔曼滤波 （ Linear  x^ （ k） = Φk，k －1 x^ （ k － 1） + B k，k －1 u（ k － 1）

 P － （ k） = Φ T
k，k －1 P（ k － 1） Φk，k －1 + Q（ k － 1）
Adaptive Square-Root Kalman Filtering，LASRKF） 算 
 P － （ k） H（ k） T
法。该算法直接对系统状态方差阵和噪声方差阵的  K（ k） =
 H（ k） P － （ k） H（ k） T + R（ k）
平方根进行递推与估算，确保了状态和噪声方差阵
 x^ （ k） = x^ － （ k） + K（ k） （ z（ k） － H（ k） x^ － （ k） ）
的对称性和非负定性。 
 P（ k） = （ I － K（ k） H（ k） ） P － （ k）
（ 3）

收稿日期： 2012-03-12 基金项目： 国家自然科学基金（ 51207129、

61104030） 资助
作者简介： 周 勇（ 1978—） ，西北工业大学讲师，主要从事电气工程及自动化的研究。
 · 90· 西 北 工 业 大 学 学 报 第 31 卷

由上式知，只要给定初值 x^ （ 0） 和 P（ 0） ，根据 k 2. 1 Sage-Husa 自适应滤波算法

时刻的观测值 z（ k） ，就可递推计算得到 k 时刻的状 对于（ 1） 式和 （ 2） 式所描述的线性离散系统，
态估计值 x^ （ k） （ k = 1，
2，…） 。 Sage 和 Husa 提出了基于极大后验估计的自适应滤
［6］
1. 2 EKF 波算法，邓自立等人 在此基础上，引入了遗忘因
考虑如下随机非线性离散系统 子，使该算法具有了实时估测未知时变噪声的能力 ，
x（ k） = f（ x（ k － 1） ，u（ k － 1） ） + w（ k － 1） 具体计算流程如下：
（ 4） 1） 初始化，k = 0
z（ k） = h（ x（ k） ） + v（ k） （ 5）  x^ （ 0） = E［x（ 0） ］
式中 f（ x） 和 h（ x） 为系统的非线性函数，其它参数  P（ 0） = E［（ x（ 0） － x^ （ 0） ） （ x（ 0） － x^ （ 0） ） T］
^
的定义同（ 1） 式和 （ 2） 式，则系统的扩展卡尔曼滤  Q（ 0） = Q－ 0
波方程可表示为  R（
^ 0） = R－ 0
－
 x^ （ k） = f（ x^ （ k － 1） ，u（ k － 1） ） （ 7）
 P － （ k） = Φk，k －1 P（ k － 1） ΦTk，k －1 + Q（ k － 1） 2，…，进行迭代：
2） 对于 k = 1，

 P － （ k） H（ k） T a） 时间更新
 K（ k） =

{
 H（ k） P － （ k） H（ k） T + R（ k） x^ － （ k） = Φk，k －1 x^ （ k － 1） + B k，k －1 u（ k － 1）
 x^ （ k） = x^ － （ k） + K（ k） （ z（ k） － h（ x^ － （ k） ） ） ^ k － 1）
 P － （ k） = Φk，k －1 P（ k － 1） ΦTk，k －1 + Q（
 P（ k） = （ I － K（ k） H（ k） ） P － （ k）
（ 8）
（ 6） b） 测量更新
式中 估计测量噪声统计特性

{
f H（ k） = h z珓= z（ k） － H（ k） x^ － （ k）
Φk，k －1 = x = x^ （ k －1）
x x x = x^ － （ k） ^ k） = （ 1 － d（ k） ） R（
^ k － 1） +
u = u（ k －1） R（
EKF 算法围绕滤波值 x^ （ k） 将非线性函数 f（ x） d（ k） （ z珓
（ k） z珓
（ k） T
－ H（ k） P － （ k） H（ k） T ）
和 x） 展成泰勒级数并略去二阶及其以上高阶项
h（ （ 9）
来进行线性化，从而得到非线性系统的线性化模型 ， 估算校正后的状态值并求解后验状态方差
因此它是一种次优滤波，并且只能在滤波误差 x（ k） － T － T
 K（ k） = P （ k） H（ k） （ H（ k） P （ k） H（ k） +

－
－ x^ （ k） 及一步预测误差 x（ k） － x^ （ k） 较小时才能 ^ k） ） －1
R（
适用。  －
 x^ （ k） = x^ （ k） + K（ k） z珓
 P（ k） = （ I － K（ k） H（ k） ） P － （ k）
2 基 于 改 进 Sage-Husa 的 LASRKF （ 10）
算法 由上式可知，Sage-Husa 自适应算法在利用测量
数据进行递推滤波的同时，实时估计和修正系统过
KF 和 EKF 都假定系统的噪声统计特性是已知 程噪声和测量噪声的统计特性，从而达到自适应滤
的，而这往往与实际不符，很多系统的过程噪声方差 波的目的。该方法原理简单，实时性好，因此在许多
矩阵 Q 或观测噪声方差矩阵 R 事先是不知道的，有 领域得到推广应用。
时这些参数甚至是时变的。若 Q 和 R 的取值小于实 2. 2 LASRKF 算法
际噪声分布，就会造成真值的不确定范围过小 ，导致 针对伺 服 系 统 的 非 线 性 特 征，本 节 结 合 文 献
有偏估计，并可能引起滤波发散； 相反，若 Q 和 R 的 ［2］平方根滤波的思想，对传统的 Sage-Husa 自适应
取值大于实际噪声分布，则状态估计误差就会增大， 滤波算法进行了改进，提出了 LASRKF 算法。
在统计意义上也可能导致滤波发散 。因此，构建自适 针对（ 1） 式和（ 2） 式描述的随机线性离散系统，
应卡尔曼滤波器，在线调节 Q 和 R ，对提高滤波的 LASRKF 的具体流程如下：
精确度和稳定性具有重要意义。 1） 初始化，k = 0
第1 期 周 勇等： 基于 Sage-Husa 的线性自适应平方根卡尔曼滤波算法 · 91·

 x^ （ 0） = E［x（ 0） ］ R = ST ，
子 S 的转置，即 珟 珟RT 珟
R = AA T 。qr｛ A｝ 表示矩阵
 S（ 0） = chol｛ E［（ x（ 0） － x^ （ 0） ） （ x（ 0） － A 的 QR 分解，返回 珟R。若 S 为下三角矩阵，P = SS T ，

 x^ （ 0） ） T ］｝ 且 P 为非负定矩阵，则称 S 是 P 的 cholesky 因子，记
 ^ T T
 槡Q（ 0 ） = S（ 0） 作 S = chol（ P） 。同 时 称 P new = SS ± 槡ruu 的
 R（ ^ 0 ） = chol｛ E［（ z（ 0） － z^ （ 0） ） （ z（ 0） － cholesky 因 子 为 cholesky 一 阶 更 新， 记 作
槡
 z^ （ 0） ） T ］｝ cholupdate｛ S，u，± r｝ ，若 u 为矩阵，则对其各列依
（ 11） 次递推进行 cholesky 一阶更新。B = diag（ A） 的定
2，…，进行迭代：
2） 对于 k = 1， 义与 Matlab 中相同，即若 A 为矩阵，则 B 是由 A 对角
a） 时间更新 线上的元素组成的列向量； 若 A 为向量，则 B 是由 A

{
－
x^ （ k） = Φk，k －1 x^ （ k － 1） + B k，k －1 u（ k － 1） 的元素组成的对角矩阵。
LASRKF 算法与标准的 KF 和 EKF 算法相比，
S － （ k） = qr { [ Φk，k －1 S（ k － 1） 槡Q（
^ k － 1） ] }
直接对系统状态方差阵的平方根进行推算 ，确保了
（ 12）
系统方差阵的对称性和半正定性，克服了标准 KF
b） 测量更新
易随着计算舍入误差积累导致系统状态方差阵失去
计算测量残差
非负定性，造成滤波发散的缺点。
（ k） = z（ k） － H（ k） x^ － （ k）
z珓 （ 13）
估计测量噪声统计特性
3 仿真验证
 槡R = chol｛ 槡1 － d（ k） 槡R（ ^ k － 1） ，
＊＊

 （ k） | ，d（ k） ｝
| z珓
 为了验证本章所提出的新型自适应平方根滤波
*
 槡R = chol｛ 槡R ，
＊＊
算法的有效性，以直线运动模型为例，在 Matlab 中
 H（ k） S － （ k） ，－ d（ k） ｝
 进行仿真验证。
 R（ T
假设某物体直线运动的模型可表示为
槡^ k） = diag｛ 槡 diag（ 槡R * 槡R * ） ｝
2
（ 14）   s（ k）   1 T T /2 
 x（ k） =  v（ k）  =  1 T  x（ k － 1） +
计算滤波增益   a（ k）   1 
P xz （ k） = S － （ k） S － （ k） T H（ k） T
（ 15） 
S z （ k） = qr { [ H（ k） S （ k） 槡R（
－ ^ k） ] } （ 16）  0  0 
  0  u（ k － 1） +  0 
K（ k） = P xz （ k） （ S z （ k） S z （ k） T ） －1 （ 17）  1  w （ k － 1） 
估算校正后的状态值  α

 z（ k） = s（ k） + v （ k） = [ 1 0 0 ] x（ k） + v （ k）
x^ （ k） = x^ － （ k） + K（ k） z珓
（ k） （ 18） α α

求解后验状态方差平方根 （ 21）

{
2 2
U = K（ k） S z （ k） 式中 w α （ k） ～ N（ 0，σ ） ，v α （ k） ～ N（ 0，σ ） 。状 态
q r
（ 19）
S（ k） = chol｛ S － （ k） ，U，－ 1 ｝ x（ k） 中 3 个变量的物理意义分别为： s（ k） 表示 k 时
更新估测过程噪声统计特性 刻物体的直线位移，v（ k） 表示 k 时刻物体的速度，即
s（ k） 的导数，a（ k） 表示 k 时 刻 物 体 的 加 速 度，即
 槡Q ^ k － 1） ，
＊＊
= chol｛ 槡Q（
 －
v（ k） 的导数。
 | x^ （ k） － x^ （ k） | ，d（ k） ｝
 在本例中，如下选取各参数的值
*
 槡Q = chol｛ 槡Q ，U，－ d（ k） ｝
＊＊
T = 1

 Q（
( 100 （ k － 1 ） ) － 0. 003
T
2π
槡^ k） = diag｛ 槡 diag（ 槡Q * 槡Q * ） ｝ u（ k － 1） = 0. 01sin
（ 20）
－3
式中若 A 为 L × N 阶矩阵，且 N ≥ L，则 A = QR，Q
T σ q = 10
σ r = 10，x（ 0） = [0 0 0]T
为 N × N 阶正交矩阵，R 为 N × L 阶上三角矩阵。R 的
上三角方阵 珟 R（ L × L 阶） 为矩阵 P = AA T cholesky 因 假定已知 σ r = 10，σ q 未知，取初始值 σ^ q （ 0） =
 · 92· 西 北 工 业 大 学 学 报 第 31 卷

10 －2 ，分别利用 KF 和 LASRKF 算法对系统状态 x 进 从图 1 可以看出，当仿真进行到 80 步以后，基

行估计，迭代运行 200 次，仿真结果如图 1 和图 2 所 于 LASRKF 的加速度估计误差逐渐减小，而基于 KF
示（ 图中横坐标 k 即为迭代次数） 。 的估计误差则有增大的趋势，尤其是在第 100 步到
第 160 步之间，基于 LASRKF 的加速度估计误差一
直保持在 0. 01 以下，而基于 KF 的估计误差有时甚
至超 过 了 0. 05。 导 致 上 述 结 果 的 主 要 原 因 是：
LASRKF 与标准 KF 相比，具有较强的自适应能力，
它能够在递推估计系统状态的同时，辨识未知的系
统过程噪声统计特性。 如图 2 所示，当仿真进行到
第 80 步时，LASRKF 已经基本估算出了接近真值的
过程噪声标准差，从而迅速提高了系统滤波的精确
性。此外，LASRKF 直接推算系统状态方差阵的平
方根，克服了标准 KF 易随着计算舍入误差积累导
致系统状态方差阵失去非负定性，从而造成滤波发
散的缺点。

4 结 论

本文在阐述了标准 KF 和 EKF 原理之后，结合

平方根滤波的思想，对传统 Sage-Husa 自适应滤波
算法进行了改进，提出了一种新的自适应平方根卡
尔曼滤波算法———线性自适应平方根卡尔曼滤波
（ LASRKF） 算法。
LASRKF 算法直接对系统状态方差阵和噪声方
差阵的平方根进行递推与估算，确保了系统状态和
噪声方差阵的对称性和非负定性，有效的克服了标
准 KF、EKF 和传统 Sage-Husa 自适应滤波算法存在
的一些缺点，大大提高了滤波器的精确性、稳定性和
自适应能力。

参考文献：

［1］ Qi Song，Han Jianda． An Adaptive UKF Algorithm for the State and Parameter Estimations of a Mobile Robot． Acta Automatica
Sinica，2008，34（ 1） ： 72-79
［2］ Merwe Rvd，Wan E A． The Square-Root Unscented Kalman Filter for State and Parameter Estimation． IEEE International Con-
ference on Acoustics，Speech，and Signal Processing，2001，6： 3461-3464
［3］ Rotstein H． H2 / H ∞ Filtering Theory and an Aerospace Application． International Journal of Robust and Nonlinear Control，
1996，6（ 4） ： 347-366
［4］ Samantaray S R，Dash P K，Upadhyay S K． Adaptive Kalman Filter and Neural Network Based High Impedance Fault Detection
in Power Distribution Networks． International Journal of Electrical Power and Energy Systems，2009，31（ 4） ： 167-172
［5］ Jwo D J，Wang S H． Adaptive Fuzzy Strong Tracking Extended Kalman Filtering for GPS Navigation． IEEE Sensors Journal，
2007，7（ 5） ： 778-789
［6］ 邓自立，郭一新． 油田产油量、产水量动态预报． 自动化学报，1983，9（ 2） ： 121-126
Deng Zili，Guo Yixin． Dynamic Prediction of The Oil and Water Outputs in Oil Field． Acta Automatica Sinica，1983，9（ 2） ：
第1 期 周 勇等： 基于 Sage-Husa 的线性自适应平方根卡尔曼滤波算法 · 93·

121-126 （ in Chinese）
［7］ 张常云． 自适应滤波方法研究． 航空学报，1998，19（ 7） ： 96-99
Zhang Changyun． Approach to Adaptive Filtering Algorithm． Acta Aeronautica et Astronautica Sinica，1998，19（ 7） ： 96-99 （ in
Chinese）
［8］ Duan Z，Han C，Dang H． An Adaptive Kalman Filter with Dynamic Rescaling of Process Noise． Proceedings of the Sixth Inter-
national Conference of Information Fusion，2003，2： 1310-1315
［9］ Shi Y，Han C，Liang Y． Adaptive UKF for Target Tracking with Unknown Process Noise Statistics． 2th International Conference
on Information Fusion，2009，1815-1820

A Novel Algorithm of Linear Adaptive Square-Root

Kalman Filtering Based on Sage-Husa
Zhou Yong1 ，Zhang Yufeng2 ，Zhang Chao1 ，Zhang Juzhong3
 1． College of Aeronautics，Northwestern Polytechnical University，Xi'an 710072，China 
 2． School of Mechatronics，Northwestern Polytechnical University，Xi'an 710072，China 
 
 3． 713th Institute of China Shipbuilding Industry Corporation，Zhengzhou，450000，China 

Abstract： Aiming at the flaws of the standard Kalman Filter（ KF） and Extended Kalman Filter （ EKF） ，and based
on the square-root filtering algorithm，we modify traditional Sage-Husa adaptive filter and present a novel algorithm
of Linear Adaptive Square-Root Kalman Filtering（ LASRKF） in this paper． With this new filter，the square root of
system state covariance matrix is calculated recursively and the estimation of the square root of the system noise co-
variance matrix is obtained straightforwardly． Then the positive semi-definiteness of system state and noise covari-
ance matrix are guaranteed； the stability and the adaptability of filter are also enhanced． Compared with the tradi-
tional Sage-Husa adaptive filtering algorithm，LASRKF algorithm improves the anti-divergence capability． Simula-
tion results show preliminarily that the stability，accuracy and adaptability of the filter are improved greatly．

Key words： algorithms，covariance matrix，estimation，Kalman filters，linear systems，stability； LASRKF（ Linear

Adaptive Square-Root Kalman Filtering） algorithm