www.pudn.com > Kalman.rar > ex1.m, change:2012-02-14,size:659b


%Kalman滤波 
clear 
N=800; 
w(1)=0; 
%系统预测的随机白噪声 
w=randn(1,N)  
x(1)=0; 
a=1; 
for k=2:N; 
%系统的预测值 
x(k)=a*x(k-1)+w(k-1);  
end 
%测量值的随机白噪声 
V=randn(1,N);  
q1=std(V); 
Rvv=q1.^2; 
q2=std(x); 
Rxx=q2.^2; 
q3=std(w); 
Rww=q3.^2; 
c=0.2; 
%测量值 
Y=c*x+V;  
p(1)=0; 
s(1)=0; 
for t=2:N; 
%前一时刻X的协方差系数 
p1(t)=a.^2*p(t-1)+Rww;  
%Kalman增益 
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);  
%经过滤波后的信号 
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));  
%t状态下x(t|t)的协方差系数 
p(t)=p1(t)-c*b(t)*p1(t);  
end 
subplot(131) 
plot(x) 
title('系统的预测值') 
subplot(132) 
plot(Y) 
title('测量值') 
subplot(133) 
plot(s) 
title('滤波后的信号')