clear;
clc;

if?~exist（‘result‘）
????mkdir（‘result‘）;
end

%?Kalman?filter?initialization
%[p_xp_yv_xv_y]??
%?Si+1?=?Ai*Si?+?ni
%?xi?=?Hi*Si?+?mi
dt=1;
A=[10dt0;010dt;0010;0001];?
H=[1000;0100];
P?=?100*eye（4）;?%狀態(tài)向量協(xié)方差
Q=0.01*eye（4）;?%系統(tǒng)動態(tài)噪聲
R=[[0.28450.0045]‘[0.00450.0455]‘];?%觀測噪聲
KF_Init_flag?=?1;

%?VideoName?=?‘move2.MOV‘;%待檢測視頻的文件名
VideoName?=?‘video\HBY09.mov‘;%待檢測視頻的文件名
faceDetector?=?vision.CascadeobjectDetector（）;
videoReader?=?vision.VideoFileReader（VideoName）;
obj_writer?=?VideoWriter（‘result\FaceTracker.avi‘）;
open（obj_writer）;
obj?=?VideoReader（VideoName）;
TotalframeNum?=?obj.NumberOfframes;
error?=?zeros（1TotalframeNum）;
Predeict_center?=?zeros（TotalframeNum2）;
Poster_center?=?zeros（TotalframeNum2）;
clear?obj;
k?=1;
while?~isDone（videoReader）
????if?KF_Init_flag==1?%根據(jù)上一幀判斷跟蹤目標(biāo)是丟失
????????videoframe??=?step（videoReader）;
????????bboxs?=?step（faceDetectorvideoframe）;
????????bboxs_area?=?bboxs（:3）.*bboxs（:4）;
????????[bbox_areaindex]?=?sort（bboxs_area‘Descend‘）;
????????Predict_box?=?[];
????????if?~isempty（bbox_area）
????????????if?bbox_area（1）<12000
???????????????bbox?=?[];???????????????
????????????else
???????????????bbox?=?bboxs（index（1）:）;
???????????????box_size?=?bbox（3:4）;%后驗測量
???????????????State?=?[bbox（1:2）+bbox（3:4）00]‘;
???????????????KF_Init_flag?=0;
????????????end
????????else
????????????bbox?=?[];????????????
????????end
????else??%若上一幀判斷跟蹤目標(biāo)沒有丟失，則對跟蹤目標(biāo)在本幀的位置進(jìn)行預(yù)測（先驗）
????????State?=?A?*?State;
????????Predict_box?=?[State（1:2）‘-box_size/2box_size];
????????%檢查是否出界
????????if?Predict_box（1）>?size（videoframe2）||?Predict_box（2）>?size（videoframe1）
????????????Predict_box?=?[];
????????else
????????????if?Predict_box（1）<0
???????????????Predict_box（1）?=?0;?
????????????end
????????????if?Predict_box（2）<0
????????????????Predict_box（2）?=?0;
????????????end
????????????if?Predict_box（1）?+?Predict_box（3）?>?size（videoframe2）
????????????????Predict_box（3）?=?size（videoframe2）?-?Predict_box（1）;
????????????end
????????????if?Predict_box（2）?+?Predict_box（4）?>?size（videoframe1）
????????????????Predict_box（4）?=?size（videoframe1）?-?Predict_box（2）;
????????????end
????????end
????????%根據(jù)對運(yùn)動目標(biāo)的觀測值對狀態(tài)向量進(jìn)行修正更新（后驗）
????????videoframe??=?step（videoReader）;
????