0718.m

JPDA算法,仿真MATLAB 大家仔细学习啊

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);
plot(S(k)'r',V(k)'g'),xlabel('采样次序'),ylabel('系统输出')
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];%初始结束