?? 卡爾曼濾波.m
字號:
N=800;
w(1)=0;
w=randn(1,N); %系統預測的隨機白噪聲
s(1)=0;
a=1;
for k=2:N;
s(k)=a*s(k-1)+w(k-1); %系統的預測值
q3=std(w);
Rww=q3.^2;
end
n=randn(1,N); %測量值的隨機白噪聲
q1=std(n);
Rnn=q1.^2;
c=0.2;
x=c*s+n;
p(1)=0;
s0(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww; %前一時刻X的相關系數
b(t)=c*p1(t)/(c.^2*p1(t)+Rnn); %卡爾曼增益
s0(t)=a*s0(t-1)+b(t)*(x(t)-a*c*s0(t-1)); %經過濾波后的信號
p(t)=p1(t)-c*b(t)*p1(t);%t狀態下x(t|t)的相關系數
end
figure(1)
plot(s)
title('系統的預測值')
figure(2)
plot(x)
title('測量值')
figure(3)
plot(s0)
title('濾波后的信號')
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -