基于MATLAB卡尔曼滤波器实现动态人物的跟踪检测

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用。例如在图像处理方面，应用卡尔曼滤波对由于某些噪声影响而造成模糊的图像进行复原。在对噪声作了某些统计性质的假定后，就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方差最小的真实图像，使模糊的图像得到复原。又如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

扩展卡尔曼滤波器，是由kalman filter考虑时间非线性的动态系统，常应用于目标跟踪系统。这一重要的属性，在诸多重要领域都能得到应用。因此掌握这一算法也是很有必要的。

先来介绍卡尔曼滤波的几个重要公式。
离散控制过程的系统，它可用一个线性随机微分方程来描述：
X(k)=A X(k-1)+B U(k)+W(k) ；
Z(k)=H X(k)+V(k) ，系统的测量值：其中，W(k)和V(k)分别表示过程和测量的噪声。对于其余的参数或函数的意义在此不再赘述。
X(k|k-1)=AX(k-1|k-1)+B U(k) ………… (1)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

这是卡尔曼滤波的五大公式，也是核心所在。代码在应用嵌套的时候也多是围绕这五个公式进行编程。它的主要思想在于通过系统的预测值和测量值不断的递归来得到最优的最真实的结果（针对最优，回归，分类，决策等问题我会做一个总结并罗列一下主要的算法。因为初学者对这些概念会混淆，他们之间略有不同又互相联系）。
下面列出我用卡尔曼滤波做的人脸跟踪检测。
clear;
clc;

if ~exist(‘result’)
mkdir(‘result’);
end%

% Kalman filter initialization，
%[p_x,p_y,v_x,v_y]
% Si+1 = AiSi + ni
% xi = HiSi + mi
dt=1;
A=[1,0,dt,0;0,1,0,dt;0,0,1,0;0,0,0,1];
H=[1,0,0,0;0,1,0,0];
P = 100eye(4);
Q=0.01eye(4);
R=[[0.2845,0.0045]’,[0.0045,0.0455]’];
KF_Init_flag = 1;

% VideoName = ‘move2.MOV’;
VideoName = ‘C:\Users\1.mp4’;
faceDetector = vision.CascadeObjectDetector();
videoReader = vision.VideoFileReader(VideoName);
obj_writer = VideoWriter(‘result\FaceTracker.avi’);
open(obj_writer);
obj = VideoReader(VideoName);
TotalFrameNum = obj.NumberOfFrames;
error = zeros(1,TotalFrameNum);
Predeict_center = zeros(TotalFrameNum,2);
Poster_center = zeros(TotalFrameNum,2);
clear obj;
k =1;
while ~isDone(videoReader)
if KF_Init_flag==1 %根据上一帧判断跟踪目标是丢失
videoFrame = step(videoReader);
bboxs = step(faceDetector,videoFrame);% Draw the returned bounding box around the detected face.给人脸画框显示
bboxs_area = bboxs(:,3).*bboxs(:,4);%.*两矩阵相同位置相乘，结果在相同位置
[bbox_area,index] = sort(bboxs_area,‘Descend’);%sort对数组按指定方向（下降方向）进行升序排序
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),0,0]’;
KF_Init_flag =0;
end
else
bbox = [];
end
else %若上一帧判断跟踪目标没有丢失，则对跟踪目标在本帧的位置进行预测（先验）
State = A * State;
Predict_box = [State(1:2)’-box_size/2,box_size];
%检查目标是否出界%【
if Predict_box(1)> size(videoFrame,2)|| Predict_box(2)> size(videoFrame,1)
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(videoFrame,2)
Predict_box(3) = size(videoFrame,2) - Predict_box(1);
end
if Predict_box(2) + Predict_box(4) > size(videoFrame,1)
Predict_box(4) = size(videoFrame,1) - Predict_box(2);
end
end %】
%根据对运动目标的观测值对状态向量进行修正更新（后验）
videoFrame = step(videoReader);
bboxs = step(faceDetector,videoFrame);
bboxs_area = bboxs(:,3).*bboxs(:,4);
[bbox_area,index] = sort(bboxs_area,‘Descend’);
if ~isempty(bbox_area)
if bbox_area(1)<12000
bbox = [];
KF_Init_flag = 1;
else
bbox = bboxs(index(1);
box_size = bbox(3:4);%实测
center = bbox(1:2)’+bbox(3:4)’/2;

           PP = A*P*A' + Q;%卡尔曼公式2
           K = PP*H'*inv(H*PP*H'+R);%inv取反
           State = State + K*(center-H*State);
           P = (eye(4)-K*H)*PP;%卡尔曼公式
           
           bbox = [State(1:2)'-box_size/2,box_size];%后验修正
            %检查是否出界
            if bbox(1) > size(videoFrame,2) ||bbox(2) > size(videoFrame,1)
                bbox = [];
                KF_Init_flag = 1;
            else
                if bbox(1) < 0
                    bbox(1) = 0;
                end
                if bbox(2) < 0
                   bbox(2) = 0; 
                end
                if bbox(1) + bbox(3) > size(videoFrame,2)
                    bbox(3) = size(videoFrame,2) - bbox(1);
                end
                if bbox(2) + bbox(4) > size(videoFrame,1)
                    bbox(4) = size(videoFrame,1) - bbox(2);
                end
            end
        end
    else
        bbox = [];
        KF_Init_flag = 1;
    end
end

videoOut = insertObjectAnnotation(videoFrame,'rectangle',Predict_box,'Predict','Color','yellow');%在指定位置上返回用形状和标签注释的真彩色图像
videoOut = insertObjectAnnotation(videoOut,'rectangle',bbox,'Modify','Color','red');
imwrite(videoOut, strcat('result\',num2str(k),'.jpg'));
writeVideo(obj_writer,videoOut);

% 预测值与修正值的误差
if isempty(Predict_box) || isempty(bbox)%isempty（a），判断是否为空是空返回1，非空返回0
   error(k) = 0; 
   if k >1
      Predeict_center(k,:) = Predeict_center(k-1,:);%k-1时刻，k时刻
      Poster_center(k,:) =  Poster_center(k-1,:);
   end
else
    Predeict_center(k,:) = Predict_box(1:2) + Predict_box(3:4)/2;
    Poster_center(k,:) = bbox(1:2) + bbox(3:4)/2;
    error(k) = sqrt(sum((Poster_center(k,:) - Predeict_center(k,:)).^2));
end
k =k+1;

end
close(obj_writer);
release(videoReader);
if bbox(2) < 0
bbox(2) = 0;
end
if bbox(1) + bbox(3) > size(videoFrame,2)
bbox(3) = size(videoFrame,2) - bbox(1);
end
if bbox(2) + bbox(4) > size(videoFrame,1)
bbox(4) = size(videoFrame,1) - bbox(2);
end
end
end
else
bbox = [];
KF_Init_flag = 1;
end
end

videoOut = insertObjectAnnotation(videoFrame,'rectangle',Predict_box,'Predict','Color','yellow');%在指定位置上返回用形状和标签注释的真彩色图像
videoOut = insertObjectAnnotation(videoOut,'rectangle',bbox,'Modify','Color','red');
imwrite(videoOut, strcat('result\',num2str(k),'.jpg'));
writeVideo(obj_writer,videoOut);

这个代码已删掉一部分，但前面部分已全部完成，只剩下最后的预测和测量误差的递归。。我处理的是动态视频中的人脸，但CSDN只能上传图片，就用图片代替吧。。