jerk模型算法

采用jerk模型算法对机动目标进行跟踪的实现

function [ MMX0 ] = Jerk_f（ sigmarsigmajalphaTMX0 ）
%UNtitleD2 Summary of this function goes here
%   该函数的目的是利用jerk模型算法对机动目标进行跟踪
%%%参数意义如下：MMX0，输出目标状态
%%%             sigmar，目标位置标准差
%%%             sigmaj，目标加加速度标准差
%%%             alpha，自相关时间常数
%%%             T，雷达采样时间间隔
%%%             MX0，目标位置量测值
%%
NN=length（MX0（1:））;%信号长度
%%%%%%%%%%参数初始化%%%%%%%%%
MMX0=zeros（8NN）;
%%%利用前三次量测值进行初始化
MMX0（11）=MX0（11）;
MMX0（12）=MX0（12）;
MMX0（13）=MX0（13）;
MMX0（22）=（MX0（12）-MX0（11））/T;
MMX0（23）=（MX0（13）-MX0（12））/T;
MMX0（33）=（MX0（13）-2*MX0（12）+MX0（11））/T;
MMX0（51）=MX0（21）;
MMX0（52）=MX0（22）;
MMX0（53）=MX0（23）;
MMX0（62）=（MX0（22）-MX0（21））/T;
MMX0（63）=（MX0（23）-MX0（22））/T;
MMX0（73）=（MX0（23）-2*MX0（22）+MX0（21））/T;
%%%%%%%%%%%%%%%%%%%%
F1=[1 T T^2/2 T^3/6;0 1 T T^2/2;0 0 1 T;0 0 0 1];
F=[F1 zeros（4）;zeros（4） F1];%%%状态矩阵
H=[1 0 0 0 0 0 0 0;
    0 0 0 0 1 0 0 0];%%%测量矩阵
Q1=2*alpha*sigmaj^2*[T^7/252 T^6/72 T^5/30 T^4/24;
                    T^6/72 T^5/20 T^4/8 T^3/6;
    T^5/30 T^4/8 T^3/3 T^2/2;T^4/24 T^3/6 T^2/2 T];
Q=[Q1 zeros（4）;zeros（4） Q1];%%%过程噪声协方差
R=sigmar^2*eye（2）;
P0=zeros（44）;
P0（11）=R（11）;
P0（12）=R（11）/T;
P0（13）=R（11）/T^2;
P0（21）=P0（12）;
P0（22）=2*R（11）/T^2;
P0（23）=3*R（11）/T^3;
P0（24）=5/6*sigmaj^2*T^2;
P0（31）=P0（13）;
P0（32）=P0（23）;
P0（33）=6*R（11）/T^4;
P0（34）=sigmaj^2*T;
P0（42）=P0（24）;
P0（43）=P0（34）;
P0（44）=sigmaj^2;
P=[P0zeros（4）;
    zeros（4）P0];%%%初始化信息协方差
%%%%%%%%%%%%%%%%%%%%%%%
X=MMX0（:3）;
%%
%%%%%卡尔曼滤波%%%%%%%
for k=4:NN
    X1=F*X;
    Z1=H*X1;
    P1=F*P*F‘+Q;
    S=H*P1*H‘+R;
    V=MX0（:k）-Z1;
    W=P1*H‘*inv（S）;
    X=X1+W*V;
    P=P1-W*S*W‘;
    MMX0（:k）=X;
end

end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1851  2015-03-12 19:47  Jerk_f.m