卡尔曼滤波.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('滤波后的信号')