卡爾曼濾波器及其簡(jiǎn)matlab仿真-資料下載頁(yè)
2025-06-20 05:38本頁(yè)面
【正文】 狀態(tài)矩陣V=randn(1,N)。 %產(chǎn)生隨機(jī)高斯分布q1=std(V)。Rvv=q1.^2。 %Rq2=std(w)。Qww=q2.^2。 %Qh=1。 %觀測(cè)矩陣Y=25+V。 %測(cè)量誤差值p(1)=10。X(1)=0。%%%%%%%%%% kalman filter %%%%%%%%%%%x(k)=x(k|k1),X(k)=x(k|k),p1(k)=p(k|k1),p(k)=p(k|k)for k=2:N。x(k)=a*X(k1)。 % X(k|k1)=A X(k1|k1)+B U(k) ……… (1)p1(k)=a*p(k1)*a39。+Qww。 % P(k|k1)=A P(k1|k1) A’+Q ……… (2)Kg(k)=p1(k)*h39。/(h*p1(k)*h39。+Rvv)。 % Kg(k)= P(k|k1) H’ / (H P(k|k1) H’ + R) ……… (4)X(k)=x(k)+Kg(k)*(Y(k)h*x(k))。 % X(k|k)= X(k|k1)+Kg(k) (Z(k)H X(k|k1)) ……… (3)p(k)=p1(k)Kg(k)*h*p1(k)。 % P(k|k)=（IKg(k) H）P(k|k1) ……… (5)endfor i = 1: N expValue(i) = 25。 end k=1:N。plot(k,X,39。r39。,k,Y,39。g39。,k,expValue,39。b39。)。legend(39。真實(shí)值39。,39。估計(jì)值39。,39。測(cè)量值39。)。axis([0 N 10 30])xlabel(39。時(shí)間39。)。ylabel(39。房間溫度39。)。title(39。卡爾曼濾波39。)。濾波結(jié)果如下圖： 由上圖可以看出，濾波效果比較明顯，濾波后的溫度基本等于房間的真實(shí)值25度，將溫度測(cè)量的大部分誤差都過(guò)濾掉了。從仿真結(jié)果可以看出，卡爾曼濾波器能夠有效地在一定的噪聲干擾下比較準(zhǔn)確地反映真實(shí)值。
語(yǔ)文相關(guān)推薦
鄂ICP備17016276號(hào)-1