当前模型卡尔曼滤波算法资料

%%% main of current function
%   其状态方程和量测方程如下：
%   X（k+1）=Ak+U*a+W;
%   Z（k+1）=HX（k）+V;
%   T为观测周期
%   A为转移矩阵;
%   H为观测矩阵;
%   U为控制矩阵 
%   obs为观测点数
%   M为合成运动轨迹
%   Xk（:1）是目标初始状态
%   Q 过程噪声协方差;
%  假设目标状态为 [x vx ax y vy ay]
%  aMax    最大加速度
%  aMin    最小加速度
%  alpha   机动参数

clc;
clear;
T=1;% 观测周期
obs = 500;  % 观测点数
% Xk（: 1） = [0; 10; 2; 0; 8; 3];  %状态向量初始值
% Xk =[Xk（: 1） zeros（6 obs-1）]; 
Xk =zeros（6 obs）;
Xk（: 1） = [0; 10; 1; 0; 8; 5];  %状态向量初始值
% Xk（: 2） = [11; 12; 3; 9; 10; 5];  %状态向量初始值
% Xk（: 3） = [23; 15; 3; 20; 14; 5];  %状态向量初始值

%Xk
%当前模型初始化
aMax = 9;   %最大加速度
aMin = -5;  %最小加速度
alpha = 1/20; %机动频率
sigma=0.1 ; %机动加速度初始方差
% [ A U H Q M ]= current_init（XkTobsaMaxaMinalpha）;
% M

A=[ 1  T  （-1+alpha*T+exp（-alpha*T））/alpha; 
    0 1 （1-exp（-alpha*T））/alpha;
    0 0 exp（-alpha*T）];
A=[A zeros（33）;
    zeros（33） A];

U=[ -（-T+alpha*T^2/2+（1-exp（-alpha*T））/alpha）/alpha;
    T-1/alpha*（1-exp（-alpha*T））;
    1-exp（-alpha*T）];
% U=[ U zeros（33）; zeros（33） U ];
% U1=[ U ;  zeros（31） ];
% U2=[  zeros（31）; U  ];
U=[ U zeros（31）; zeros（31） U ]; %注意矩阵格式

H=[ 1 0 0 0 0 0 ;
    0 0 0 1 0 0];

% 过程噪声
q11 = 1 / （2 * alpha ^ 5） * （1 - exp（-2 * alpha * T） + 2 * alpha * T + 2 * alpha ^ 3 * T ^ 3 / 3 ...
      - 2 * alpha ^ 2 * T ^ 2 - 4 * alpha * T * exp（-1 * alpha * T））;
q12 = 1 / （2 * alpha ^ 4） * （exp（-2 * alpha * T） + 1 - 2 * exp（-1 * alpha * T） ... 
     + 2 * alpha * T * exp（-1 * alpha * T） -2 * alpha * T + alpha ^ 2 * T ^ 2）;
q13 = 1 / （2 * alpha ^ 3） * （1 - exp（-2 * alpha * T） - 2 * alpha * T * exp（-1 * alpha * T））;
q22 = 1 / （2 * alpha ^ 3） * （4 * exp（-1 * alpha * T） - 3 - exp（-2 * alpha * T） + 2 * alpha * T）;
q23 = 1 / （2 * alpha ^ 2） * （exp（-2 * alpha * T） + 1 - 2 * exp（-1 * alpha * T））;
q33 = 1 / （2 * alp