一个应用实例详解卡尔曼滤波及其算法实现

5条公式。5Noise，254度。由于k2325Kg^2=5^2/(5^2+4^2)Kg=0.78k23+0.78*(25-23)=24.56度covariance比较小（比较相信温度计），因此k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现232.35k+1k时刻估算出的最优温度值的偏差（3）。就是这样，covariance递归，从而估算出最优的温度值。他运行的很快，（TheKalmanFilterDrKalman的卡尔曼滤波器。下面的描述，会涉及某些基本的概念知识，涉及概率（Probability），随机变量（RandomVariable），高斯或正态分派（GaussianDistribution）State-spaceModelStochasticDifferenceequation）来描述：%%X(k)=AX(k-1)+BU(k-再加上系统的测量值Z(k)=H上两式子中，X(k)k时刻的系统状态，U(k)k时刻对系统的控制量。AB是系统参数，对于多W(k)V(k)分别表达过程和测量的噪声。他们被假设成高斯白噪声(WhiteGaussianNoise)，他们的covarianceQ，R（这里我们假设他们不随系统状态变化而变化。对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息covarias（类似上一节那个温度的例子%%X(k|k-1)=AX(k-1|k-1)+BU(k+W(k)……….. （错X(k|k-1)=AX(k-1|k-1)+BU(k-1)+W(k-控制量0。到现在为止，我们的X(k|k-1)covariance(协方差)还没更新。Pcovariance：P(k|k-1)=AP(k-1|k-1)A’+Q………对应的能够得到现在状态(k)X(k|k)：X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………Kg为卡尔曼增益(KalmanKg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………kX(k|k)covariance（协方差）：P(k|k)=（I-Kg(k)H）P(k|k-1)………其中I为1的矩阵，对于单模型单测量，I=1k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。1，2，3，4555个公式，Matlab（ASimple精确。我们所懂得的这个房间的温度是跟前一时刻的温度相似的，因此A=1。没有控制量，因此X(k|k-1)=X(k-1|k- P(k|k-1)=P(k-1|k-1) 由于测量的值是温度计的，跟温度直接对应，因此H=1。式子3，4，5能够改成下列：X(k|k)=X(k|k-1)+Kg(k)(Z(k)-X(k|k-1)) Kg(k)=P(k|k-1)/(P(k|k-1)+ 25度，但是加入了原则偏差为几度的高斯白噪声（在图中为蓝线。X(0|0)P(0|0)。他们的值不用太在乎，随便给一种就能够了，由于随着卡尔曼的工作，XP，普X(0|0)=1度，P(0|0)=10。Q=1e-6，R=1e-1）。forfort=2:N;matlabkalman滤波程序,尚有下面一种Matlab源程序，显示效果更加好。CON25;%x=y2^0.5*randn(1,N)CON;%加过程噪声的状态输出x(1)=1;p=Qcov(randn(1,N));%Rcov(randn(1,N));%观察噪声协方差fork=2:Nx(k)x(k1);%k时刻状态变量的值p=p+Q;%对应于预估值的协方差kg=p/(p+R);%kalmangainx(k)=x(k)+kg*(y(k)-x(k));p=(1-kg)*p;Filter_Wid=10;smooth_res=zeros(1,N);fori=Filter_Wid+1:Ntempsum=0;forj=i-Filter_Wid:i-1tempsum=tempsum+y(j);smooth_res(i)=tempsum/Filter_Wid;%%hist(y);expValue=zeros(1,N);fori=1:NexpValue(i)=CON;legend('expected','estimate','measure','smoothresult');axis([0N2030])xlabel('Sampletime');ylabel('RoomTemperature');title('SmoothfilterVSkalman77mountai