基于卡尔曼滤波的二维目标跟踪（Matlab代码实现）

clc; 
clear all;
close all;
%% Video Initialization
video_name = 'sample3.mp4'; %Video name
vid = VideoReader(video_name); 
nframes = vid.NumberOfFrames; %Number of frames
Height = vid.Height; % Height :)
Width = vid.Width; % Width :)
thr = 10; % Threshold for generating binary image of the noise
%% Kalman Filter Definition
% First, we define the state of interest. In this case, we define the
% following variables for our states: state(t) = [X Y dx dy (d^2)x (d^2)y](t)
% X(t+1) = 1/2(a)T^2 + V(t)T + X(t); where a and V denotes the acceleration
% and velocity respectively.
% V(t+1) = aT + V(t)
% a(t+1) = a(t) ; assuming constant acceleration 
%State(t+1) = A.State(t) + B.u + <State Uncertainty|State Noise>
dt=0.5;
% A = [1 0 dt 0 (dt^2)/2 0;
%      0 1 0  dt 0    (dt^2)/2;
%      0 0 1  0  dt    0;
%      0 0 0  1  0     dt;
%      0 0 0  0  1     0;
%      0 0 0  0  0     1];
A = [1 0 dt 0;
     0 1 0 dt;
     0 0 1 0 ;
     0 0 0 1 ;
     ];
B = [(dt^2)/2 (dt^2)/2 dt dt]';
%B = [(dt^2)/2 (dt^2)/2 dt dt 1 1]';
% B=0;
% Input to the system (here acceleration).
u = 4e-3;
%We are observing the X, and Y position. So our observation is: y = [X Y]
%y(t) = H.State(t) + <Measurement Noise>
% H = [1 0 0 0 0 0
%      0 1 0 0 0 0];
H = [1 0 0 0;
     0 1 0 0];