卡爾曼濾波器和matlab代碼-資料下載頁
2025-06-20 03:42本頁面
【正文】 ,t,x,39。b39。)。%紅色卡爾曼，綠色觀測值，藍(lán)色狀態(tài)值legend(39。kalman estimate39。,39。ovservations39。,39。truth39。)。 最速下降法clcclear allN=30。q=。%q1amp。amp。q2/Ryx最大特征值hn=zeros(1,N)。hn(:)=5。vg=0。Rxx=xcorr(1)。Ryx=min(min(corrcoef(1, 1+randn)))。echo offfor i=1:N1。 %vg=2*Rxx*hn(:,i)2*Ryx。 %hn(:,i+1)=hn(:,i)1/2*q*vg。 vg=2*Rxx*hn(i)2*Ryx。 hn(i+1)=hn(i)1/2*q*vg。 m(i)=1。endt=1:N1。plot(t,hn(t),39。r39。,t,m(t),39。b39。)。 后聯(lián)平滑濾波器的卡爾曼濾波器clearclc。N=300。CON = 5。 x = zeros(1,N)。x(1) = 1。p = 10。Q = randn(1,N)*。%過程噪聲協(xié)方差R = randn(1,N)。%觀測噪聲協(xié)方差y = R + CON。%加過程噪聲的狀態(tài)輸出for k = 2 : N Q1 = cov(Q(1:k1))。%過程噪聲協(xié)方差 Q2 = cov(R(1:k1))。 x(k) = x(k 1)。%預(yù)估計k時刻狀態(tài)變量的值 p = p + Q1。%對應(yīng)于預(yù)估值的協(xié)方差 kg = p / (p + Q2)。%kalman gain x(k) = x(k) + kg * (y(k) x(k))。 p = (1 kg) * p。endFilter_Wid = 10。smooth_res = zeros(1,N)。kalman_p = zeros(1,N)。for i = Filter_Wid + 1 : N tempsum = 0。 kalman_m = 0。 for j = i Filter_Wid : i 1 tempsum = tempsum + y(j)。 kalman_m = kalman_m+x(j)。 end kalman_p(i) = kalman_m/Filter_Wid。 smooth_res(i) = tempsum / Filter_Wid。%平滑濾波end% figure(1)。% hist(y)。t=1:N。figure(1)。expValue = zeros(1,N)。for i = 1: N expValue(i) = CON。endplot(t,expValue,39。r39。,t,x,39。g39。,t,y,39。b39。,t,smooth_res,39。k39。,t,kalman_p,39。m39。)。legend(39。truth39。,39。estimate39。,39。measure39。,39。smooth result39。,39。smooth kalman39。)。%axis([0 N 0 30])xlabel(39。Sample time39。)。ylabel(39。Room Temperature39。)。title(39。Smooth filter VS kalman filter39。)。 專業(yè)整理分享
數(shù)學(xué)相關(guān)推薦
鄂ICP備17016276號-1