一個應(yīng)用實例詳解卡爾曼濾波及其算法實現(xiàn)

5條公式。5Noise，254度。由于k2325Kg^2=5^2/(5^2+4^2)Kg=0.78k23+0.78*(25-23)=24.56度covariance比較?。ū容^相信溫度計），因此k時刻的最優(yōu)溫度值了，下一步就是要進入k+1時刻，進行新的最優(yōu)估算。到現(xiàn)232.35k+1k時刻估算出的最優(yōu)溫度值的偏差（3）。就是這樣，covariance遞歸，從而估算出最優(yōu)的溫度值。他運行的很快，（TheKalmanFilterDrKalman的卡爾曼濾波器。下面的描述，會涉及某些基本的概念知識，涉及概率（Probability），隨機變量（RandomVariable），高斯或正態(tài)分派（GaussianDistribution）State-spaceModelStochasticDifferenceequation）來描述：%%X(k)=AX(k-1)+BU(k-再加上系統(tǒng)的測量值Z(k)=H上兩式子中，X(k)k時刻的系統(tǒng)狀態(tài)，U(k)k時刻對系統(tǒng)的控制量。AB是系統(tǒng)參數(shù)，對于多W(k)V(k)分別表達過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的covarianceQ，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化。對于滿足上面的條件(線性隨機微分系統(tǒng)，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息covarias（類似上一節(jié)那個溫度的例子%%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。到現(xiàn)在為止，我們的X(k|k-1)covariance(協(xié)方差)還沒更新。Pcovariance：P(k|k-1)=AP(k-1|k-1)A’+Q………對應(yīng)的能夠得到現(xiàn)在狀態(tài)(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（協(xié)方差）：P(k|k)=（I-Kg(k)H）P(k|k-1)………其中I為1的矩陣，對于單模型單測量，I=1k+1狀態(tài)時，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) 由于測量的值是溫度計的，跟溫度直接對應(yīng)，因此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;%加過程噪聲的狀態(tài)輸出x(1)=1;p=Qcov(randn(1,N));%Rcov(randn(1,N));%觀察噪聲協(xié)方差fork=2:Nx(k)x(k1);%k時刻狀態(tài)變量的值p=p+Q;%對應(yīng)于預(yù)估值的協(xié)方差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