Matlab 卡爾曼濾波代碼

n = length(x);%輸入信號長度
%設置參數
y(1,1)=x(1,1); %初始值
p(1)=0; %誤差協方差初始值
R=2^-4;%高斯分佈的測量噪聲
Q=2^-6;%高斯分佈的過程噪聲
for i=2:n
    k(i,1)=p(i-1)/(p(i-1)+R);
    y(i,1)=y(i-1)+k(i)*(x(i)-y(i-1));
    p(i,1)=(1-k(i,1))*p(i-1)+Q;
end

%y爲輸出信號，濾波效果