卡尔曼滤波器(代码)

第第77章章卡尔曼滤波器卡尔曼滤波器††7.17.1卡卡尔曼滤波器的信号模型尔曼滤波器的信号模型††7.27.2卡卡尔曼滤波器的算法尔曼滤波器的算法††7.37.3举例举例参考书目：随机信号处理，陆光华等编著，西电出版社参考书目：随机信号处理，陆光华等编著，西电出版社，，200220027.17.1卡尔曼滤波器的信号模型卡尔曼滤波器的信号模型††19601960年，年，R.E.KalmanR.E.Kalman发表了采用递推的方法解决离散数发表了采用递推的方法解决离散数据线性滤波的问题据线性滤波的问题((ANewApproachtoLinearFilteringANewApproachtoLinearFilteringandPredictionProblemsandPredictionProblems，，TransactionsoftheASMETransactionsoftheASME--JournalofBasicEngineering,JournalofBasicEngineering,82(82(SeriesDSeriesD):35):35--45,1960)45,1960)，从此以后，随着数字计算技术的发展，，从此以后，随着数字计算技术的发展，KalmanKalman滤波器得到了深入的研究和发展。滤波器得到了深入的研究和发展。问题的描述：卡尔曼滤波器是用来估计一个由线性随机差分方程卡尔曼滤波器是用来估计一个由线性随机差分方程所描述的时间离散系统的状态所描述的时间离散系统的状态xx::()(1)(1)(1)()()()xkAxkBukwkykCxkvk=−+−+−=+(7.1.1)††其中其中xx为系统的状态，为系统的状态，A,BA,B是由系统的结构确定的矩是由系统的结构确定的矩阵，阵，yy为观测值，为观测值，CC为观测矩阵，随机变量为观测矩阵，随机变量ww和和vv代表系代表系统噪声和观测噪声，且相互独立，一般假定统噪声和观测噪声，且相互独立，一般假定ww和和vv服从服从正态分布，且系统噪声和观测噪声的协方差阵分别为正态分布，且系统噪声和观测噪声的协方差阵分别为Q,RQ,R。。††卡尔曼滤波器的任务：是从观测值卡尔曼滤波器的任务：是从观测值yy中得到系统状态中得到系统状态xx在在最小均方误差意义下的最佳逼近最小均方误差意义下的最佳逼近。。()(0,)()(0,)pwNQpvNR∼∼(7.1.2)ˆx††设从第一时刻到第设从第一时刻到第kk时刻，系统的观测值为时刻，系统的观测值为，，根据这根据这kk个观测数据，对第个观测数据，对第jj时刻的系统状态时刻的系统状态进行估进行估计。计。††当当时，称为预测或外推；时，称为预测或外推；††当当时，称为内插；时，称为内插；††当当jj==kk时，称为滤波。时，称为滤波。12,,kyyyjxjkjk7.27.2卡尔曼滤波器的算法卡尔曼滤波器的算法††定义定义为第为第kk步对状态的先验估计，步对状态的先验估计，为在得到观测值为在得到观测值yykk后对状态的后验估计。分别定义先验和后验估计误差后对状态的后验估计。分别定义先验和后验估计误差为：为：††则先验估计误差的协方差为：则先验估计误差的协方差为：††后验估计误差的协方差为后验估计误差的协方差为ˆkx−ˆkxˆˆkkkkkkexxexx−−=−=−(7.2.1)[]TkkkpEee−−−=[]TkkkpEee=(7.2.2)(7.2.3)1ˆˆ(1)kkxAxBuk−−=+−††我们所要寻找的状态的后验估计我们所要寻找的状态的后验估计可以构造为先验估可以构造为先验估计计和实际测量值和实际测量值yykk与估计测量值与估计测量值之差的加权值的之差的加权值的线性组合，即：线性组合，即：ˆkxˆkx−ˆkCx−11ˆˆˆ()ˆˆ()kkkkkkkkkxxKyCxAxKyCAx−−−−=+−=+−(7.2.4)上式中的上式中的称为测量新息。称为测量新息。正是由于存在正是由于存在和和才产生了才产生了，，它是实际观测值与没有考虑噪它是实际观测值与没有考虑噪声的估计值之差，即声的估计值之差，即隐含了隐含了与与的信息，也就是说的信息，也就是说隐含隐含了当前最新的观测值了当前最新的观测值yykk的信息，故称的信息，故称为新息为新息((InnovationInnovation))1ˆkkkyyCAx−=−1kw−kvky1kw−kv(1)0Buk−=kykyky††矩阵矩阵KK为可以使后验估计协方差为可以使后验估计协方差ppkk最小的增益矩阵。最小的增益矩阵。该矩阵的求解：将该矩阵的求解：将(7.2.4)(7.2.4)代入代入(7.2.1)(7.2.1)式，然后代入式，然后代入(7.2.3)(7.2.3)式计算式计算ppkk，，对对ppkk关于关于KK求导并令其为求导并令其为00，则可解出，则可解出KK。。11()()TTkkkTkkkkkKpCCpCRpApAQpIKCp−−−−−−=+=+=−增益方程均方误差方程(7.2.5)(7.2.6)(7.2.7)††从上式可以观察到：从上式可以观察到：††当测量误差的协方差阵当测量误差的协方差阵RR减小直至趋近于零时，增益矩减小直至趋近于零时，增益矩阵阵KK的作用逐渐增强，特别地的作用逐渐增强，特别地††当先验估计误差的协方差阵当先验估计误差的协方差阵减小直至趋近于零时，减小直至趋近于零时，增益矩阵增益矩阵KK的作用逐渐减弱，特别地的作用逐渐减弱，特别地10limRKC−→=0lim0kpK−→=(7.2.8)(7.2.9)kp−††卡尔曼滤波器采用了一种反馈的形式来估计状态－－卡尔曼滤波器采用了一种反馈的形式来估计状态－－卡尔曼滤波器的方程分为两部分：卡尔曼滤波器的方程分为两部分：1.1.时间更新方程。时间更新方程。该方程负责获得当前时刻状态的先验估计以及为下一该方程负责获得当前时刻状态的先验估计以及为下一时刻获得误差协方差估计。即由时刻获得误差协方差估计。即由kk--11时刻的状态和协方时刻的状态和协方差估计计算出差估计计算出kk时刻的相应值。时刻的相应值。11ˆˆkkTkkxAxpApAQ−−−−⎧=⎨=+⎩(7.2.10)†2.测量更新方程。测量更新方程负责反馈，将先验估计和新的测量值结测量更新方程负责反馈，将先验估计和新的测量值结合起来获得校正后的后验估计。合起来获得校正后的后验估计。首先计算卡尔曼增益首先计算卡尔曼增益KKkk，下一步通过实际的测量得到，下一步通过实际的测量得到yykk，然后，然后通过通过(7.2.12)(7.2.12)获得系统状态的后验估计。最后一步是获得后验估计获得系统状态的后验估计。最后一步是获得后验估计误差协方差阵。误差协方差阵。1()ˆˆˆ()()TTkkkkkkkkkkkKpCCpCRxxKyxpIKCp−−−−−−=+=+−=−(7.2.13)(7.2.12)(7.2.11)随着时间更新方程和测量更新方程的计算，系统状态随着时间更新方程和测量更新方程的计算，系统状态的估计被重复进行着：的估计被重复进行着：前一次的后验估计被用于预测前一次的后验估计被用于预测下一时刻的先验估计。下一时刻的先验估计。（）时间更新方程预测（）测量更新方程校正1.更新状态2.更新先验协方差估计1.计算卡尔曼增益2.用测量值更新状态估计3.更新误差协方差估计1kkxAx−∧∧−=1TkkpApAQ−−=+TkkTkpCKCpCR−−=+()kkkkkxxKyx−−∧∧∧=+−()kkkpIKCp−=−给定初始条件11,kkxp∧−−Kalman滤波器的特点††1.1.卡尔曼滤波器是以最小均方误差为准则的最佳线性估卡尔曼滤波器是以最小均方误差为准则的最佳线性估计和滤波。计和滤波。††2.2.卡尔曼滤波器只需要前一个状态的估计值和当前的观卡尔曼滤波器只需要前一个状态的估计值和当前的观测值就可以估计当前状态，是一种递推的方法。测值就可以估计当前状态，是一种递推的方法。††3.3.卡尔曼增益矩阵卡尔曼增益矩阵KK的计算与观测值无关，因此可以事的计算与观测值无关，因此可以事先计算增益矩阵，然后有一个观测值，就可以得到估计先计算增益矩阵，然后有一个观测值，就可以得到估计值，便于实时计算。值，便于实时计算。††4.4.模型误差的问题、模型误差的问题、A,B,CA,B,C时变、模型非线性的问题。时变、模型非线性的问题。††5.5.影响实时计算的障碍：状态维数影响实时计算的障碍：状态维数nn和增益矩阵的计和增益矩阵的计算。算。7.37.3举例举例†问题：从一系列电压测量值中估计真实值，真实的电压值被方差为0.1v的白噪声污染。††该问题可以由如下的随机过程来描述：该问题可以由如下的随机过程来描述：111kkkkkkkkkkkxAxBuwxwyCxvxv−−−=++=+⎧⎨=+=+⎩(7.3.1)即令即令10ABCu===⎧⎨=⎩(7.3.2)††为了用卡尔曼滤波器估计信号的真实值，方程如下：为了用卡尔曼滤波器估计信号的真实值，方程如下：11ˆˆkkkkxxppQ−−−−⎧=⎨=+⎩1()ˆˆˆ()(1)kkkkkkkkkkkKppRxxKzxpKp−−−−−−⎧=+⎪=+−⎨⎪=−⎩(7.3.3)(7.3.4)选定初始条件：选定初始条件：11ˆ0,1kkxp−−==††仿真时，我们假定将要估计的真实值为仿真时，我们假定将要估计的真实值为yy==--0.37727,0.37727,产生产生均值为均值为00，，0.10.1为方差的为方差的5050组随机数加上组随机数加上yy作为测量值。作为测量值。††取取20.10.01,15RQe===−†%parameterdecided†R=0.01;Q=1e-5;†N=50;%numberofiretation†%produceobservation†a=sqrt(R)*randn(1,N);†y=-0.37727+a;%meanis-0.37727andobservationvarisR.†%initialization†P=zeros(1,N);†x=zeros(1,N);†P(1)=1;†x(1)=0;†%startcal†fori=1:N-1†pp=P(i)+Q;†K(i)=pp/(pp+R);†x(i+1)=x(i)+K(i)*(y(i)-x(i));†P(i+1)=(1-K(i))*P(i);†end†%plot†t=1:N;†%plot(t,x,t,z,'x',t,P);†a(1,:)=-0.37727†plot(t,x,t,z,'x',t,a);11ˆˆkkkkxxppQ−−−−⎧=⎨=+⎩1()ˆˆˆ()(1)kkkkkkkkkkkKppRxxKzxpKp−−−−−−⎧=+⎪=+−⎨⎪=−⎩05101520253035404550-0.7-0.6-0.5-0.4-0.3-0.2-0.10红：真实值红：真实值黑：估计值黑：估计值蓝：测量值蓝：测量值0510152025303540455000.10.20.30.40.50.60.70.80.91计算计算5050步后，步后，ppkk的变化情况的变化情况KalmanKalman滤波器的网上资源滤波器的网上资源