卡尔曼滤波

第三章卡尔曼(Kalman)滤波第一节引言卡尔曼生平卡尔曼全名RudolfEmilKalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们在现代控制理论中要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《ANewApproachtoLinearFilteringandPredictionProblems》（线性滤波与预测问题的新方法）。1.引言卡尔曼(Kalman)滤波和维纳(Wiener)滤波都是以最小均方误差为准则的最佳线性估计或滤波。2.适用范围维纳滤波只适用于平稳随机过程(信号)卡尔曼滤波没有这个限制，信号可以是平稳的，也可以是非平稳的。3.处理方法维纳滤波器根据全部过去的和当前的观测数据x(n),x(n-1),…来估计信号的当前值以均方误差最小条件下求解系统的传递函数H(z)或单位冲激响应h(n)卡尔曼滤波不需要全部过去的观察数据来估计信号的当前值它是用状态空间法描述系统，即由状态方程和量测方程组成。解是以估计值(是状态变量的估计值)的形式给出的只根据前一个估计值和最近一个观察数据-1ˆkxky其算法是递推且状态空间法采用在时域内设计滤波器的方法因而适用于多维随机过程的估计；离散卡尔曼算法适用计算机处理。4.信号模型的建立从信号模型的建立来看：维纳滤波的信号模型是从信号与噪声的相关函数得到。卡尔曼滤波的信号模型则是从状态方程和量测方程得到。卡尔曼滤波器的特点是什么？第二节卡尔曼滤波器的信号模型—离散状态方程与量测方程引入在讨论维纳滤波时，提出一个基本概念：任何具有有理功率谱密度的随机信号都可看作是白色噪声通过一个线性网络所形成。由此得到维纳滤波器的信号模型()wn()Az()sn()wn()Az()sn()xn()vn()wn()Bz()xn为了得到卡尔曼过滤的信号模型，必须首先讨论状态方程和量测方程。一、离散状态方程及其解离散状态方程的基本形式是：(1)()()xkAxkBek其中x(k)代表一组状态变量组成的多维状态矢量，而A，B都是矩阵，它们是由系统的拓扑结构、元件性质和数值所确定的。)(ke是激励信号。22120110(1)(0)(0)(2)(1)(1)(0)(0)(1)(1)(0)()()(0)()kkkjjkkkjjxAxBexAxBeAxABeBexkAxABejxkAxABej)(kx状态方程是多维一阶的差分方程。当已知初始状态x(0)，可用递推的方法得到它的解(0)(0)()kAxAxe其中第一项：：只与系统本身的特性和初始状态有关，与激励信号无关零输，称为入响应；110()kkjjABej：只与激励和系统本身特性有关，而与初始状态无关，零第二项：称为状态响应。0()()(0)()kekxkkxAxk当时，00kkkk由此可见，通过可将时的状态过渡到任何过渡矩阵的状态。故称为或转移矩阵。10()()()(0)(1)()kkjAkxkkxkjBej令代入，得：(0)()xejABxk当已知初始状态、激励以及与矩阵，即可求得。。01,00,1()()()kkkkjjkxkxkBej00(0)kx：表示从初始状态开始递推。0()kkxk如果用表示起始点的值从开始递推，从而有,00kkkk：代表从状态到状态的转移矩阵。01kk如果，就得到一步递推公式：,1()(1)(0)(1)kkxkxkBek0(0)AI由于，代入上式，得：,1()(1)(1)kkxkxkBek1-1()()(1)(-1)kkkkxkAkxkwkkxAxw则：　为了书写方便，将变量放在下标表示，(1)(-1)Bekwk假设激励源为白噪声，即＝　称为系统动态噪声，,1()()kkAkAk而系统是时变的，即　＝其中为状态变量之间的增益矩阵，可以随时间发生变化；()(-1)(-1)(-1)kxkxkxkxk说明：在时刻的状态可以由它前一个时刻的状态来求得，即：时刻以前各状态的影响都已记忆在中一步递推状了。称为态方程。状态方程的核心是：设置状态变量，状态变量是网络内部（最少的）节点变量，一般设在延迟支路的输出端，状态方程刻画了状态变量下一时刻的取值与当前时刻的状态变量和输入之间的关系。(1)()()xkAxkBek()()(1)(-1)xkAkxkwk一步递推状态方程：总结二、离散时间系统的量测方程卡尔曼滤波需要依据观测数据对系统状态进行估计。因此，除了要建立系统的状态方程，还需要建立一个量测方程。kkkkycx假设观测系统是线性的，对于离散时间系统的量测方程可写成：()kkkkkkkkycmnmyncxx式中：为观察或量测到的信号矢量序列，为观察噪声序列，是观测矩阵：表示状态变量与输出信号之间的增益矩阵，可随时间变化；为的维数，为的维数，是信号真值，它是状态变量各分量的线性组合。kx信号的真值是一个多维矢量，它是状态变量各分量的线性组合。kkkscx即：　　　　　kkkkkkycxs将式子代入　：　　＝＋＝＋ky含义：观察或量测到的信号包括信号的真值与噪声。ˆ()()()snsnsn在维纳滤波中：希望得到的估计值与真值间的均方误差最小。ˆˆˆkkkkkxxxxs在卡尔曼滤波中：希望得到的估计值与间最小均方误差。有了也就得到了。kksx提问：和的关系？kkkscxkksx信号表示为状态变量的线性组合。kkxs为何要用来求？一阶多维的方由于状态方程是一个，可程一步递以用推法求解。因为把待求的量表示为状态方程中的状态变量的线性组合具有很多优点。为何在计算上卡尔曼滤波相对于维纳滤波器更优？正因卡尔曼滤波利用状态方程得到的。1kkkyxC当与都是一维变量，且时，则有：()()()()kxnsnnysn此时卡尔曼滤波的量测方程与维纳滤波的信号方程完全相同。只是在符号上有所差异。其中和都表示观察到的数据。=+=kkkkkyxs卡尔曼滤波的信号模型-1zkAkCkw+1kxkxkskky多维情况卡尔曼滤波的信号模型一维情况()Azkwksk=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:1sswkkkkkkkkzzzAzAzzzzSzAzzWzsswAysC根据等式：））可以求得：变换到时域得：得到又因为：所以第三节卡尔曼滤波的方法1、卡尔曼滤波的基本思想'ˆ(2)=-ˆkkkkkyyyxHP再用观测数据的估计误差去修正状态的估计值，通过选择修正矩阵使得状态估计误差的均方值最小。卡尔曼滤波是采用递推的算法实现的，是以卡尔曼滤波的信号模型为基础。''(1)ˆˆkkkkwxy先不考虑激励噪声和观测噪声，得到状态的估计值和观测数据的估计值。2、研究对象－离散系统离散系统的n维状态方程：1-1kkkkxAxw离散系统的m维量测方程：kkkkycx＝＋ˆkkxx求解：最小均方误差下信号的估计值。3、卡尔曼滤波一步递推法模型1-1kkkkkkkkxAxwyCx状态方程：量测方程：＝＋1ˆˆkkkkkACyxx上式中、已知，是观测到的数据，也已知，假设信号的上一个估计值已知，求当前时刻的估计值。-1'''''11'-1(1)ˆˆˆˆˆˆˆ,=ˆkkkkkkkkkkkkkkkkkkwxxyxAxyCxCAxyyw解：假设上式中忽略、，可立即求得。　用和分别表示：＝显然：观测值和估计值之间有误差，它们之间的误差是由于忽略动态噪声和量测噪声引起的。(2)innovation):ky设它们之间的差称为新息（'ˆ=-kkkyyy-1-1,kkkkkwyHw显然：新息的产生中包含了:动态噪声和量测噪声的信息成分。因此，可用新息乘以一个修正矩阵来代替。kx对进行估计：-1,kkkyHw用新息乘以一个修正矩阵来代替1-1ˆˆkkkkxAxw'111ˆˆ=-ˆˆ=-kkkkkkkkkkkkAxHyyAxHyCAx1ˆˆkkkkkxAxHyˆkx卡尔曼求信号估计值的一步递推模型ˆkxˆkx-1ˆkxAk0ˆx'ˆkxkyky-'kykCkH-1z-1ˆ,,ˆkkkyxx看出：输入为已知观测值和上一个信号估计值输出为信号估计值。4、求卡尔曼滤波递推公式kkkkACw解：假设状态变量之间的增益矩阵和状态变量与输出信号之间的增益矩阵不随时间发生变化，动态噪声与观测噪声，都是均值为零的正态噪声，且两者互不相关，即：ˆ()kxHk从上可以看出：要求卡尔曼滤波递推公式要估计出就必须要先找到最小均方误差下的修正矩阵220cov,0cov,,cov,0,,0,1,2TkkkjkjkkjwkTkkkjkjkkjkTkjkjwE：，，：，var=varTkkkkTkkkkQwEwwRE式中＝称为噪声协方差阵，＝称为噪声协动态量测方差阵。系统初始条件为：0000000000var(-)(-)cov,0cov,0TTkkTkkExxExxpxwExwxEx123/,,,,,kjjkmknkyyyykjxx在上述条件下，通过维线性量测系统，从第一时刻到第时刻，对维线性动态系统作次观测根据这个观测数据，对第时刻的状态进行估计，记为。//////ˆxˆxˆxˆˆ,,ˆjkjkjkkkkkkkkkkkkjkjkjkxxxxPPxx当时，称为预测或外推；当时，称为内插；当时，称为，并简单记为：目的：如何求滤波最小均方误差下的估计值。即：研究滤波的情况。/////ˆ-jkjjkTjkjkjkxxxPExx估计误差为：　估计的均方误差：＝1kkwv（）先不考虑噪声与，得到状态方程和量测方程为：''-1-1ˆˆkkkkxxyy式中是的估计值。实测值与估计值的差：''-1'''-1ˆˆˆˆˆ=kkkkkkkkkxAxycxcAxkkyy为新息，为新息过程。再此先介绍新息过程的重要性质。'ˆ-kkkyyy新息过程的性质kkyy表明：新过程与原观测过程的线性空间正交。123-11,,,,nnnyyyyy性质：时刻的新息与所有过去的观测数据正交，即0,1-1nkEyykn　　　　0,1-1nkEyykn　　　　　2ny性质　新息过程由彼此正交的随机序列组成，即：kkyy表明：不相关性质。　　意味着的每个值都带来新的信息。12123,,,,,,nnyyyyyy性质　表示观测数据的随机序列与表示新息过程的随机序列一一对应，即：1212,,,,,,nnyyyyyykkyy表明：包含在观测数据中的信息也存在于新的过程之中。ky常将新的过程称作为原观测过程的新息过程，这是新息的确切涵义。2,ˆ,kkkkwvHx（）考虑噪声与从上面一步递推法模型中可得，为了提高状态估计的质量，获得最优估值，用系数矩阵来校正估计值可得到更好的估计。'-1-1-1ˆˆˆˆˆ(-)=(-)kkkkkkkkkkkkkxAxHyyAxHyCAx卡尔曼滤波估计的递此式为推公式。kH其中为增益矩阵，实质是一加权矩阵。ˆ-,ˆkkkTkkkkkkk