第33卷 第7期系统工程与电子技术
Vol.33 No.72011年7月
Systems Engineering and Electronics July 2011
文章编号:1001 506X(2011)07 1454 04
收稿日期:2010 09 01;修回日期:2010 11 11。基金项目:国家自然科学基金(60972146)资助课题
作者简介:穆静(1979 ),女,讲师,博士研究生,主要研究方向为再入弹道目标状态估计。E mail:mu jing1977@https://www.doczj.com/doc/3c17662777.html,
迭代容积卡尔曼滤波算法及其应用
穆 静,蔡远利
(西安交通大学电子与信息工程学院自动控制工程研究所,陕西西安710049)
摘 要:将Gauss New ton 迭代和容积卡尔曼滤波(cubature Kalm an f ilt er,CKF)算法相结合,建立了一种迭代CKF(iterated CKF,ICKF)算法。该算法使用容积数值积分原则直接计算非线性随机函数的均值和方差,且在迭代过程中利用最新量测信息并改进迭代过程产生的新息方差和协方差,可获得较高的估计精度。针对弹道系数未知的再入弹道目标状态估计问题,仿真实验结果显示,该方法实现简单,比无迹卡尔曼滤波方法(unscented Kalman filter,UKF)及CKF 方法效果要好。
关键词:容积原则;容积卡尔曼滤波;再入弹道目标状态估计
中图分类号:T P 391 文献标志码:A DOI :10.3969/j.issn.1001 506X.2011.07.05
Iterated cubature Kalman filter and its application
MU Jing,CAI Yuan li
(Institute of A utomatic Control Engineering ,School of Electronic and Inf ormation Engineering ,
X i an Jiaotong University,X i an710049,China)
Abstract:An iterated cubat ure Kalm an filter (ICKF)is proposed,w h ich com bines the Gauss N ew ton iter
ate m ethod w ith the cubature Kalman filter (CKF).In the ICKF algorithm ,cubat ure rule based numerical inte gration m ethod is direct ly used to calculate t he mean and covariance of t he nonlinear random f unct ion,and the lat est m easurement,im proved innovation covariance and cross covariance are iterat ively used in the m easure m ent update,so the h igher accuracy of st ate estim ate is ach ieved.T he ICKF algorithm is applied to stat e est i m ation for reentry ballistic target w ith unknow n ballist ic coeff icient.T he sim ulation result s indicate t hat the implem entat ion of the proposed method is easy and simple.M oreover,the higher accuracy of stat e estimat ion is obtained com pared w ith UKF and CKF.
Keywords:cubature rule;cubature Kalman filter;stat e est imation for reent ry ballist ic target
0 引 言
非线性系统的状态估计是工程领域广泛存在的问题,获得非线性状态估计问题的最优解需要得到系统状态的后验概率分布的完整描述,然而只有在很少的情况下才能精确的描述,为此在过去的几十年里人们提出了大量的次优滤波方法。扩展卡尔曼方法(extended Kalman filter,EKF)是工程中一种广泛应用的次优滤波方法[1 2],其基本思想是线性化状态和量测方程后使用卡尔曼滤波算法。对于高度非线性问题,EKF 可能产生较大的滤波误差,甚至出现计算发散。为了改进EKF 的性能,提出了迭代EKF 滤波方法(it erat ed EKF,IEKF)
[3]
。IEKF 在量测更新过程
中通过迭代过程充分使用最新的量测信息以提高估计的精度。当估计值非常接近真实值时,IEKF 性能优于EKF,但
这种情况在实际中很少出现。EKF 和IEKF 实现时需要计算雅克比矩阵,限制了它们的使用范围。
近年来,发展了许多免微分的状态估计方法。无迹卡尔曼滤波方法(unscented Kalman filter,UKF)通过一组确定的加权采样点来逼近随机变量的分布函数,当这组采样点通过非线性函数传播时,捕获非线性函数的统计特性,精度达到二阶[4]。但使用UKF 对高于3维状态向量估计时,为了避免非正定协方差矩阵的传播,需要仔细的 调整 UKF 中的参数[5]
。文献[6 7]提出的离差差分滤波器(divided difference filter,DDF)是基于低阶多维Stirling 内插多项近似非线性函数,结合卡尔曼滤波算法实现状态估计。UKF 与DDF 具有相同的确定性采样和加权统计估计属性,估计精度接近,因此UKF 和DDF 统称为Sigma 点滤波方法[8]。近年来提出的粒子滤波方法(particle filter,PF)依据蒙特卡罗思想,随机产
第7期穆静等:迭代容积卡尔曼滤波算法及其应用 1455
生大量粒子近似计算后验概率密度。随着迭代次数的增加,
PF实现时会出现粒子退化和贫化现象[9 10]。为了解决粒子
退化问题,出现了许多改进的粒子算法[11 12]。但是PF实现
时需要产生大量的粒子,计算量非常大,且降低粒子退化和
贫化现象是以增加PF算法的复杂度为代价的,因此PF很
难满足实时性的要求。而文献[13 14]提出的求积滤波器
(quadrature Kalman filter,QKF),使用高斯 厄尔米特数值积
分获得随机变量非线性变换后的统计特性,QKF比EKF和
UKF具有更高的估计精度,但QKF实现时,使用的积分点
数按状态维数的指数增长,会引起维数灾难。
最近,文献[15 16]提出了一种新的状态估计方法容积
卡尔曼滤波(cubature Kalman filter,CKF),为非线性估计
问题提供了一种新的实现方式。CKF采用一组等权值的
容积点集解决贝叶斯滤波的积分问题,即使用容积数值积
分原则计算非线性变换后的随机变量的均值和协方差。相
比于EKF、U KF、DDF和PF等非线性滤波算法,CKF算法
具备更优的非线性逼近性能、数值精度以及滤波稳定性,且
CKF实现简单和滤波精度较高。
对于未知弹道系数的再入弹道目标状态估计这种高度
非线性状态估计问题,由于初始误差较大和量测方程的高
度非线性,降低了CKF算法对其进行状态估计的精度。本
文在CKF算法的基础上,结合Gauss New ton迭代方法并
改进迭代过程产生的新息方差和协方差,提出了迭代CKF
(iterat ed CKF,ICKF)。ICKF汲取CKF算法的优点并充
分利用最新量测信息,可有效地降低状态估计误差。仿真
结果表明,ICKF有效地降低了状态估计误差,可获得较高
精度的状态估计。
1 容积卡尔曼滤波算法
考虑如下离散时间非线性动态系统
x k=f(x k-1)+w k-1
z k=h(x k)+v k(1)
式中,x k为系统状态向量;z k为量测值。假定过程噪声
w k-1和量测噪声v k相互独立,且w k-1:N(0,Q k-1),v k:
N(0,R k)。
容积卡尔曼滤波算法首先计算加权函数为标准正态分
布密度的积分的基本容积点和对应的权值[15]
j=m
2[1]j, j=
1
m
(2)
式中,m表示容积点总数。使用三阶容积原则,容积点总数是状态维数的2倍,即m=2n x,n x为系统的状态维数。基本容积点按照下列方式产生的,记n x维单位向量为e= [1,0, ,0]T,使用[1]表示对e的元素进行全排列和改变元素符号产生的点集,称为完整全对称点集,[1]j表示点集中[1]的第j个点。若k-1时刻的后验概率为p(x k-1| z1:k-1)~N(x k-1;x^k-1,P k-1),且S k-1=chol{P k-1},chol{}表示矩阵的乔列斯基分解。容积卡尔曼滤波算法如下:
(1)时间更新
计算容积点
X j,k-1=S k-1 j+x^k-1(3) 计算通过非线性状态方程传播的容积点
X*j,k=f(X j,k-1)(4) 计算状态和方差预测
x-= m j=1 j X*j,k
P-k= m j=1 j X*j,k X*T j,k-x-x-T k+Q k-1(5) (2)量测更新
分解因式
S-k=chol(P-k)(6) 计算容积点
X j,k=S-k j+x-k(7) 计算通过非线性量测方程传播的容积点
Z j,k=h(X j,k)(8) 计算量测预测、新息方差和协方差估计
z-k= m j=1 j Z j,k
P zz,k= m j=1 j Z j,k Z T j,k-z-k z-T k+R k(9)
P x z,k= m j=1 j X j,k Z T j,k-x-k z-T k
计算增益,状态和协方差估计
W k=P xz,k P-1zz,k
x^k=x-k+W k(z k-z-k)(10)
P k=P-k-W k P zz,k W T k
2 迭代容积卡尔曼滤波算法
在高斯和变量独立的假设下,可知x k:N(x-k,P-k)和y k: N(h(x k),R k)。变量x k,y k的似然函数定义为
(x
k
,y k)=C exp{-0.5[(x k-x-k)T P--1k (x k-x-k)+ (y k-h(x k))T R-1k (y k-h(x k))]}(11)式中,C为常数。
定义代价函数
J(x k)=(x k-x-k)T P--1k(x k-x-k)+
(y k-h(x-k))T R-1k(y k-h(x-k))(12) 式(11)的最大似然估计等价于求代价函数的极小值。通常使用非线性迭代方法求解代价函数的极小点。使用Gauss New ton非线性迭代方法并线性化量测方程,可写出求解J(x k)极小点的迭代公式
x^(i+1)
k
=x-k+P-k(H(i)k)T(H(i)k P-k(H(i)k)T+R k)-1
[z k-h(x^(i)k)-H(i)k(x-k-x^(i)k)](13)式中,H(i)k= h(x k)/ x k|x k=x^(i)k。而(H(i)k P-k(H(i)k)T+R k)-1
系统工程与电子技术第33卷 1456
第7期穆静等:迭代容积卡尔曼滤波算法及其应用 1457
根据几何关系,可建立量测方程
z k=h(x k)+v k(32)
式中,z k=[L k E k A k]T,且
L k=x2k+y2k+z2k+v L(33)
E k=arctan
z k
x2k+y2k
+v E(34)
A k=arctan y k
x k
+v A(35)
假设量测噪声v k是均值为零的白噪声,协方差矩阵为R k= diag([ 2L 2E 2A]), L, E和 A分别为距离、俯仰角和方位角的量测误差标准差。
3.2 仿真实验
设雷达量测间隔T=0.1s,跟踪目标80s。雷达量测的
距离、俯仰角和方位角误差的标准差分别为
L =100m、
E
=
0.017rad和
A
=0.017rad。目标初始位置为(232km, 232km,80km),初始速度为2290m/s,俯仰角为210 ,方位角为45 ,目标弹道系数真实值为 =20000kg m-1 s-2,初始状态方差选取:P0=diag[1002,2002,1002,2002,1002, 2002,10002]。
运行50次蒙特卡罗仿真,将提出的ICKF方法分别与UKF、CKF滤波算法相比较,三种滤波算法的位置、速度和弹道系数的均方根误差(
root mean square error,RM SE)如图1~图3所示。
图3 弹道系数均方根误差(纵坐标用幅值对数表示)
由图1和图2可知,ICKF充分使用了最新的量测信息,与CKF和U KF相比较,位置误差和速度误差大大降低,状态的估计精度明显高于U KF和CKF。
从图3可知,在估计的前半部分,由于缺乏估计弹道系数的最新信息,估计误差变化不大,而在后半部分,ICKF使用最新量测信息,降低了弹道系数估计误差。
比较滤波算法的另外一种性能指标是平均累加均方根误差(average accumulated mean square root error,AAM S RE)。目标位置的AAMSRE其定义为
AAMSRE p=
1
N
N
i=1
sqrt
1
M
M k=1[( x(i)k)2+
( y(i)k)2+( z(i)k)2](36)式中,M为量测数据点数;N为蒙特卡罗次数。类似位置的平均累加均方根误差的定义,可定义速度和弹道系数的平均累加均方根误差。三种滤波算法的位置、速度与弹道系数的平均累加均方根误差如表1所示。
表1 三种滤波器的平均累加均方根误差
滤波算法
AAM SRE p/
m
AAM S RE v/
(m s-1)
AAM SRE /
(kg m-1 s-2) UKF269.1660.63624.09
CKF269.1260.63623.24
ICKF201.9152.97615.77
从表1可知,U KF累加均方根误差较大,CKF较小, ICKF最小。因此从图1~图3和表1可知ICKF算法是一个有效的状态估计方法。
4 结 论
本文建立了一种ICKF,该算法使用容积数值积分方法直接计算经受非线性变换的随机变量的均值和方差,实现简单,使用Gauss New ton迭代方法充分利用最新量测信息,且改进迭代过程产生的新息方差和协方差,ICKF降低初始误差和量测方程的非线性对状态估计的影响,提高了状态估计精度。将ICKF算法应用于未知弹道系数的再入弹道目标的状态估计,仿真结果表明,ICKF滤波性能明显优于U KF和CKF。
(下转第1509页)
第7期朱伟等:两维数字阵列雷达的数字单脉冲测角方法 1509
用[J].现代雷达,2003,25(5):16 18.(Niu B J,Li Y B.Study and applic ation of angle measurement for2 D phased array mono pluse tracking[J].Modern Radar,2003,25(5):16 18.)
[10]朱庆明.数字阵列雷达述评[J].雷达科学与技术,2004,2(3):
136 141.(Zhu Q M.An overview of digital array radar develop ment[J].Radar S cience and Technology,2004,2(3):136 141.) [11]何明友.数字阵列雷达和软件化雷达[M].北京:电子工业出版
社,2008.(He M Y.Digital array radar and sof tw are rad ar[M].
Bei jing:Publishing House of Electronics Industry,2008.)
[12]Bu rrell R H.Granularity of beam positions in digital phased
arrays[J].Pr oce eding s of IE EE,1968,56(11):1795 1800. [13]Robert J M.Phased arr ay antenna hand book[M].London:
Artech Hous e,2005.
[14]Shelton J P,Kelleher K S.M ultiple beams from linear arrays[J].
I EEE Trans.on A ntennas and Prop agation,1961,9:154 161.
[15]Rich ard O N.Accuracy of angle es tim ation w ith monopulse
processing u sing tw o beams[J].IE EE Tr ans.on Ae rosp ace and Electronic S ystems,2001,37(4):1419 1423.
[16]Zhen W,Ab hijit S,Peter W,et al.Angle es tim ation for two
un res olved targets w ith monopulse radar[J].I EE E T rans.on Aer osp ace and E le ctr onic S ystems,2004,40(3):998 1019. [17]J rome G,Eric C,Pascal L.Joint detection estimation problem
of m on opulse angle m easurement[J].IE EE T ran s.on Ae ro sp ace and E le ctr onic S yste ms,2010,46(1):397 413.
[18]陈伯孝,胡铁军,郑自良,等.基于波瓣分裂的米波雷达低仰角
测高方法及其应用[J].电子学报,2007,35(6):1021 1025.
(Ch en B X,H u T J,Zheng Z L,et al.M eth od of altitude measurement based on beam split in VH F radar an d its applica tion[J].Ac ta E le ctr onic S inica,2007,35(6):1021 1025.) [19]Chen B X,Zhao G H,Zhang S H.Altitude measurement based on
beam split and frequency di versity i n VHF radar[J].IEEE Trans.
on A erospace and Elec tronic S ystems,2010,46(1):3 13.
[20]赵光辉,陈伯孝,吴向东,等.基于差分预处理的米波雷达低仰
角处理算法[J].电子与信息学报,2009,31(2):363 365.
(Zhao G H,Chen B X,W u X D,et al.An algorithm b as ed on differential p reprocess ing of low elevation estim ation in VH F radar[J].J ournal of Electronic s&Inf or mation Tec hnology, 2009,31(2):363 365.)
(上接第1457页)
参考文献:
[1]Bar S halom Y,Li X R.E stimation w ith ap p lica tions to trac k
ing and nav ig ation[M].New Yor k:W iley,2001:381 395. [2]Grewal M S,Andrews A P.K alman f iltering:theory and p ractice
using matlab[M].2nd ed.New York:Wi ley,2001:169 200. [3]Lefeb vr e T,Bruyninckx H,Schutter J D.Kalman filters for
non linear sy stems:a comparison of perform an ce[J].Interna tional Journal of Control,2004,77(7):639 653.
[4]Ju lier S J,Uhlman n J K,Dur rant Whyten H F.A new method
for the n onlinear tran sformation of mean s and covariances in fil ters and estimations[J].I EE E T rans.on Au tomatic Control, 2000,45(3):472 482.
[5]Ju lier S J,U hlmann J K.Uns cented filterin g and nonlinear esti
mation[J].Pr ocee ding of the I EE E,2004,92(3):401 422. [6]N rgaard M,Poulsen N K,Ravn O.New developments i n state esti
mation for nonlinear systems[J].Automatic a,2000,36:1627 1638. [7] imandl M,Dun k J.Derivative free estimation methods:new
results and performance an aly sis[J].A utomatic a,2009,45(7): 1749 1757.
[8]M erw e R V D.Sigm a point filters for probabilistic infer ence in
dynamic state space m odels[D].Am erica:Oreg on Health& Science University,2004.
[9]Gordon N J,Salmond D J,Smith A F M.Novel approach to
nonlinear/non Gauss ian Bayes ian s tate estimation[J].IE E P rocee ding of F,1993,140(2):107 113.
[10]Arulampalam S,Askell S,Gordom N,et al.A tutorial on particle
filters for online nonlinear/non Gaussian Bayesi an tracking[J].
I EEE Trans.on Signal P rocessing,2002,50(2):174 188.
[11]Fu X Y,Jia Y M.An Improvem ent on resam pling algorithm of
particle filters[J].I EEE T rans.on S ignal P rocessing,2010, 58(10):5414 5420.
[12]Kabaoglu N.Target tracking u sing particle filters with su pport
vector regression[J].I EE E T rans.on Veh icu lar Tec hnology, 2009,58(5):2569 2573.
[13]Ito K,Xiong K.Gaussian fi lters for nonli near filtering problems[J].
I EEE Trans.on Automatic Contr ol,2000,45(5):910 927.
[14]Arasaratnam I,H aykin S,Elliott R J.Discrete time nonlinear
filtering algorithm s using Gauss H erm ite quadrature[J].P ro
c ee
d ing of th
e I EE E,2007,95(5):953 977.
[15]Arasaratnam I,H ay kin S.Cu batu re Kalm an filter s[J].I E EE
Tr ans.on A utomatic Control,2009,54(6):1254 1269.
[16]Arasaratnam I,Haykin S,Hurd T R.Cubature Kalman filtering
for continuous discrete systems:theory and simulations[J].IEEE Trans.on S ignal Processing,2010,58(10):4977 4993.
[17]Li X R,J ilkov V P.A survey of man euvering target tracking
part :b allistic target m odels[C] Pr oc.of S ignal and Data P rocessing of S mall T arg ets,2001:559 581.
[18]Frin a A,Ristic B,Benven uti D.T racking a b allistic target:
com paris on of several nonlinear filters[J].I EE E T rans.on Aer osp ace and E le ctr onic S ystems,2002,38(3):854 867. [19]Ris tic B,Farina A,Benvenuti D,et al.Performan ce bounds
and com parison of nonlin ear filter s for tracking a ballistic ob ject on re en try[J].I EE Pr ocee ding of Rad ar Sonar and Nav ig a tion,2003,150(2):65 70.
2017 年秋季学期研究生课程考核 （读书报告、研究报告） 考核科目:雷达系统导论 学生所在（系）:电子与信息工程学院 学生所在学科:电子与同学工程 学生姓名: 学号: 学生类别: 考核结果阅卷人 第 1 页（共页）
几种非线性滤波算法的介绍与性能分析 作者姓名：学号： 专业院系：电信学院电子工程系 电子邮件： 摘要—非线性滤波算法在雷达目标跟踪中有着重要的应用，对雷达的跟踪性能有着至关重要的影响。好的滤波算法有利于目标航迹的建立及保持，能够得到较精确的目标位置，为发现目标后的后续工作提供可靠的数据依据。本文重点介绍了雷达数据处理中的几种非线性滤波算法：扩展卡尔曼滤波（EKF）、不敏卡尔曼滤波（UKF）、粒子滤波（PF），并且给出了一个利用这三种算法进行数据处理的一个实例，通过这个实例对比分析了这三种算法的性能以及优劣。 关键字—非线性滤波算法；扩展卡尔曼滤波；不敏卡尔曼滤波；粒子滤波； I.概述（一级表题格式） 在雷达对目标进行跟踪前要先对目标进行检测。对于满足检测条件的目标就需要进行跟踪，在跟踪的过程中可以利用新获得的数据完成对目标的进一步检测比如去除虚假目标等，同时利用跟踪获得数据可以进一步完成对目标动态特性的检测和识别。因此对目标进行准确的跟踪是雷达性能的一个重要指标。在检测到满足条件的目标后，根据目标运动状态建立目标运动模型，然后对目标跟踪算法进行设计，这是雷达目标跟踪中的核心部分。 目前主要的跟踪算法包括线性自回归滤波，两点外推滤波、维纳滤波、- αβ滤波、加权最小二乘滤波、维纳滤波和卡尔曼滤波[1]。对于线性系统而言最优滤波的方法就是卡尔曼滤波，卡尔曼滤波是线性高斯模型下的最优状态估计算法。但是实际问题中目标的运动模型往往不是线性的，因此卡尔曼滤波具有很大的局限性。目前主要用的非线性滤波算法可以分为高斯滤波和粒子滤波[2]。不敏卡尔曼滤波和扩展卡尔曼滤波就是高斯滤波中的典型代表，也是应用相对较为广泛的。粒子滤波的应用范围比高斯滤波的适用范围要广，对于系统状态非线性，观测模型非高斯等问题都有很好的适用性。本文具体分析阐述了扩展卡尔曼滤波算法，不敏卡尔曼滤波算法，粒子滤波算法，并且通过一个实例利用仿真的方法分析了这三种算法在滤波性能上的优劣，最后对这三种算法做了一定的总结。 我本科毕业设计题目为《基于历史数据的路径生成算法研究》，由于我是跨专业保研到电信学院，该课题所研究内容不属于雷达系统研究范围，是一种城市路网最快路径生成算法。 II.几种非线性滤波算法 A.扩展卡尔曼滤波 扩展卡尔曼滤波是将非线性系统转换为近似的线性系统的一种方法，其核心思想是围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上的项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成状态估计。 扩展卡尔曼滤波状态空间模型： k k k w x f+ = + ) ( x 1 状态方程 k k k v x h+ =) ( z观测方程 其中(.) f和(.) h为非线性函数 在扩展卡尔曼滤波中，状态的预测以及观测值的预测由非线性函数计算得出，线性卡尔曼滤波中的状态转移矩阵A阵和观测矩阵H阵由f和h函数的雅克比矩阵代替。 对 (.) f和(.) h Taylor展开，只保留一次项有： ) ? ( ) ?( ) ( k k k k k x x A x f x f- + ≈ ) ? ( ) ?( ) ( k k k k k x x H x h x h- + ≈ 其中： k k x x k k dx df A ?= =为f对 1- k x求导的雅克比矩阵 k k x x k k dx dh H ?= =为h对 1- k x求导的雅克比矩阵 ) ?( ? 1-k k x f x=，于是可以得出： k k k k k k k w x A x f x A x+ - + ≈ + ) ? ) ?( ( 1 k k k k k k k v x H x h x H z+ - + ≈ + ) ? ) ?( ( 1 通过以上变换，将非线性问题线性化。接下来EKF 滤波过程同线性卡尔曼滤波相同，公式如下: )) | (?( ) |1 ( X?k k X f k k= + ) ( ) ( ) | ( ) ( ) |1 (P k Q k k k P k k k+ Φ' Φ = + )1 ( )1 ( ) |1 ( )1 ( )1 (S+ + + ' + + = +k R k H k k P k H k )1 ( )1 ( ) |1 ( )1 ( K1+ + ' + = +-k S k H k k P k
Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程： -=--+（1）先验估计 X k k AX k k Bu k (|1)(1|1)() -=--+（2）协方差矩阵的预测(|1)(1|1)' P k k AP k k A Q
第十三章 卡尔曼滤波 在本章中，我们介绍一种被称为卡尔曼滤波的十分有用的工具。卡尔曼滤波的基本思想是将动态系统表示成为一种称为状态空间表示的特殊情形。卡尔曼滤波是对系统线性投影进行序列更新的算法。除了一般的优点以外，这种算法对计算确切的有限样本预测、计算Gauss ARMA 模型的确切似然函数、估计具有时变参数的自回归模型等，都提供了重要方法。 §13.1 动态系统的状态空间表示 我们已经介绍过一些随机过程的动态表示方法，下面我们在以前的假设基础上，继续分析动态系统的表示方法。 13.1.1 继续使用的假设 假设t y 表示时刻t 观测到的n 维随机向量，一类非常丰富的描述t y 动态性的模型可以利用一些可能无法观测的被称为状态向量(state vector)的r 维向量t ξ表示，因此表示t y 动态性的状态空间表示(state-space representation)由下列方程系统给出： 11+++=t t t v ξF ξ 状态方程(state model) (13.1) t t t w ξH x A y t +'+'= 量测方程(observation model) (13.2) 这里F ，A '和H '分别是阶数为r r ?，k n ?和r n ?的参数矩阵，t x 是1?k 的外生或者前定变量。方程(13.1)被称为状态方程(state model)，方程(13.2)被称为量测方程(observation model)，1?r 维向量t v 和1?n 维向量t w 都是向量白噪声，满足： ???≠=='τ ττ t t E t ,,)(0Q v v (13.3) ? ??≠=='τττt t E t ,,)(0R w w (13.4) 这里Q 和R 是r r ?和n n ?阶矩阵。假设扰动项t v 和t w 对于所有阶滞后都是不相关的，即对所有t 和τ，有： 0w v =')(τ t E (13.5) t x 是外生或者前定变量的假定意味着，在除了包含在121,,,y y y Λ--t t 内的信息以外，t x 没有为s t +ξ和s t +w (Λ,2,1,0=s )提供任何新的信息。例如，t x 可以包括t y 的滞后值，也可以包括与τξ和τw (任意τ)不相关的变量。 方程系统中方程(13.1)至方程(13.5)可以表示有限观测值的序列 },,,{21T y y y Λ，这时需要状态向量初始值1ξ。假设1ξ与t v 和t w 的任何实现都不
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。
卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_ #if _MSC_VER > 1000 #pragma once #endif// _MSC_VER > 1000 #include
kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman(); }; #endif// !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_) ============================kalman.cpp=============== ================= #include "kalman.h" #include
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.doczj.com/doc/3c17662777.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
卡尔曼滤波算法总结-标准化文件发布号：（9556-EUATWK-MWUB-WUNN-INNUL-DDQTY-KII
2015.12.12 void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; }
首先是卡尔曼滤波的5个方程： (|1)(1|1)() X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)')Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1))X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_0 1_0 Angle dt Angle dt Q bias Q bia o s Gyr -= + 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)() X k k AX k k Bu k -=--+ (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵
卡尔曼滤波的基本原理及应用卡尔曼滤波在信号处理与系统控制领域应用广泛，目前，正越来越广泛地应用于计算机应用的各个领域。为了更好地理解卡尔曼滤波的原理与进行滤波算法的设计工作，主要从两方面对卡尔曼滤波进行阐述：基本卡尔曼滤波系统模型、滤波模型的建立以及非线性卡尔曼滤波的线性化。最后，对卡尔曼滤波的应用做了简单介绍。 卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。卡尔曼滤波器包括两个主要过程：预估与校正。预估过程主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；校正过程负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。这样的一个过程，我们称之为预估－校正过程，对应的这种估计算法称为预估－校正算法。以下给出离散卡尔曼滤波的时间更新方程和状态更新方程。 时间更新方程： 状态更新方程： 在上面式中，各量说明如下： A：作用在X k-1上的n×n 状态变换矩阵 B：作用在控制向量U k-1上的n×1 输入控制矩阵 H：m×n 观测模型矩阵，它把真实状态空间映射成观测空间 P k－：为n×n 先验估计误差协方差矩阵 P k：为n×n 后验估计误差协方差矩阵 Q：n×n 过程噪声协方差矩阵 R：m×m 过程噪声协方差矩阵 I：n×n 阶单位矩阵K k：n×m 阶矩阵，称为卡尔曼增益或混合因数 随着卡尔曼滤波理论的发展，一些实用卡尔曼滤波技术被提出来，如自适应滤波，次优滤波以及滤波发散抑制技术等逐渐得到广泛应用。其它的滤波理论也迅速发展，如线性离散系统的分解滤波（信息平方根滤波，序列平方根滤波，UD 分解滤波），鲁棒滤波（H∞波）。 非线性样条自适应滤波：这是一类新的非线性自适应滤波器，它由一个线性组合器后跟挠性无记忆功能的。涉及的自适应处理的非线性函数是基于可在学习
Kalman 滤波算法 姓名：刘金强 专业：控制理论与控制工程 学号：2007255 ◆实验目的： （1）、掌握klman 滤波实现的原理和方法 （2）、掌握状态向量预测公式的实现过程 （3）、了解Riccati 差分方程实现的过程和新息的基本性质和过程的计算 ◆实验要求： 问题： F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(n)sin(0.1n)]的转置，V2(n)=[n2(n) n3(n)];n1(n)为均值为零，方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0，方差为0.1的均匀分布白噪声,n1(n),n2(n),n3(n)相互独立，试用卡尔曼滤波器算法估计x^(n). ◆实验原理： 初始条件： 1?(1)x =E{x(1)} K(1,0)=E{[x(1)- (1)x ][x(1)- (1)H x ]},其中(1)x =E{x(1)} 输入观测向量过程： 观测向量序列={y(1),…………y(n)} 已知参数： 状态转移矩阵F(n+1,n) 观测矩阵C(n) 过程噪声向量的相关矩阵1()Q n 观测噪声向量的相关矩阵2()Q n 计算：n=1,2,3,………………. G(n)=F(n+1,n)K(n,n+1) ()H C n 12[()(,1)()()]H C n K n n C n Q n --+ Kalman 滤波器是一种线性的离散时间有限维系统。Kalman 滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵P(n)的迹最小化。这意味着，kalman 滤波器是状态向量x(n)的线性最小方差估计。 ◆实验结果： ◆程序代码： （1）主程序
Kalman滤波算法的特点： （1）由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入/输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳随机过程的滤波，而且特别适用于非平稳或平稳马尔可夫序列或高斯-马尔可夫序列的滤波，所以其应用范围是十分广泛的。 （2）Kalman滤波算法是一种时间域滤波方法，采用状态空间描述系统。系统的过程噪声和量测噪声并不是需要滤除的对象，它们的统计特征正是估计过程中需要利用的信息，而被估计量和观测量在不同时刻的一、二阶矩却是不必要知道的。 （3）由于Kalman滤波的基本方程是时间域内的递推形式，其计算过程是一个不断地“预测-修正”的过程，在求解时不要求存储大量数据，并且一旦观测到了新的数据，随即可以算的新的滤波值，因此这种滤波方法非常适合于实时处理、计算机实现。 （4）由于滤波器的增益矩阵与观测无关，因此它可预先离线算出，从而可以减少实时在线计算量。在求滤波器增益矩阵时，要求一个矩阵的逆，它的阶数只取决于观测方程的维数，而该维数通常很小，这样，求逆运算是比较方便的。另外，在求解滤波器增益的过程中，随时可以算出滤波器的精度指标P，其对角线上的元素就是滤波误差向量各分量的方差。 Kalman滤波的应用领域 一般地，只要跟时间序列和高斯白噪声有关或者能建立类似的模型的系统，都可以利用Kalman滤波来处理噪声问题，都可以用其来预测、滤波。Kalman滤波主要应用领域有以下几个方面。 （1）导航制导、目标定位和跟踪领域。 （2）通信与信号处理、数字图像处理、语音信号处理。 （3）天气预报、地震预报。 （4）地质勘探、矿物开采。 （5）故障诊断、检测。 （6）证券股票市场预测。 具体事例： （1）Kalman滤波在温度测量中的应用； （2）Kalman滤波在自由落体运动目标跟踪中的应用； （3）Kalman滤波在船舶GPS导航定位系统中的应用； （4）Kalman滤波在石油地震勘探中的应用； （5）Kalman滤波在视频图像目标跟踪中的应用；
卡尔曼滤波研究综述 1 卡尔曼滤波简介 1.1卡尔曼滤波的由来 1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文-《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。 其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 1.2标准卡尔曼滤波-离散线性卡尔曼滤波 为了描述方便我们作以下假设：物理系统的状态转换过程可以描述为一个离散时间的随机过程；系统状态受控制输入的影响；系统状态及观测过程都不可避免受噪声影响；对系统状态是非直接可观测的。在以上假设前提下，得到系统的状体方程和观测方程。
X ?? 1-1 式中:X k 为状态向量,L k 为观测向量,Φk,k-1为状态转移矩阵,U k-1为控制向量,一般 不考虑,Γk,k-1,B k 为系数矩阵,Ωk-1为系统动态噪声向量,Δk 为观测噪声向量,其随机模 型为 E(Ωk ) =0;E(Δk ) =0;cov(Ωk ,Ωj ) = D Ω(k )δkj , cov(Δk ,Δj ) = D k (k )δkj ;cov(Ωk ,Δj ) =0;E(X 0) =μx(0) var(X 0) = D(X 0);cov(X 0,Ωk ) =0;cov(X 0,Δk ) =0. 1-2 卡尔曼滤波递推公式为 X ∧(k/k) = X ∧(k/k-1)+J k (L k -B k X ∧(k/k-1)), D(k/k) = (E-J k B k )D x (k/k-1), J k = D x (k/k-1)BT k [B k D x (k/k-1)]B T k +D Δ(k)]-1, X ∧ (k/k-1) =Φk ,k-1X ∧ (k-1/k-1), D x (k/k-1) =Φk ,k-1D x (k-1/k-1)ΦT k ,k-1+Γk ,k-1D Δ(k-1)ΓT k ,k-1. 1-3 2 几种最新改进型的卡尔曼滤波算法。 2.1 近似二阶扩展卡尔曼滤波 标准的卡尔曼滤波只适用于线性系统，而工程实际问题涉及的又大多是非 线性系统，于是基于非线性系统线性化的扩展卡尔曼滤波(EKF)在上世纪70年代 被提出,目前已经成为非线性系统中广泛应用的估计方法。近似二阶扩展卡尔曼 滤 波方法(AS-EKF)基于线性最小方差递推滤波框架,应用均值变换的二阶近似从 而得到非线性系统的递推滤波滤波框架 该滤波基于线性最小方差递推框架，状态X 的最小方差估计为
1．什么是卡尔曼滤波器 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。卡尔曼是一个人的名字。 卡尔曼全名Rudolf Emil Kalman，1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文 《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。 简单来说，卡尔曼滤波器是一个 “optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。
假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。（所谓高斯白噪声中的高斯是指概率分布是正态函数，而白噪声是指它的二阶矩不相关，一阶矩为常数，是指先后信号在时间上的相关性。这是考查一个信号的两个不同方面的问题。 高斯白噪声：如果一个噪声，它的幅度分布服从高斯分布，而它的功率谱密度又是均匀分布的，则称它为高斯白噪声。） 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平
一个应用实例详解卡尔曼滤波及其算法实现 标签：算法filtermatlabalgorithm优化工作 2012-05-14 10:4875511人阅读评论(25)收藏举报分类： 数据结构及其算法（4） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。 我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。 我们也把这些偏差看成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23 度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance（协方差）来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56 度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
卡尔曼滤波的原理以及应用 滤波，实质上就是信号处理与变换的过程。目的是去除或减弱不想要成分，增强所需成分。卡尔曼滤波的这种去除与增强过程是基于状态量的估计值和实际值之间的均方误差最小准则来实现的，基于这种准则，使得状态量的估计值越来越接近实际想要的值。而状态量和信号量之间有转换的关系，所以估计出状态量，等价于估计出信号量。所以不同于维纳滤波等滤波方式，卡尔曼滤波是把状态空间理论引入到对物理系统的数学建模过程中来，用递归方法解决离散数据线性滤波的问题，它不需要知道全部过去的数据，而是用前一个估计值和最近一个观察数据来估计信号的当前值，从而它具有运用计算机计算方便，而且可用于平稳和不平稳的随机过程(信号)，非时变和时变的系统的优越性。 卡尔曼滤波属于一种软件滤波方法，概括来说其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。其所得到的解是以估计值的形式给出的。 卡尔曼滤波过程简单来说主要包括两个步骤：状态变量的预估以及状态变量的校正。预估过程是不考虑过程噪声和量测噪声，只是基于系统本身性质并依靠前一时刻的估计值以及系统控制输入的一种估计；校正过程是用量测值与预估量测值之间的误差乘以一个与过程
噪声和量测噪声相关的增益因子来对预估值进行校正的，其中增益因子的确定与状态量的均方误差有关，用到了使均方误差最小的准则。而这一过程中体现出来的递归思想即是：对于当前时刻的状态量估计值以及均方误差预估值实时进行更新，以便用于下一时刻的估计，使得系统在停止运行之前能够源源不断地进行下去。 下面对于其数学建模过程进行详细说明。 1.状态量的预估 （1）由前一时刻的估计值和送给系统的可控制输入来预估计当前时刻状态量。 X(k|k-1)=A X(k-1|k-1)+B U(k) 其中，X(k-1|k-1)表示前一时刻的估计值，U(k)表示系统的控制输入，X(k|k-1)表示由前一时刻估计出来的状态量的预估计值，A表示由k-1时刻过渡到k时刻的状态转移矩阵，B表示控制输入量与状态量之间的一种转换因子，这两个都是由系统性质来决定的。 （2）由前一时刻的均方误差阵来预估计当前时刻的均方误差阵。 P(k|k-1)=A P(k-1|k-1)A’+Q 其中，P(k-1|k-1)是前一时刻的均方误差估计值，A’代表矩阵A 的转置，Q代表过程噪声的均方误差矩阵。该表达式具体推导过程如下： P(k|k-1)=E{[Xs(k|k)-X(k|k-1)][Xs(k|k)-X(k|k-1)]’}------ 其中Xs(k|k)=A Xs(k-1|k-1)+B U(k)+W(k-1)表示当前时刻的实际值，Xs(k-1|k-1)表示前一时刻的实际值，可以看出与当前时刻的预估计值
卡尔曼滤波简介说明及其算法MATLAB实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.doczj.com/doc/3c17662777.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
基于matlab的卡尔曼信号滤波设计 卡尔曼滤波的基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 语音信号在较长时间内是非平稳的，但在较短的时间内的一阶统计量和二阶统计量近似为常量，因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳态输出。假定语音信号可看成由一AR模型产生： 时间更新方程： 测量更新方程： K(t)为卡尔曼增益，其计算公式为： 其中 、分别为过程模型噪声协方差和测量模型噪声协方差，测量协方差可以通过观测得到， 则较难确定，在本实验中则通过与两者比较得到。 由于语音信号短时平稳，因此在进行卡尔曼滤波之前对信号进行分帧加窗操作，在滤波之后对处理得到的信号进行合帧，这里选取帧长为256，而帧重叠个数为128； 下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号：
原声音信号与加噪声后的信号 原声音信号与经卡尔曼滤波处理后的信号 MATLAB程序实现如下： %%%%%%%%%%%%%%%%%基于LPC全极点模型的最大后验概率估计法,采用卡尔曼滤波%%%%%%%%%%%%%% clear; clc; %%%%%%%%%%%%%%%%%%%%%%%%%%%加载声音数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% loadvoice.mat y=m1(2,:); x=y+0.08*randn(1,length(y)); %%%%%%%%%%%%%%%原声音信号和加噪声后的信号%%%%%%%%%%%%%%% figure(1); subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号'); subplot(212);plot(m1(1,:),x);xlabel('时间');ylabel('幅度');title('加噪声后的信号');