您所在位置：主题：高手帮我看看基于kalman滤波的跟踪目标的简单matlab程序
80522957
[专家分：0] 发布于 2007-04-18 11:01:00
N=500;
w(1)=0;
v(1)=0;
S(1)=9;
V(1)=11;
for k=2:N;
w=randn(1,k);
v=randn(1,k);
S(k)=S(k-1)+V(k-1)+w(k-1)/2;%目标做匀速直线运动
V(k)=V(k-1)+w(k-1);
end
X=[S;V];
F=[1 1;0 1];
H=[1 0];
A=[1/2;1];
for k=2:N;
X=F*X+A*w;
Z=H*X+w;
end
q1=std(w);
Rw=q1.^2;
q2=std(v);
Rv=q1.^2;
x=[Z(2);Z(2)-Z(1)];%估计状态初始化
p=[1 1;1 2];%初始结束
for k=2:N;
x=F*x;
z=H*x;
m=Z-z;
p=F*p*F'+Rw;
S=H*p*H'+Rv;
s=inv(S);
for k=2:N;
K=p*H'*s;
x=x+K*m;
end
end
w(1)=0;
v(1)=0;
S(1)=9;
V(1)=11;
for k=2:N;
w=randn(1,k);
v=randn(1,k);
S(k)=S(k-1)+V(k-1)+w(k-1)/2;%目标做匀速直线运动
V(k)=V(k-1)+w(k-1);
end
X=[S;V];
F=[1 1;0 1];
H=[1 0];
A=[1/2;1];
for k=2:N;
X=F*X+A*w;
Z=H*X+w;
end
q1=std(w);
Rw=q1.^2;
q2=std(v);
Rv=q1.^2;
x=[Z(2);Z(2)-Z(1)];%估计状态初始化
p=[1 1;1 2];%初始结束
for k=2:N;
x=F*x;
z=H*x;
m=Z-z;
p=F*p*F'+Rw;
S=H*p*H'+Rv;
s=inv(S);
for k=2:N;
K=p*H'*s;
x=x+K*m;
end
end