kalman匀速直线目标跟踪matlab程序

%     设置初始化信息
%  N：设置卡尔曼滤波器追踪点数
%  r：设置估计变量个数，这里r=3
%  s：被追踪的火箭的距离，初始值为1000m
%  v：火箭的速度，初始值为50m/s
%  a：火箭的加速度，初始值为20m/s2，此时加速度默认为不变
%  T: 雷达的扫描间隔，此时设为1秒
%  wt: 系统噪声，方差为20
%  vt: 量测噪声，方差为16
%航迹噪声与测量噪声不相关
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
close all;
N = 100;
T = 1;
t = 1:N/T;
num=50 ;%mentocarlo仿真次数
qk=2;
Qk=qk*eye（2）;
var = 10;
Rk = var^2;    %量测噪声，方差为16

%%
%生成真实目标运动轨迹
[sx_realsy_realsz_realRMSE_xk]=trueTrack（NT）;
A = [1 T ;   0 1];
H = [1 0 ];
% A = [1 T 0 0;
%         0 1 0 0;
%         0 0 1 T;
%         0 0 0 1];
% H = [1 0 0 0; 0 0 1 0];
P0 = var^2 *eye（2）;
P1_x = P0;
P1_y = P0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波部分，继承之前初始化变量
% A：转移矩阵
% H：量测矩阵
% Qk：系统噪声矩阵
% Rk：量测噪声矩阵
% P0：均方误差矩阵初始值
% Y：火箭的状态矩阵，由k_s，k_vk_a组成
% Y0：状态矩阵的初始值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for m=1:num
    sx_measure = sx_real + var * rand（1N/T）; 
    sy_measure = sy_real + var * rand（1N/T）; 
   % sz_measure = sz_real + var * rand（1N/T）;     %引入测量噪声
    xks（1）=sx_measure（1）; 
    yks（1）=sy_measure（1）;
    xks（2）=sx_measure（2）;
    yks（2）=sy_measure（2）;
    vx = （xks（2） - xks（1））/T;
    vy = （yks（2） - yks（1））/T;
  
    Zkx=[xks（1）;vx];
    Zky=[yks（