1、第三章卡尔曼(Kalman)滤波第一节 引言卡尔曼生平n卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们在现代控制理论中要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法）。1.引言n卡尔曼(Kalman)滤波和维纳(Wiener)滤波都是以最小均方误差为准则的最佳线性估计或滤波。2
2、.适用范围n维纳滤波只适用于平稳随机过程(信号)n卡尔曼滤波没有这个限制，信号可以是平稳的，也可以是非平稳的。3.处理方法维纳滤波器根据全部过去的和当前的观测数据x(n),x(n-1),来估计信号的当前值以均方误差最小条件下求解系统的传递函数H(z)或单位冲激响应h(n)卡尔曼滤波不需要全部过去的观察数据来估计信号的当前值它是用状态空间法描述系统，即由状态方程和量测方程组成。解是以估计值(是状态变量的估计值)的形式给出的只根据前一个估计值 和最近一个观察数据-1kxky其算法是递推递推且状态空间法采用在时域内设计滤波器的方法因而适用于多维随机过程的估计；离散卡尔曼算法适用计算机处理。4.信号模
3、型的建立n从信号模型的建立来看：n维纳滤波的信号模型是从信号与噪声的相关函数得到。n卡尔曼滤波的信号模型则是从状态方程和量测方程得到。卡尔曼滤波器的特点是什么？第二节 卡尔曼滤波器的信号模型离散状态方程与量测方程引入n在讨论维纳滤波时，提出一个基本概念：任何具有有理功率谱密度的随机信号都可看作是白色噪声通过一个线性网络所形成。由此得到维纳滤波器的信号模型 ()w n()A z()s n()w n()A z()s n()x n()v n()w n()B z()x nn为了得到卡尔曼过滤的信号模型，必须首先讨论状态方程和量测方程。一、离散状态方程及其解一、离散状态方程及其解 离散状态方程的基本形式
4、是：(1)()()x kAx kBe k其中x(k)代表一组状态变量组成的多维状态矢量，而A，B都是矩阵，它们是由系统的拓扑结构、元件性质和数值所确定的。)(ke是激励信号。22120110(1)(0)(0)(2)(1)(1)(0)(0)(1)(1)(0)()()(0)()kkkjjkkkjjxAxBexAxBeA xABeBex kAxABe jx kA xABe j )(kx状态方程是多维一阶的差分方程。当已知初始状态x(0)，可用递推的方法得到它的解(0)(0)()kA xAxe 其中第一项：只与系统本身的特性 和初始状态有关，与激励信号无关零输，称为入响应；110()kkjjABe j
5、：只与激励和系统本身特性有关，而与初始状态无关，零第二项：称为状态响应。0()()(0)()ke kx kk xA x k 当时，00kkkk由此可见，通过可将时的状态过渡到任何过渡矩阵的状态。故称为或转移矩阵。10()()()(0)(1)()kkjAkx kk xkj Be j 令代入，得：(0)()xe jABx k当已知初始状态、激励以及 与 矩阵，即可求得。01,00,1()()()kk kk jj kx kx kBe j 00(0)kx：表 示 从 初 始 状 态开 始 递 推。0()kkx k如果用 表示起始点的 值从开始递推，从而有,00k kkk：代表从状态到 状态的转移矩阵。
6、01kk如果，就得到一步递推公式：,1()(1)(0)(1)k kx kx kBe k 0(0)AI由于，代入上式，得：,1()(1)(1)k kx kx kBe k 1-1()()(1)(-1)kkkkx kA k x kw kkxA xw则：为了书写方便，将变量 放在下标表示，(1)(-1)Be kw k假设激励源为白噪声，即称为系统动态噪声，,1()()k kA kA k而系统是时变的，即其中为状态变量之间的增益矩阵，可以随时间发生变化；()(-1)(-1)(-1)kx kx kx kx k说明：在 时刻的状态可以由它前一个时刻的状态来求得，即：时刻以前各状态的影响都已记忆在中一步递推状
7、了。称为态方程。状态方程的核心是：设置状态变量，状态变量是网络内部（最少的）节点变量，一般设在延迟支路的输出端，状态方程刻画了状态变量下一时刻的取值与当前时刻的状态变量和输入之间的关系。(1)()()x kAx kBe k()()(1)(-1)x kA k x kw k一步递推状态方程：总结二、离散时间系统的量测方程卡尔曼滤波需要依据观测数据对系统状态进行估计。因此，除了要建立系统的状态方程，还需要建立一个量测方程。kkkkyc x假设观测系统是线性的，对于离散时间系统的量测方程可写成：()kkkkkkkkycmnmync xx式中：为观察或量测到的信号矢量序列，为观察噪声序列，是观测矩阵：表
8、示状态变量与输出信号之间的增益矩阵，可随时间变化；为的维数，为的维数，是信号真值，它是状态变量各分量的线性组合。kx信 号 的 真 值 是 一 个 多 维 矢 量，它 是 状 态 变 量各 分 量 的 线 性 组 合。kkksc x即：kkkkkkyc xs将式子代入：ky含义：观察或量测到的信号包括信号的真值与噪声。()()()s ns ns n在 维 纳 滤 波 中：希 望 得 到的 估 计 值与 真 值间 的 均 方 误 差 最 小。kkkkkxxxxs在卡尔曼滤波中：希望得到的估计值与间最小均方误差。有了也就得到了。kksx提问：和的关系？kkksc xkksx信号表示为状态变量的线性
9、组合。kkxs为何要用来求？一 阶 多 维 的 方由 于 状 态 方 程 是 一 个，可程一 步 递以 用推 法 求 解。因为把待求的量表示为状态方程中的状态变量的线性组合具有很多优点。为何在计算上卡尔曼滤波相对于维纳滤波器更优？正因卡尔曼滤波利用状态方程得到的。1kkkyxC 当与都是一维变量，且时，则有：()()()()kx ns nnys n此时卡尔曼滤波的量测方程与维纳滤波的信号方程完全相同。只是在符号上有所差异。其中和都表示观察到的数据。=+=kkkkkyxs卡尔曼滤波的信号模型-1zkAkCkw+1kxkxkskky多维情况多维情况卡尔曼滤波的信号模型一维情况一维情况()A zkw
10、ksk=kkkys例1-1z0.36()0.81.25(1-0.8(1-0.8()(),kkksskkyszzzzmmAC设卡尔曼滤波中量测方程为：已知信号的自相关函数的 变换为：，）噪声的自相关函数为：信号和噪声统计独立。求卡尔曼滤波信号中的和。解：-12-1-1-1-10.36()()()=(1-0.8(1-0.8()()1-0.8()0.8:0.8:1sswkkkkkkkkz zzA z A zzzzS zA zzW zsswAysC根据等式：）可以求得：变换到时域得：得到又因为：所以第三节卡尔曼滤波的方法1、卡尔曼滤波的基本思想(2)=-kkkkkyyyxHP再用观测数据的估计误差去修
11、正状态的估计值，通过选择修正矩阵使得状态估计误差的均方值最小。卡尔曼滤波是采用递推的算法实现的，是以卡尔曼滤波的信号模型为基础。(1)kkkkwxy先不考虑激励噪声和观测噪声，得到状态的估计值 和观测数据的估计值。2、研究对象离散系统n离散系统的n维状态方程：1-1kkkkxA xwn离散系统的m维量测方程：kkkkyc xkkxx求解：最小均方误差下信号的估计值。3、卡尔曼滤波一步递推法模型1-1kkkkkkkkxA xwyC x状态方程：量测方程：1kkkkkACyxx上式中、已知，是观测到的数据，也已知，假设信号的上一个估计值已知，求当前时刻的估计值。-111-1(1),=kkkkkkk
12、kkkkkkkkkkkwxxyxA xyC xC A xyyw解：假设上式中忽略、，可立即求得。用 和 分别表示：显然：观测值 和估计值 之间有误差，它们之间的误差是由于忽略动态噪声和量测噪声 引起的。(2)innovation):ky 设它们之间的差 称为新息（=-kkkyyy-1-1,kkkkkwyHw显然：新息的产生中包含了:动态噪声 和量测噪声 的信息成分。因此，可用新息 乘以一个修正矩阵来代替。kx对 进行估计：-1,kkkyHw用新息 乘以一个修正矩阵来代替1-1kkkkxA xw111=-=-kkkkkkkkkkkkA xHy yA xHy C A x1kkkkkxA xH yk
13、x卡尔曼求信号估计值 的一步递推模型kxkx-1kxAk0 xkxky ky-kykCkH-1z-1,kkkyxx看出：输入为已知观测值和上一个信号估计值输出为信号估计值。4、求卡尔曼滤波递推公式kkkkACw解：假设状态变量之间的增益矩阵和状态变量与输出信号之间的增益矩阵不随时间发生变化，动态噪声与观测噪声，都是均值为零的正态噪声，且两者互不相关，即：()kxH k从上可以看出：要求卡尔曼滤波递推公式要估计出 就必须要先找到最小均方误差下的修正矩阵220 cov,0 cov,cov,0,0,1,2TkkkjkjkkjwkTkkkjkjkkjkTkjkjwE ww wE w wQQEERRwE
14、 wk j ：，：，var=varTkkkkTkkkkQwE w wRE 式中称为噪声协方差阵，称为噪声协动态量测方差阵。系统初始条件为：0000000000var(-)(-)cov,0cov,0TTkkTkkE xxExxpx wE x wxE x123/,kjj kmknkyyyykjxx在上述条件下，通过维线性量测系统，从第一时刻到第 时刻，对 维线性动态系统作 次观测根据这 个观测数据，对第 时刻的状态进行估计，记为。/x x x,j kj kj kk kkk kkk kkkkjkjkjkxxxxPPxx当时，称为预测或外推；当时，称为内插；当时，称为，并简单记为：目的：如何求滤波最小
15、均方误差下的估计值。即：研究滤波的情况。/-j kjj kTj kj kj kxxxPE xx 估计误差为：估计的均方误差：1kkwv（）先不考虑噪声与，得到状态方程和量测方程为：-1-1kkkkxxyy式中是的估计值。实测值 与估计值 的差：-1-1 =kkkkkkkkkxA xyc xc A x kkyy为新息，为新息过程。再此先介绍新息过程的重要性质。-kkkyyy新息过程的性质 kkyy表明：新过程与原观测过程的线性空间正交。123-11,nnnyyyyy性质：时刻的新息与所有过去的观测数据正交，即 0,1-1nkE y ykn0,1-1nkE y ykn 2ny 性质 新息过程由彼此
16、正交的随机序列组成，即：kkyy表明：不相关性质。意味着 的每个值都带来新的信息。12123,nny yyy yy性质 表示观测数据的随机序列与表示新息过程的随机序列一一对应，即：1212,nny yyy yykkyy 表明：包含在观测数据 中的信息也存在于新的过程之中。ky 常将新的过程 称作为原观测过程的新息过程，这是新息的确切涵义。2,kkkkwvHx（）考虑噪声与从上面一步递推法模型中可得，为了提高状态估计的质量，获得最优估值，用系数矩阵来校正估计值可得到更好的估计。-1-1-1(-)=(-)kkkkkkkkkkkkkxA xHyyA xHyC A x卡尔曼滤波估计的递此式为推公式。k
17、H其中为增益矩阵，实质是一加权矩阵。-,kkkTkkkkkkkkkkxxxkPE x xxPHHxx 时刻的滤波误差定义为：时刻的均方误差阵（即误差的协方差矩阵）定义为：如果能求 与 最小条件下的然后再将代入上式，则所求得的 将是对 的线性最优估计。03(-)(-)kkkTkkkkkHxwvPExxxx（）求最小均方误差条件下的增益矩阵。设初始状态 与、均不相关，设未考虑噪声时的均方误差阵定义为：kkkkkPPHHP卡尔曼滤波要求状态变量的估计误差的均方值为最小，因此卡尔曼滤波的关键就是要得到与的关系式，即通过选择合适的，使取得最小值。-kkkTkkkkkxxxPExxxxkkxP经过校正后（
18、考虑噪声）的状态变量的估计误差及均方误差阵分别用和表示：kkxP未经校正（未考虑噪声）的状态变量的估计误差及均方误差阵分别用和表示：-kkkTkkkkkxxxPExxxx(a),(b),(c)(d)(5)kkkkkxxxPP推导步骤：推导状态变量的估计值推导状态变量的估计误差计算的均方值，化简，得到一组卡尔曼滤波的递推公式。(a)kx推导状态变量的估计值-1-1-1-1-1-1-1(-)=(+-)=(-)+=(-)+()kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkxA xH yyA xH C xC A xI H C A xH C xHI H C A xH C A xw
19、H(b)kx 推导状态变量的估计误差-1-1-1-1-1-1-1-1-1-1-1-1-1-1-1-1-1-=-(-)+()=-+-=(-)-+(-)-=(-)-+-kkkk kkkkk kkkk kkk kkkkkkkkkkkkkk kkkkkkkkkk kkkkkkkk kxx xAxwI H C AxH C AxwHA xxH C A xxwH C wHI H C A xxI H C wHI H CA xxwH-1-1-1,-(-)-(-)kkkkkkkkkkkkxxab caIH CAxxbIH CwcH由上式看出：状态变量的估计误差由三部分组成即：其中：(c)kkxP计算 的均方值-1
20、-1-1,-=+-+-=-(-)=(-)=TTkkkTTTTTTTTTTTTTkkkkkTTTkkkTTTkkPE x xEab cab cE aabbccab aca b bc a c b caxxAI H CbwI H CcH其中：9kP状态变量的估计误差的均方值 由 项组成：-1-1-1-1-1-1-1-1-TkkkkkTkkkTkkkExxxxPE wwQER 化简上式：其中(d)kP化简-1-1-1-1-1-1-1-1=(-)-(-)=(-)(-)=(-)(-)(-)(-)=TTTTkkkkkkkkkkTTkkkkkkkTTTkkkkkkTkkkkkTTTTkkkkkkkE aaEI
21、 H C A xxxxA I H CI H C A P A I H CE bbEI H C w wI H CI H C QI H CE ccE HHH R H得：-2-2-1-100kkkkjkkjjACxA xAw根据假设条件，状态变量的增益矩阵、不随时间发生变化，且：-1001-2-1-1-1-1-1,0kkkTTkkkkxxwwwwE xwE wx看出与，有关，与无关，则：-1-1-2-1-1-1-1-1-1-2-kkkkkkkkkkxA xHCxCA x又根据状态方程：-1-1001-2-1-1-1-1-1-1-1-1-1-1-1-1(,)-0-0kkkkkkTTkkkkkkTTkkk
22、kkkxxxwwwwExxExxExxwE wxx 看出依赖于即依赖于，及而与和不相关，故有：-1-1-1-1-1-1-1-1-1-1=(-)-(-)=0=(-)-=0=(-)-(-)=0(-)=0=-TTTkkkkkkkkTTTkkkkkkkTTTTkkkkkkkkTTTkkkkkTkkkE abEIH CAxxwIH CE acEIH CAxxHE a bEIH CwxxAIH CE bcEIH CwHE a cE Hxx可得：-1-1(-)=0=(-)=0TTTkkkkTTTTTkkkkkAIH CE b cE HwIH C-1-1-1-1=(-)(-)+(-)(-)+=(-)+(-)+
23、kTTTkTTkkkkkkkTTkkkkkkkkTTTkkkkkkkkkkkPPE aaE bbE ccIH CA P AIH CIH CQIH CH R HIH CA P AQIH CH R H可知，仅有其中的三项不为零，化简成-1-1-1-1-1-1-1-1-1-1-1-1-1-1-1-1-1-:-=-=-=-kkTkkkkkTkkkkkkkkkkTkkkkkkkkTTkkkkkkkkPPPExxxxEA xwA xA xwA xEAxxwAxxwA ExxxxAE ww 为了进一步化简，推导未经误差校正的状态估计误差的均方值1-1-1=+TTkkkkA P AQTkkkkkPPPPP把代
24、入 中，其中 是一对称矩阵，满足：-1-1(-)+(-)+=(-)(-)+=-+TTTkkkkkkkkkkkkTTkkkkkkkkTTTTkkkkkkkkkkkkkPI H CA P AQI H CH R HI H CP I H CH R HP H C P PC HHC PCRH=-+TTTTkkkkkkPP H UUHH SS H得：+()=TTkkkkTTTTkkkkkkC PCRSSUPCC PC P其中：是正定阵令-1-1-1-1-1-1=-+=-+-TTTTkkkkkkTTTTTkkkTTTTTkkkkkkkkkkkPP H UUHH SS HH S U SH S U SP U SS
25、UH S U SH S U SPPCC PCRC P将后三项配对得：-1-1-1-1-1-=0=kkkTkTTTToptkkkkkkHPHH S U SHU SSU SSPCC PCR第三项、第三项与无关，第一项为一半正定阵因此使 最小应满足-1=-=-=-kTTkkkkkkkkkkkoptkkkkkPPPPCC PCRC PPHC PI H CP代入 式中，得到最小均方误差阵：-1-1-1-1-1(-)-kkkkkkkkTTkkkkkkkTkkkkkkkkkxA xHyC A xHP CC P CRPA PAQPIH CP(e)得到一组卡尔曼滤波的递推公式-1-10000varkkkkkkk
26、ACQRyxPxE xPx假设初始条件，已知，其中，则(f)卡尔曼滤波的递推流程-1-1kkxP，-kkkkPI H CP-1-1=+TkkkkkPA PAQkP-1TTkkkkkkkHPCC PCRkxkPkH-1-1(-)kkkkkkkkxA xHyC A x5、离散卡尔曼滤波算法总结n状态方程：1-1kkkkxA xwn量测方程：kkkkyc xn统计特性：0 cov,0 cov,cov,0,0,1,2TkkjkjkkjTkkjkjkkjTkjkjE wwwE w wQEERwE wk j ，0000000000var(-)(-)cov,0cov,0TTkkTkkE xxExxpx wE
27、 x wxE xn初始条件：-1-1(-)kkkkkkkkxA xHyC A xn递推公式：n增益方程：-1()TTkkkkkkkHPCC PCR-1-1TkkkkkPA P AQ（未考虑噪声）-kkkkPI H CP（滤波的）n均方误差阵：-1123-1,kkkkkkkkkHxxyxPHy yyyH可见，若已知利用前一个 的估计值和当前的观测值 就可求得。和的计算不依赖于观测序列可预先计算并存储增益矩阵。6、卡尔曼滤波算法的计算流程图kP计算未考虑噪声均方误差阵0 x初始值kH计算增益矩阵kP计算滤波器的均方误差阵kx计算估计值0P初始值1kk1kkkx输出ky观测值例2-1-10z0.36
28、()0.81.25(1-0.8(1-0.8()(),0,1,10,0,1,2,3,4,5,6,7;23kkksskkkkyszzzzmmsPksP ksP设卡尔曼滤波中量测方程为：已知信号的自相关函数的 变换为：，）噪声的自相关函数为：信号和噪声统计独立,已知求：（）在时刻开始观测信号。试用卡尔曼滤波公式求和（）稳态时的和。（）对于平稳随机信号，当过渡过程结束后，即信号进入稳态后，卡尔曼滤波与维纳滤波之间存在什么关系？解：-1-1-1-1-1-1-22=0.64+0.36 (1)()=(1)(2)-=1-10.8,=1,=0.3(3)6,=10.8(-0.8TkkkkkkTTkkkkkkkkk
29、kkkwkkkkkkkkkkkkPA P AQPHPCC PCRP PPI H CPHPssHysACQR由例 的结果知道：代入公式：1-1)(4)(1)=(1)kkkkkHP PPP由于是一维情况，因此-1-1-10(1)234:0.640.3650.641.360,1,045kkkkkPPPHPsPk把式代入（）、（）、（）式，消去，得到（）初始条件为开始观测，利用等式（）和（）进行递推：00001110122212333234443440,1,1,1,0.5,1,0.4+0.52,0.4048,0.4048,0.4762+0.40483,0.3824,0.3824,0.4941+0.38
30、244,0.3768,0.3768,0.4985+0.37685,0.3kPHsykPHssykPHssykPHssykPHssykP44454445644467755,0.3755,0.4996+0.37686,0.3751,0.3751,0.4999+0.37687,0.3750,0.3750,0.5000+7 80.3 6kkHssykPHssykPHssyys给定每个时刻的观察值 就可得到每一时刻的信号估计值。-1-12-1,53 =838kkkkkkPPPPH上面是递推过程，还没达到稳态。（）假设到了某一时刻前后时刻的均方误差相等，也就是误差不再随着递推增加而下降，达到最小的均方误差
31、了，即稳态情况。式中的均方误差方阵：代入（）式可计算到稳态时的均方误差为：即稳态时的修正矩阵：-1-1430.5838()1-0.5kkkssyzH zz代入式（）得到稳态时的信号估计：变换到 域有：-122min3z0.36()0.81.25(1-0.8(1-0.8()0,()10()()sssoptzzzzmnmHzEe n（）由于维纳滤波只考虑信号进入稳态后情况，因此，用维纳的功率谱分解方法，已知信号的自相关函数的 变换为：，）信号与噪声不相关，即噪声是零均值、单位功率的白噪声（，）求维纳滤波器和。22-1-1-1-1-12-11()()()()()0.361.6(1-0.5(1-0.5
32、)=+1=(1-0.8(1-0.8(1-0.8(1-0.8)()(1-0.5()=,=1.6(1-0.8xxsswwzzzB z B zzzzzzzB zzB zz 解：根据，由于噪声与信号不相关，所以）考虑必须是因果稳定的系统，得到）2-1+-1-1-1+-1-1-1+-1()11(),()()()()1-0.80.36(1-0.8=1.6(1-0.5(1-0.8(1-0.8(1-0.5)1-0.80.36=1.6(1-0.5(1-0.8(1-0.5)1-0.8=1.6(1xsoptxssswzHzzzB zB zzzzzzzzzzzz 考虑系统物理可实现情况：）-1-1-130.68=-0
33、.5(1-0.81-0.5zzz）2-1min-1-11()=()-()()2510.368=21-0.5(1-0.8(1-0.83=8ssoptssccdzEe nzHzzjzdzjzzzz）(-1)()kpx kyH z看出：卡尔曼滤波的稳态解与维纳解是相同。但维纳滤波是通过已知前 个观测数据及信号与噪声的相关函数，通过建立模型的方法分析的。而卡尔曼滤波只要求已知前一个时刻的状态估计值和当前的观测值，由状态方程和量测方程递推而得结果。维纳滤波的解以的形式给出，卡尔曼滤波是以状态变量的估计值给出解的形式。但它们都是采用均方误差最小的准则，但卡尔曼有一个过渡过程，与维纳滤波不完全相同，但到达稳
34、态后，结果相同。7、一步预测估计的卡尔曼预测器0,0,kxkmw将被估计（滤波）的信号和被预测的信号联系起来，在纯预测情况下，且若则可以不考虑。1/11,kkkkkkxxx 1,)kkkkxA说明：给出被滤波信号 后，未来一步信号的最优估计忽略了噪声的影响，并假设转移矩阵（即只对估计起作用。kA用乘估计（滤波）方程，并用上式可得到预测方程：1/-1/-1-kkkk kkkkk kxA xGyc xkkkGA H式中称做预测增益矩阵。-1/-1-1/-1/-1)TTkkkkkkkkk kkTTkkk kkkk kkkAHPCC PCRPPGA PCC PCR将乘以式：（用表示 得预测均：方（称为
35、误差阵。1/-1-Tkkkkkk kkkPAG CPAQ预测误差协称做方差矩阵。/-1-k kkkkkkPPPI H CP用表示 代入式子：得：8、预测与滤波之间比较-1-1-1-1-1(-)-kkkkkkkkTTkkkkkkkTkkkkkkkkkxA xHyC A xHPCC PCRPA P AQPI H CP滤波：1-1-1-1-1-11-1(-)-kkkk kkkkkk kTTkkk kkkk kkkTkkkkkk kkkxA xGyC A xGA PCC PCRPAG CPAQ预测：9、同时有过滤和预测输出的方框图1,()kkkAkxky ky-kykCkH-1zkx1kx由图可知：能
36、够从卡尔曼滤波器中获得一步预测。例322AR(2)()1.74(-1)-0.81(-2)()(-1)(0)0()()()()()()0.049Kalman()x nx nx nw nxxy nx nnx nw nnx n用如下差分方程产生一个随机序列：用量测方程：观测，其中和分别是方差为和 的高斯白噪声，利用预测对进行预测。解：21,(-1),()(-1)01(-2)0()-0.81 1.74(-1)()()()()Kalman010,-0.81 1.740.0401,9kkkkkkx kX kx kx kx kx kx kw ky kX kkAQCR令（则状态方程为：量测方程：因此得到预测的
37、参数为：1-1-1-1-1-11-1(-)-kkkk kkkkkk kTTkkk kkkk kkkTkkkkkk kkkxA xGyC A xGA PCC PCRPAG CPAQ利用公式：2(1)()1.74(-1)-0.81(-2)()(-1)(0)0()2()()()(1)3(1)(+1)4Kalmanx nx nx nw nxxx ny nx nnx nx nx n实验方法如下：由将代入产生随机序列的一次实现。（）利用作为观测数据得到预测值。（）比较预测值 和真实值。（）评价预测的跟踪性能。Kalman预测的跟踪性能 增益的变化曲线 10、卡尔曼滤波公式中各个参数之间关系0kkkHPQR
38、讨论：卡尔曼滤波公式中增益矩阵与初始均方误差阵、动态噪声协方差阵及量测噪协方差阵之间的关系。(1)kkRH由上式可知：增大时，变小，因为如果量测噪声增大，则增益应取小些，以减弱量测噪声的影响。-1()TTkkkkkkkHPCC PCR-1-1TkkkkkPA P AQ（未考虑噪声）1-12kkkPQP（）当变 小 或变 小 或 两 者 都 变 小 时，从 上 式 看 出变 小。-kkkkPIH CP0-1kkPQH因 为变 小 表 示 初 始 估 计 较 好，变 小 表 示 状 态 转 移 的 随 机 波 动 小，所 以 新 加 进 的 观 测 值 对 状 态 预 测 的校 正 影 响 减 弱
39、，增 益 矩 阵变 小。kkkPPH变小，可以看出变小，变小。-1()TTkkkkkkkHP CC P CR第四节卡尔曼滤波与维纳滤波的关系1、举例-13()8 ()()1-0.5zX zH zY zz同上例，设计卡尔曼滤波，得到其传递函数，取其 变换：与用维纳滤波器原理及计算方法，将得到完全相同的结果。-10001212-1-100010110 var1,030.58333 0.5()8882kkkkxPxx x xPPxxxyxyxyxxyy其次：再按已知条件 ，求及，已知 ，可以从式子：递推出012122201-1033=0.5+=(+)88 223()8 223/8.1,kkkkkkk
40、kyyxxyyyyxyHPP HP上式中认为所有的均等于又已知，可递推出值。2、结论2kH（）用不稳态的代入，将不可能与维纳滤波有相同结果。为何？1()kHH z当卡尔曼滤波：（）把稳态的代入有关式子，得到与维纳滤波有相同结果。卡尔曼滤波：在稳态下与维纳滤波相同的结果，是因为最小均方误差为准则它们都是以：的线性估计器。卡尔曼滤波采用递推的方法实现，解具有一个过渡过程；当卡尔曼滤波达到稳态时，这两种方法的解是相同的。卡尔曼滤波不是一种新的滤波理论，它仅是维纳滤波的一种算法。维纳滤波卡尔曼滤波已知条件误差准则均方误差最小均方误差最小解的形式模型建立信号与噪声相关函数 状态方程与量测方程(),(-1
41、),x n x n(),()H z h n()x k(-1),()x ky k3、卡尔曼滤波与维纳滤波不同n(1)卡尔曼滤波与维纳滤波中解决最佳滤波的方法不相同。n维纳滤波：是用频域及传递函数的方法；n卡尔曼滤波：是用时域及状态变量的办法；n(2)卡尔曼在理论上是维纳滤波的推广和发展，特别在处理多变量系统、时变线性系统及非线性系统的最佳滤波等领域，为我们提供了一种比较有效的方法，克服了基于频域处理所遇到的困难。n这些困难包括：维纳滤波要求平稳，而卡尔曼滤波则不要求；n卡尔曼容许初始时间不是负无穷大，这在很多情况下是有实际意义的；n(3)卡尔曼滤波的另一个不同点是把状态或信号过程的产生看成是白噪
42、声激励有限维数系统的输出；n维纳滤波要求过程的自相关函数和互相关函数的简单知识，而卡尔曼滤波则要求时域中状态变量及信号产生过程的详细知识。4、卡尔曼滤波的优点n在时域上采用线性递推形式对观测值进行处理，能实时地给出系统状态的最优估计，并突破了单维输入和输出的限制。n卡尔曼滤波算法的这些优点使它在信号和信息系统中得到比较广泛的应用。5、卡尔曼滤波的缺点n（1）模型误差和数值发散。模型误差模型误差:卡尔曼滤波算法的关键是建立系统的状态模型。但实际系统有时很难得到精确描述，往往只能用近似模型来代替，因为即使能够获得精确的模型，也常会因为精确模型太复杂，维数过高而与实时处理必须减少计算量及尽量简化模型
43、的要求相矛盾。近似或简化的模型都与精确模型之间存在误差，模型误差必然会给滤波带来影响，严重时还会造成滤波结果不收敛。抑制方法：采用逐渐衰减记忆法、限定记忆法、限定下界法和人为增加模型输入噪声方差。数值发散数值发散:舍入误差的影响以及递推算法使得舍入误差积累的影响。计算机存贮单元的长度有限，不可避免地存在舍入误差，它相当于在状态方程和量测方程中加入噪声，带来的后果是有可能改变某些矩阵的性质，引起误差矩阵失去正定性和对称性，如均方误差阵列受到扰动而离开稳定解，如没失去正定性，仍可返回稳定解，可用双精度运算得以改善，但会增加运算量，目前采用平方根法，即求均方误差阵P改用其平方根P1/2实现。n(2)
44、实时要求。影响卡尔曼滤波算法的实时性主要是状态维数n和增益矩阵的计算，它们往往有很大的计算量。n一般在计算中采取某些措施，例如应用定常系统新算法或在精度损失允许情况下尽量减小维数等措施，从而减小计算量以满足实时滤波的要求。6、卡尔曼滤波的应用n在空间技术、工业过程控制与电子工程等领域得到了比较广泛的应用，特别在信号处理的二次加工数据处理方面应用更广，诸如雷达的位置、速度的估计，以及空中交通管制系统对飞行器航迹的估计与导航等领域都得到了广泛而成功的应用。(1)应用举例-雷达跟踪目标物n说明卡尔曼滤波的应用。n雷达跟踪目标的基本原理是通过发射脉冲，根据接收到的脉冲与发射脉冲的时间间隔，来确定目标物
45、的距离和速度。n由于干扰的影响，接收到的脉冲波形变化很大，那么一次的测量结果可能存在很大的误差。n为了减小误差，往往采取发射一串脉冲的方法进行测量。确定空间中的一点需要由径向距离和方位角来确定。T设雷达跟踪的目标为飞行器，发射的脉冲时间间隔为(),+1(+1),kRkkRk在时间 径向距离为在时间径向距离为两者之间有秒的延时。()(+1)TRkk：表示空间一次扫描的时间间隔。：表示平均距离，和：表示对平均值的偏差。假定偏差是统计随机的，均值为零，(1)()()()kkTkk可写出：其中：表距离方程示速度。(1)(1)-()()Tkkkk 设 表示加速度，则可得到：加速度方程其中：表示速度。22
46、)1)0,)kEkkEk假定加速度（是零均值的平稳白噪声，即满足：（()()()x kkz kkz kk定义：表示第 个雷达回波脉冲获得的目标距离，表示第 个雷达脉冲进行数据处理之后的目标距离估计，表示第 个雷达脉冲进行数据处理之后的目标速度估计。()4x k设定状态变量，选择状态变量有 个，分别表示径向距离、径向速度、方位角和方位角速度1234()(),(),(),()(),(),(),()TTX kxkxkxkxkkkkk即：11222133444212(1)()()(1)()()(1)()()(1)()()()()x kx kTxkxkxkkx kx kTxkxkxkkkTkT根据状态变
47、量的物理含义，得到以下方程：式中：表示在区间 径向加速度，：表示在区间 角加速度。1122133442(1)()0100(1)()()0100(1)()0001(1)()()0001x kx kTxkxkkx kx kTxkxkk将上式写成矩阵形式：:(1)(1,)()()X kA kk X kw k由此得到卡尔状态曼滤波型程信的方号模11123212:()()()()()()()()z kx kkzkx kkkk再看量测方程，距离和方向的估计值为其中：，为观测偏差。1121232422:()()()()()()()1000=+()()()0010()()0pZ kCX kV kx kz kx
48、kkzkx kkxkV k将上两式写成向量形式和矩阵形式其中：观测噪声，假定为高斯噪声，均值为，方差为和。212222112222()()()0000000()()()=0000000=,=,TkjTE w k wjQ kQ kE w k wkETET 状态方程激励信号的协方差阵为：其中：为径向加速度在 时刻的方差；为角加速度在 时刻的方差。1.11.22.12.221122212222222()()()()()()()()()=()()()()0=0()TkjTEkjR kEkEkkR kEkkEkkEkkk 量测方程的噪声协方差阵为：22:1(),21=3fMMM假定在各个方向，加速度服从
49、均匀分布，其概率密度函数并将 的值限制在之间，那么，加速度的方差：2111()()()()()()()TP kP kE e k ekP kpkE ek根据误差信号协方差阵的定义：可以计算出，单个信号的均方误差的协方差矩阵为：2112221211122122()()()()=()()()()()=()()E ekE e k e kP kEE e k e kE ekpkpkpkpk两个信号的协方差矩阵为：11221121132422(1)(2),(1)(2),()(2)(2)1()(2)(2)-(1)()(2)(2)1()(2)(2)-(1)zzzzx kzxkzzTx kzxkzzT根据接收到的
50、相邻两个回波脉冲，可以测量出飞行器的距离和方位角和可根据这四个数据，求得状态变量的估计值：1122121111111112323232()(2)-(1)(2)-(1)()(2),(2),2,(2)(2)(2)(2)(2)(2)-(1)(1)(1)+(2)=(2)(2)(2)(2)-(1)(1)TkX kzzzzX kzzTTkxxxxxTXxxxT可求得 时刻的状态向量可以写为：若取用量测方程可得：1323242)-(1)(2)(2)(2)-(1)(1)+TxxT2(2)(2)-(2)(2)-(2)TkPEXXXX 计 算时 刻 的 误 差 信 号 的 协 方 差 阵：1213422,(2)(
