design_EKF

   X_0=[x,y,z,vx,vy,vz]'
   y_0=[ul_0,vl_0,ur_0,vr_0]'


   A = [1      0      0      delt_T  0       0 ;
        0      1      0      0       delt_T  0 ;
        0      0      1      0       0       delt_T ;
        0      0      0      1       0       0 ;
        0      0      0      0       1       0 ;
        0      0      0      0       0       1 ] ;
        

   R = [1 0 0 0 ;
       0 1 0 0 ;
       0 0 1 0 ;
       0 0 0 1 ;];          


   Q = [1     0     0       0       0       0;
        0     1     0       0       0       0;
        0     0     1       0       0       0;
        0     0     0       0.001   0       0;
        0     0     0       0       0.001   0;
        0     0     0       0       0       0.001];

   

   p_0 = [0.1   0     0       0       0       0;
          0     0.1   0       0       0       0;
          0     0     0.1     0       0       0;
          0     0     0       0.1     0       0;
          0     0     0       0       0.1     0;
          0     0     0       0       0       0.1];



当前时刻 k,上一时刻，k-1

ekf初始
根据前两帧的x,y,z估计 k-1时刻的vx,vy,vz
vx = x[1]-x[0];
vy = y[1]-y[0];
vz = z[1]-z[0];


(1)状态更新
if ID ==1 {};//第一帧不更新
else
{
读取 Y(k)(TT时刻)
 
e  = Y(k) - y(k)
X(k) = x(k)+K(k-1)e
}

(2)协方差阵更新
if ID ==1 {};//第一帧不更新
else
P=[I-KH]P


(3)状态一步预测
X = FX

(4)协方差阵预测
P = FPF(T)+Q;

(5)滤波增益矩阵
H 雅克比矩阵求出：
//
//
//
//
//
K = PH(T)[HPH(T)+R](-1)


（6）ROI区域估计
 y(k)
 [ul_p,vl_p,ur_p,vr_p]=h(x,y,z)  （TT时刻）
 保存到文档
 返回跟踪算法
 (延时20ms)