卡尔曼滤波器及其简matlab仿真

卡尔曼滤波器及其简matlab仿真一、卡尔曼滤波的起源谈到信号的分析与处理，就离不开滤波两个字。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文ANewApproachtoLinearFilteringandPredictionProblems（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。二、卡尔曼滤波的原理卡尔曼滤波思想的来源是在海图作业中，航海长通常以前一时刻的船位为基准，根据航向、船速和海流等一系列因素推算下一个船位，但是他并不轻易认为船位就一定在推算船位上，还要选择适当的方法，通过仪器得到另一个推算船位。观测和推算这两个船位一般不重合，航海长需要通过分析和判断选择一个可靠的船位，作为船舰当前的位置。就是以现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。其实质要解决的问题是要寻找在最小均方误差下KX的估计值^KX。它的特点是可以用递推的方法计算KX，其所需数据存储量较小，便于进行实时处理。具体来说，卡尔曼滤波就是要用预测方程和测量方程对系统状态进行估计。设动态系统的状态方程和测量方程分别为：11,11,KKKKKKKWXXKKKKVXHZ上两式子中，KX是k时刻的系统状态，1,KK和1,KK是k-1时刻到k时刻的状态转移矩阵，KZ是k时刻的测量值，KH是测量系统的参数，KW和KV分别表示过程和测量的噪声，他们被假设成高斯白噪声。如果被估计状态和观测量是满足上述第一式，系统过程噪声和观测噪声满足第二式的假设，k时刻的观测KX的估计^X可按下述方程求解。状态的一步预测：(1)均方误差进一步预测：(2)滤波增益矩阵：(3)滤波估计方程：(4)均方误差更新矩阵（K时刻的最优均方误差）：(5)上述就是卡尔曼滤波器的5条基本公式，只有给定初值0X和0P，根据k时刻的观测值KZ，就可以递推计算得k时刻的状态估计^KX。/1,11ˆˆkkkkkxx/1,11,1111ˆTTkkkkkkkkkkPPQ1/1/1ˆˆTTkkkkkkkkKPHHPHR/1/1ˆˆkkkkkkkxxKZHx/1ˆkkkkPIKHP下面论述卡尔曼五个公式的推导过程：设系统1111,kkkkkkxx（1）kkkkvxHzk=1（2）其中,动态噪声{k}与量测噪声{kv}是互不相关的零均值白噪声序列，对任意k,j其基本统计性质为:E{k}=0Cov(k,j)=E[kTj]=kjkQE{kv}=0Cov(kv,jv)=E[kvTjv]=kjkRCov(k,jv)=E[kTjv]=0其中kj是克罗内克函数，即：又设初始状态的统计特征为E[x0]=Varx0=E{(x0-)(x0-)T}=P0且x0与{k}，{kv}都不相关，即Cov(0x,k)=0Cov(0x,kv)=0在量测（k-1）次之后，已经有一个1/11ˆˆkkkxx的估计值，要推测k状态的状态值，因为E{1-k}=0，可定义1/ˆkkx为由k-1次量测值所估计值1ˆkx的一步预测合理数值，即11,1/ˆˆkkkkkxx（3）同样，考虑到E{kv}=0，因而量测的期望值为1ˆkkxH也是合适的。考虑到这两点以后利用第k次的量测数据kz来估计kxˆ的递推形式，其应该为：)ˆ(ˆˆˆ1/1//kkkkkkkkkkxHzKxxx（4）这里的kK是一个待定的增益矩阵，其应使误差矩阵极小。接下来推导误差方差公式。定义kkkkkxxx1/1/ˆ~（5）kkkkkkxxxx//ˆ~~（6）其中“~”表示误差，式（5）表示先验（没有测量值）的误差，式（6）表示后验的误差（经测量值校正）。则，根据式（6），将式（4），式（5）带入，得kkkkkkkkkkkkkkkkxkkkkkkkkkkkkkkkkkkvKxHKIxxHKxHKvKxHKxxxxHvxHKxxxx1/1/1/1/1/~)(~~)ˆ(ˆˆ~估计误差矩阵]})(~[])(~[~){(]~~[1/1/1/TkkTkkTkkkkTkTkTkkTkkkkkkTkkkKvHKIxvKKvHKIxxHKIExxEP（7）定义]~~[1/1/1/TkkkkkkxxEP（8）这就是一步预测误差矩阵。由模型的统计性质可知E[kvTkv]=kR（9）而且，预测误差与测量噪声不相关，即：0]~[][1/1/TkkkTkkkxvEvxE（10）将式（8），式（9），式（10）带入式（7），得TkkkTkkkkkkkKRKHKIPHKIP)()(1/（11）现寻找一个kK使式（11）最小。将式（11）右端展开后，加减同一项1/11/1/)(kkkkTkkkkTkkkPHRHPHHP再把有关kK的项归在平方项里，即])([)]()([)(11/1/1/11/1/1/11/1/1/kTkkkkTkkkkkkkkkkTkkkkTkkkkkkkkTkkkkTkkkkkkRHPHHPKRHPHRHPHHPKPHRHPHHPPP（12）在式（12）中，欲使kP极小，则11/1/)(kTkkkkTkkkkRHPHHPK（13）此时1/1/11/1/1/][)(kkkkkkkkTkkkkTkkkkkkPHKIPHRHPHHPPP（14）这就是误差迭代公式。由此可见，卡尔曼滤波的递推公式即可得到滤波的估计值，又可得到误差的方差阵。由式（3）两边同时减去kx得：kkkkkkkxxxx11,1/ˆ-ˆ(15)将式（1），式（5）带入式（15）1111,1/~~kkkkkkkxx因此TkkkTkkkkkkkQPP1111,11,1/（20）式（20）即一步预测估计误差矩阵。到此，卡尔曼滤波公式推导完毕。三、卡尔曼滤波的实例——以温度检测滤波为例假设我们要研究一个房间的温度，这个房间的真实温度是25度，以一分钟为时间单位。根据我们的经验判断，这个房间的温度是恒定的（A=1），但是对我们的经验不是完全相信，可能存在上下几度的偏差，我们把该偏差看做是高斯白噪声（系统噪声W）。另外，我们在房间里放一个温度计（H=1），温度计也不准确，测量值会与实际值存在偏差，我们也把这偏差看做是高斯白噪声（测量噪声V）。现在我们用卡尔曼滤波，过滤掉噪声，估算出房间的实际温度。系统参数名称解释如下：xk系统状态实际温度系统矩阵温度不变，为1B、uk状态的控制量无控制量，为0Zk观测值温度计读数H观测矩阵直接读出，为1wk过程噪声温度变化偏差，常量1e-1vk测量噪声读数误差，常量1e-6假如我们要估算2时刻房间的实际温度值。首先你要根据1时刻温度的估计值(就假设为1），来算出2时刻温度的估计值，即：=1（*公式1），然后由给出的1时刻的均方差（假设为10）进一步更新均方差，有=10.009（*公式2）。有方差之后，根据卡尔曼增益方程计算出增益：=0.925（*公式3）。现在，我们可以通过增益和测量值（Z=26.676，温度计有测量误差）来估计2时刻的温度了，=24.763（*公式4）。得到了2时刻的估计温度，下一步就是对3时刻的温度值进行最优估算，需要得到K时刻的最优温度（24.56）的偏差，算法如下：=0.751（*公式5）就这样，再进入3时刻的滤波循环，卡尔曼滤波器就不断的把均方误差递归，从而估算出最优的温度值，运行速度快。以下是matlab程序：2,11ˆˆxx1P2,111ˆTTkPPQ2,1P122,1ˆˆTTvvKPHHPHR22,12,1ˆˆˆxxKZHx22,1ˆPEKHPclearall;clc;closeall;%系统方程X(k)=AX(k-1)+BU(k)+W(k)%系统测量值Z(k)=HX(k)+V(k)N=200;w=0.1*randn(1,N);%产生随机高斯分布x(1)=0;%赋值与否无所谓a=1;%系统状态矩阵V=randn(1,N);%产生随机高斯分布q1=std(V);Rvv=q1.^2;%Rq2=std(w);Qww=q2.^2;%Qh=1;%观测矩阵Y=25+V;%测量误差值p(1)=10;X(1)=0;%%%%%%%%%%kalmanfilter%%%%%%%%%%%x(k)=x(k|k-1),X(k)=x(k|k),p1(k)=p(k|k-1),p(k)=p(k|k)fork=2:N;x(k)=a*X(k-1);%X(k|k-1)=AX(k-1|k-1)+BU(k)………(1)p1(k)=a*p(k-1)*a'+Qww;%P(k|k-1)=AP(k-1|k-1)A’+Q………(2)Kg(k)=p1(k)*h'/(h*p1(k)*h'+Rvv);%Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(4)X(k)=x(k)+Kg(k)*(Y(k)-h*x(k));%X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(3)p(k)=p1(k)-Kg(k)*h*p1(k);%P(k|k)=（I-Kg(k)H）P(k|k-1)………(5)endfori=1:NexpValue(i)=25;endk=1:N;plot(k,X,'r',k,Y,'g',k,expValue,'b');legend('真实值','估计值','测量值');axis([0N1030])xlabel('时间');ylabel('房间温度');title('卡尔曼滤波');滤波结果如下图：由上图可以看出，滤波效果比较明显，滤波后的温度基本等于房间的真实值25度，将温度测量的大部分误差都过滤掉了。从仿真结果可以看出，卡尔曼滤波器能够有效地在一定的噪声干扰下比较准确地反映真实值。