TBD 检测前跟踪

clear all
close all
clc

tf=100;
amaxx1=0.01;
eff=800;
N=1000;
% N=2000;
for t=1:tf
    truy（t）=1/100*t*t+t+100;
end
T=1;
% F=[1 T 1/2*T*T;0 1 T;0 0 1];
F=[1 T;0 1];
G1=[1/6*amaxx1*T^2 0;1/3*amaxx1*T 0];
% wv=0.06;
% F=[1 T 1/2*T*T;0 1 T;0 0 sin（wv*T）];
q=1/N*ones（1N）;%权重初始化
P1=1;
R=100;
a=100;
b=500;
A=rand（1N）;
B=randn（1N）;
s（1:）=a+（A-0.5）;
s（2:）=0+1.5.*（A-0.5）;
% s（2:）=1+1.*（A-0.5）;
% s（3:）=0+0.01.*（A-0.5）;
s2 = [s];
m=zeros（1N）;
mm=[m];
% q=ones（1N）;
  menxian=ones（1tf）;
Q = 0.25; % process noise covariance
for k=1:tf 
    i=1:500;
    i=i‘;
    temp=randn（5001）;
    z（:k）=rectpuls（i-1/100*k*k-k-1003）+0.5*temp;
    z（:101） = 500:-1:1;
    B = sortrows（z101）;
    B（:101） = [];
    y=B（1:5001:100）;
    r=（find（y（:k）>0.5））;%阀值为0.5
    guancezhi=r‘;
    gc=size（guancezhi2）;
    ll=zeros（1gc）;
%step2 每个粒子表示目标有无状态的转移
   for i=1:N 
        if m（i）==0
            if rand>0.9
                mm（i）=1;
            else
                mm（i）=0;
            end
        elseif m（i）==1
            if rand<0.1
                mm（i）=0;
            else
                mm（i）=1;
            end
        end
    end

    m=[mm];
    for i=1:N
        if m（i）==1
           s（:i）=F*s（:i）+0.1*（rand（21））; %继承粒子状态
        elseif m（i）==0
            s（:i）=s2（:i）;
        end
        if m（i）==1
            for g=1:gc
               temp=r（g）;
              if（abs（temp-s（1i））<=5） %波动范围是5
                 ll（g）=1/abs（temp-s（1i））;
              end
            end
            lsum=sum（ll）;
%  q（i）=q（i）*（1+lsum）;
            q（i）=q（i）*（lsum）;
         elseif m（i）==0
            q（i）=1;