卡尔曼滤波原理及公式

卡尔曼滤波是一种递归滤波器，可以对系统的状态进行最优估计。

该系统将卡尔曼滤波器运用在目标预测及跟踪上，已知使用卡尔曼滤波器的３个条件：

(1)系统是线性的；

(2)影响测量的噪声属于白噪声；

(3)噪声服从高斯分布。

在线性高斯系统中，运动方程、观测方程是线性的，且两个噪声项服从零均值的高斯分布。卡尔曼滤波就是根据上一时刻的估计值递推求解的过程。

卡尔曼滤波的基础数学方程为 状态方程与观测方程，式（１）和式（２）：

其中：xk和xk−1为k与k−1时刻的状态向量；zk为k时刻的观测向量；Akk−1为从k−1时刻到k时刻的状态转移矩阵；Hk为观测矩阵；ξｋ为系统噪声；ηｋ为观测噪声；Qk、Rk分别表示ξｋ和ηｋ的方差。

修正状态向量，式（４）：

修正误差协方差矩阵，式（５）：

状态向量预测方程，式（６）：

误差协方差预测方程，式（７）：

其中，式（６）和式（７）是状态预测过程，式子（３）、式（４）和式（５）则是对状态修正过程。

假定卡尔曼滤波的状态向量为式（８）：其中，ｘ(k) ｙ(ｋ) 是返回的视频中得到的目标中心点位置坐标，VxVy是通过计算得到的目标中心点在ｘ轴ｙ轴方向上的速度分量。）根据卡尔曼滤波公式，分别对Ａk，ｋ－１、Ｚｋ、Ｈｋ、Ｑ、Ｒ进行设定。由于无人机上摄像头返回的视频流中相邻帧之间的目标运动过程可近似看成匀速运动，因此为了便于计算，本文设定相邻两帧间的时间间隔Δｔ＝１，则状态向量的初始值可以表示为式（９）：

在基于此算法实现目标位置信息融合时，会出现两种情况：当目标物体被检测到时，卡尔曼滤波使用最新检测到的目标位置来更新状态，由此产生一个过滤的位置；当目标物体不出现在摄像头内时，卡尔曼滤波则完全依赖之前的状态信息预测当前帧中目标的位置。