来自「提供一些matlab程序 不知可不可以」· 代码 · 共 39 行

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