kalman滤波器VD、CA、CV算法实现目标跟踪

用VD算法和CA算法还有CV算法实现kalman滤波器预测动目标轨迹，来实现动目标跟踪，采用的是matlab编程，仿真数据实现。

clear all;
close all;
clc
T=10;
alpha=0.8;            %  加权衰减因子
window=round（1/（1-alpha））; %  检测机动的有效窗口长度
dt=100;               %  dt=dt_x=dt_y=100
Ta=20;              %  退出机动的检测门限
Th=100;              %  机动检测门限
N=1200/T;              %　采样次数
M=50;                 %　模拟次数

%　真实轨迹数据
t=T:T:400;
    xo0=2000+0*t;
    yo0=10000-15*t;
t=400+T:T:600;
    xo1=2000+0.075*（（t-400）.^2）/2;
    yo1=10000-15*400-（15*（t-400）-0.075*（（t-400）.^2）/2）;
t=600+T:T:610;
    xo2=xo1（length（xo1））+15*（t-600）;
    yo2=yo1（length（xo1））+0*t;   
t=610+T:T:660;
    xo3=xo2（length（xo2））+（15*（t-610）-0.3*（（t-610）.^2）/2）;
    yo3=yo2（length（xo2））-0.3*（（t-610）.^2）/2;
t=660+T:T:1200;
    xo4=xo3（length（xo3））+0*t;
    yo4=yo3（length（xo3））-15.*（t-660）;
   
x=[xo0xo1xo2xo3xo4];
y=[yo0yo1yo2yo3yo4];

e_x1=zeros（1N）;
e_x2=zeros（1N）;
e_y1=zeros（1N）;
e_y2=zeros（1N）;
px=zeros（1N）;
qy=zeros（1N）;
u=zeros（1N）;
u_a=zeros（1N）;

U = [10000500;50010000];
R=chol（U）;
for j=1:M
    Q = randn（2N）;
    Noise = R*Q;
  for i=1:N;
    zx（i）=x（i）+Noise（1i）;      %  观测数据
    zy（i）=y（i）+Noise（2i）;
    z（:i）=[zx（i）;zy（i）];
  end
  
%
X_estimate（2:）=[zx（2）（zx（2）-zx（1））/Tzy（2）（zy（2）-zy（1））/T];
X_est=X_estimate（2:）;
P_estimate=[dt^2dt^2/T00;dt^2/T（dt^2）*2/（T^2）00;00dt^2dt^2/T;00dt^2/T（dt^2）*2/（T^2）];
x1（1）=zx（1）; y1（1）=zy（1）; x1（2）=X_estimate（21）; y1（2）=X_estimate（23）;
u（1）=0; u（2）=0; 
u1=u（2）;

pp=1;% ０表示非机动，１表示机动
qq=1;
rr=1;
k=3;
while k<=N    
    if k<=20
        z1=z（:k）;
        [X_preX_estP_estimateu1]=kalmanstatic（X_estP_estimatez1ku1alpha）;
        X_estimate（k:）=X_est;
        X_predict（k:）=X_pre;
        P（k:）=[P_estimate（11）P_estimate（12）P_estimate（22）P_estimate（33）P_estimate（34）P_estimate（44）];
        x1（k）=X_estimate（k1）;
        y1（k）=X_estimate（k3）;
        u（k）=u1;
        k=k+1;
    else
        if pp==0  %　进入非机动模型
            if rr==window+1 %　由机动进入非机动模型，为防止回朔，至少递推window＋1次
                u1=0;
            else 
            end
            while rr>0
            z1=z（:k）;
            [X_preX_estP_estimateu1]=kalmanstatic（X_estP_estimatez1ku1alpha）;
            X_estimate（k:）=X_est;
            X_predict（k:）=X_pre;
            P（k:）=[P_estimate（11）P_estimate（12）P_estimate（22）P_estimate（33）P_estimate（34）P_estimate（44）];
            x1（k）=X_estimate（k1）;
            y1（k）=X_estimate（k3）;
            u（k）=u1;
            rr=rr-1;
            end
            rr=1;
            if u（k）>=Th
                pp=1;qq=1; %　“pp=1，qq=1”表示由非机动进入机动模型
            else
            end
            k=k+1;
        else   %　机动模型
            if qq==1   %由非机动进入机动模型，需要进行修正
                k=ceil（k-window-1）;  
                Xm_est=[X_estimate（k-1:）00];
                Xm_pre=[X_predict（k:）00];
                Pm_estimate=zeros（66）;
                P=P（k-1:）;
                m=0;
            else      %在机动模型中进行递推

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       5952  2018-05-07 20:02  vd1\vd1\CA.m

     文件       6001  2018-05-07 20:03  vd1\vd1\CV.m

     文件       2465  2018-05-07 17:25  vd1\vd1\kalmandynamic.m

     文件        852  2018-05-07 17:07  vd1\vd1\kalmanstatic.m

     文件       7153  2018-05-21 21:18  vd1\vd1\main.m

     文件         38  2018-05-07 20:06  vd1\vd1\说明.txt

     目录          0  2018-05-21 21:08  vd1\vd1

     目录          0  2018-05-21 21:08  vd1

----------- ---------  ---------- -----  ----

                22461                    8