|
这是一篇来自网上的matlab仿真卡尔曼滤波的程序。
根据卡尔曼滤波的步骤,我感觉这代码有问题。
clear
N=200;
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;
p1(t)=a.^2*p(t-1)+Rww;
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));
p(t)=p1(t)-c*b(t)*p1(t);
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');
根据卡尔曼的5个方程,Rww和Rvv分别是系统过程噪声的协方差和测量噪声协方差,噪声是高斯白噪声。
Rww和Rvv的来源分别是v和w,v和w都是又randn(1,N)产生的标准正态分布的随机数,这个可以理解。
但是求它们的标志差,再平方,得出相应的方差,带入卡尔曼公式中进行运算。
这样的话,它就用方差代替了协方差,这样不符合公式要求啊,我始终感觉这个有点问题。 |
阿莫论坛20周年了!感谢大家的支持与爱护!!
一只鸟敢站在脆弱的枝条上歇脚,它依仗的不是枝条不会断,而是自己有翅膀,会飞。
|