卡尔曼滤波OpenCV源码解析

卡尔曼滤波核心

参考文献：
https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
https://simondlevy.academic.wlu.edu/kalman-tutorial/
翻译原文没意思，关键要理解每个矩阵的含义，核心计算是下面的公式：

OpenCV代码详解

函数接口如下，主要的成员函数是：init()、predict()、correct()

init()

DP=dynamParams，MP=measureParams；
statePre：先验估计X(k|k-1)，statePost是后验估计X(k|k)；
transitionMatrix：[DP,DP]的转移矩阵A，对角矩阵；
processNoiseCov：[DP,DP]的过程误差协方差矩阵Q，对角矩阵；
measurementMatrix：[MP,DP]的测量矩阵H，全零；
meaurementNoiseCov：[MP,MP]的测量误差协方差矩阵R，对角矩阵；
errorCovPre：[DP,DP]的先验估计误差的协方差矩阵P(k|k-1)，全零；
errorCovPost：[DP,DP]后验估计误差的协方差矩阵P(k|k)，全零；
gain：[DP,MP]的卡尔曼增益矩阵K，全零（注意矩阵H和矩阵K的shape是相反的）；

以上参数中，每个时刻更新的是：statePre、statePost、errorCovPre、errorCovPost、gain(K)。固定不变的是：transitionMatrix(A)、measurementMatrix(H)、processNoiseCov(Q)、meaurementNoiseCov(R)。

predict()

计算过程与公式一一对应。注意通常矩阵B是用不到的，可以设为空矩阵省略掉。x(k)是每个时刻k不断预测和更新的状态变量，结合后面的例子体会。

gemm()对应公式：dst=alphasrc1src2+beta*src3，flag用来表示src1/2/3是否要转置，例如代码中的GEMM_2_T表示src2是转置的。

开头的CV_INSTUMENT_REGION()是OpenCV相关算法表现性能测试框架，测量函数执行时间，可以在CMake中define ENABLE_INSTRUMENTATION 来使能追踪测试，默认不应用次选项，所以不消耗资源。

correct()

这里要给矩阵temp3求逆矩阵，用SVD分解。注意协方差矩阵P(k)、P’(k)、R是对称矩阵，推知temp3也是对称的。

求逆矩阵的solve()函数定义如下。在源码中看到，如果是对2x2、3x3的small shape矩阵求逆，有直接的公式计算，但对于shape更大的矩阵，就需要最小二乘法甚至迭代求解，源码没复杂暂时没有深入分析了。

参考源码：
opencv-3.4.0\sources\modules\video\include\opencv2\video\tracking.hpp
opencv-3.4.0\sources\modules\video\src\kalman.cpp
opencv-3.4.0\sources\modules\core\src\lapack.cpp（solve函数的源码）

举个例子

假设有一个线性模型，要预测一个bounding box在下一帧中的位置。
设置7个状态变量，DP=7，分别是bounding box的中心点坐标(x,y)、面积area、长宽比w/h ratio、中心点的偏移量velocity of x,y、面积的变化量variation rate of area。
设置4个观测变量，(x,y)、area、w/h ratio，即DP中的前4个，并且假设w/h ratio是不变的。

因此可以得到[7,7]的状态转移矩阵A：
$$A=\lfloor$$