基於matlab模擬對卡爾曼濾波的理解
阿新 • • 發佈：2019-01-28
clear clc; N = 200; t = 1:N; w = randn(1,N); %噪聲w x(1) = 0; for i = 2:N x(i) = x(i-1)+w(i-1); end subplot(311); plot(t,x); %輸出帶噪聲函式 xlabel('原始資料'); v = randn(1,N); %噪聲v c = 0.2; %同樣一個干擾係數 y = c*x+v; %輸出帶噪聲函式 q1 = std(v); q2 = std(w); p(1) = 2; %協方差初始化 Q = q2.^2; %預測（過程）噪聲方差 R = q1.^2; %測量（觀測）噪聲方差 c = 0.2 %一個干擾係數 for k = 2:N x(k) = x(k-1); %上一次的最優解賦值，用於計算當前最優解 p(k) = p(k-1)+Q; %估算預測協方差偏差 Kg(k) = c*p(k)/(c^2*p(k)+R); %計算誤差增益 x(k) = x(k)+Kg(k)*(y(k)-c*x(k)); %修正 當前最優值為下次計算 p(k) = (1-c*Kg(k))*p(k); %修正 協方差為下次計算 end subplot(312); plot(t,x); xlabel('卡爾曼濾波結果'); Filter_Width = 10; %對x滑動濾波處理 Smooth_Result = zeros(1,N); for i = Filter_Width+1:N Temp_Sum = 0; for j = i-Filter_Width:(i-1) Temp_Sum = x(j)+Temp_Sum; end Smooth_Result(i) = Temp_Sum/Filter_Width; end subplot(313); plot(t,y); plot(t,y,'-g',t,x,'-k',t,Smooth_Result,'-m'); legend('measure','estimate','smooth_result'); xlabel('Kalman Filter Simulation');
