OpenCV3之卡尔曼滤波KalmanFilter原理、公式推导及其源码代码相关分析

---

一、适用

• 适用于线性系统中。
• 用于一些无法直接测量但可以间接测量的量。
  比如，火箭燃料室内的温度不能直接测量，但可以燃料室外的温度，从而据此估算燃料室内的温度。
• 用于从受误差影响的传感器测量值中估算系统的状态。
  比如，GPS传感器测量结果是有误差。

二、理解

1.反馈系统

例如火箭燃料问题，要测量燃料室内的温度$T_{in}$，已知火箭燃料室外的温度$T_{ext}$和燃料的流量$W_{fuel}$。

1. 燃料的流量$W_{fuel}$作为输入，通过真实模型，可以输出得到真实的燃料室内的温度$T_{in}$和火箭燃料室外的温度$T_{ext}$；
   通过数学模型，可以输出估计的燃料室内的温度$\widehat{T_{in}}$和火箭燃料室外的温度$\widehat{T_{ext}}$。
2. 比较真实的$T_{ext}$和估计的$\widehat{T_{ext}}$，得到误差$e=T_{ext}-\widehat{T_{ext}}$
3. 用$e$去通过控制器K修正（增益）模型。
   
   

2.卡尔曼滤波又叫数据融合算法

• 假设你有两个传感器，测的是同一个信号。可是它们每次的读数都不太一样，怎么办？
  取平均。

• 再假设你知道其中贵的那个传感器应该准一些，便宜的那个应该差一些。那有比取平均更好的办法吗？
  加权平均。怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。

• 接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。
  OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。这就是卡尔曼滤波。（无公式）

@cite https://www.zhihu.com/question/23971601/answer/26254459

3.意义

• 预测：
  不存在神奇的深度神经网络一样推算出不可知的神奇公式，它的预测是根据你设计的公式推算出的，即${\hat{X}}_k^{\prime }=A_k{\hat{X}}_{k-1}+B_k{U}_k$。
• 迭代：
  迭代的意义只是保留上一次的状态及其协方差矩阵为下一次用。并不能修改其数学模型中的公式。
• 作用：
  所以就是用来得到一个相对噪声小的目标状态值，没有什么神奇的预测还是别的作用。

三、公式推导

1.过程

1. 预测：由k-1时刻的最优的状态估计${\hat{x}}_{k-1}$，得出k时刻的预测的状态估计${\hat{x}}_k^{\prime }$
   【先验估计的状态向量】${\hat{X}}_k^{\prime }=A_k{\hat{X}}_{k-1}+B_k{U}_k$
   【先验估计的状态向量的协方差矩阵】$P_k^{\prime }=A_k{P}_{k-1}{A}_k^T+Q_k$
2. 处理测量：由传感器得出k时刻的测量值$z_k$
3. 融合并更新：融合${\hat{x}}_k^{\prime }$和$z_k$，得到k时刻的最优的状态估计${\hat{x}}_k$
   【卡尔曼增益】$K_k=P_k^{\prime }{H}_k^T(H_k{P}_k^{\prime }{H}_k^T+R_k{)}^{-1}$
   【最优的状态估计】${\hat{X}}_k={\hat{X}}_k^{\prime }+K_k(Z_k-H_k{\hat{X}}_k^{\prime })$
   【最优的状态估计的协方差】$P_k=P_k^{\prime }-K_k{H}_k{P}_k^{\prime }=P_k^{\prime }(I-K_k{H}_k)$

2.公式用到的变量

（）内对应KalmanFilter的变量

（1）定义三个列向量

• 状态向量$X_k$（state）
• 测量向量$Z_k$（measurement）
• 控制向量$U_k$（control）

（2）三个矩阵

• 状态转移矩阵$A_k$（transitionMatrix）
• 控制矩阵$B_k$（controlMatrix）
• 测量矩阵$Hk$（measurementMatrix）

（3）估计的状态向量及其协方差

k-1时刻：

• 后验估计的状态向量${\hat{X}}_{k-1}$（statePost）：k-1时刻的最优的状态估计
• 后验估计的状态向量的协方差矩阵$P_{k-1}$（errorCovPost）：k-1时刻的最优的状态估计的协方差

k时刻的：

• 先验估计的状态向量${\hat{X}}_k^{\prime }$（statePre）：k时刻的预测的状态估计
• 先验估计的状态向量的协方差矩阵$P_k^{\prime }$（errorCovPre）：k时刻的预测的状态估计的协方差
• 最优的状态估计${\hat{X}}_k$（statePost）：求出k时刻的最优的状态估计，就把statePost更新为此值
• 最优的状态估计的协方差$P_k$（errorCovPost）：求出k时刻的最优的状态估计的协方差，就把errorCovPost更新为此值

（4）噪声及其协方差矩阵

• 系统噪声$W_k$：其协方差为$Q_k$（processNoiseCov）
• 测量噪声$V_k$：其协方差为$R_k$（measurementNoiseCov）

（5）卡尔曼增益

• 卡尔曼增益$K_k$（gain）

3.公式意思

比如计算匀速直线运动下的角度变化。
状态$X_k=\left[\begin{array}{c}\theta \\ \delta \theta \end{array}\right]$，表示角度和角度变化量
状态转移矩阵$A_k=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]$
则${\hat{X}}_k^{\prime }=A_k\ast {\hat{X}}_{k-1}=\left[\begin{array}{c}\theta +\delta \theta \\ \delta \theta \end{array}\right]$。

4.对应代码部分

（1）预测predict

【先验估计的状态向量】${\hat{X}}_k^{\prime }=A_k{\hat{X}}_{k-1}+B_k{U}_k$

// 得出先验估计的状态向量statePre: X'(k) = A*X(k-1)
statePre = transitionMatrix * statePost;

// 得出先验估计的状态向量statePre的后部分：X'(k) += Bk*Uk
if (!control.empty())
    statePre += controlMatrix * control;

【先验估计的状态向量的协方差矩阵】$P_k^{\prime }=A_k{P}_{k-1}{A}_k^T+Q_k$

// temp1 = A*P(k-1)
temp1 = transitionMatrix * errorCovPost;

// P'(k) = temp1*A.t + Q
// errorCovPre = 1 * temp1 * transitionMatrix.t() + 1 * processNoiseCov
gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);

（2）融合更新correct

【卡尔曼增益】$K_k=P_k^{\prime }{H}_k^T(H_k{P}_k^{\prime }{H}_k^T+R_k{)}^{-1}$

/* 这是在算括号里求逆的部分 */
// temp2 = H*P'(k)
temp2 = measurementMatrix * errorCovPre;

// temp3 = temp2*H.t + R
// temp3 = 1 * temp2 * measurementMatrix.t() + 1 * measurementNoiseCov
gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);

/* 算Kk */
// temp4 = inv(temp3)*temp2，这是最小二乘法求逆再乘
solve(temp3, temp2, temp4, DECOMP_SVD);

// K(k) = temp4.t()是因为A=BC,A.t()=C.t()*B.t()
gain = temp4.t();

【最优的状态估计】${\hat{X}}_k={\hat{X}}_k^{\prime }+K_k(Z_k-H_k{\hat{X}}_k^{\prime })$

// 括号里的部分
// temp5 = Z(k)-H*X'(k)
temp5 = measurement - measurementMatrix * statePre;

// 最优的状态估计：X(k) = X'(k) + K(k)*temp5
statePost = statePre + gain * temp5;

【最优的状态估计的协方差】$P_k=P_k^{\prime }-K_k{H}_k{P}_k^{\prime }=P_k^{\prime }(I-K_k{H}_k)$

// P(k) = P'(k) - K(k)*temp2
errorCovPost = errorCovPre - gain * temp2;

四、KalmanFilter Class

class CV_EXPORTS_W KalmanFilter
{
public:    
    CV_WRAP KalmanFilter();        
	
	/** @overload
    @param dynamParams 状态向量state的维度
    @param measureParams 测量向量measurement的维度
    @param controlParams 控制向量control的维度
    @param type 这三个向量的Mat类型应该是CV_32F or CV_64F
    */
    CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
    
    /** @brief 重新初始化KalmanFilter对象，会替换原来的KF对象，原先的被扔掉
     */
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);              
  	
  	/** @brief 计算预测的状态值

    @param control 预测，计算先验估计的状态向量及其协方差矩阵
    @discuss
    调用完后，KF.statePre即为k时刻的先验估计的状态向量（预测值），
    直接调用这个值或者返回值（返回得也是这个值，但没必要多保存一个矩阵），
    errorCovPre就是k时刻的先验估计的状态向量（预测值）的协方差矩阵
     */
    const Mat& predict(const Mat& control=Mat());
    
    /** @brief 根据测量向量，融合计算最优的状态估计，并更新状态值

    @param measurement 测量向量
    @discuss
    调用完后，KF.gain即为k时刻的卡尔曼增益，
    KF.statePost即为k时刻最优的状态估计值，
    直接调用这个值或者返回值（返回得也是这个值，但没必要多保存一个矩阵），
    errorCovPost就是k时刻最优的状态估计值的协方差矩阵
     */
    const Mat& correct(const Mat& measurement);
 
 	/* 两个估计的状态向量：先验估计的状态向量（statePre）和后验估计的状态向量(statePost) */
    Mat statePre;            // 先验估计的状态向量
    Mat statePost;           // 后验估计的状态向量
    
    /* 三个矩阵 */
    Mat transitionMatrix;    // 状态转移矩阵 A
    Mat controlMatrix;       // 控制矩阵 B
    Mat measurementMatrix;   // 测量矩阵 H
    
  	/* 两个噪声的协方差矩阵 */ 
    Mat processNoiseCov;     // 系统噪声的协方差矩阵 Q
    Mat measurementNoiseCov; // 测量噪声的协方差矩阵 R

	/* 两个估计状态向量的协方差矩阵 */
    Mat errorCovPre;         // 先验估计状态向量的协方差矩阵 
    Mat errorCovPost;        // 后验估计状态向量的协方差矩阵 

	/* 一个增益 */
    Mat gain;                // 卡尔曼增益
    
 
    // 计算过程中的中间变量
    Mat temp1;	// A*P(k-1)
    Mat temp2;	// H*P'(k)
    Mat temp3;	// 卡尔曼增益括号里求逆的部分
    Mat temp4;	// 卡尔曼增益的转置
    Mat temp5;	// 最优的状态估计括号里的部分
};

五、魔改例子

OpenCV3之卡尔曼滤波KalmanFilter例子魔改代码

---

参考：

学习OpenCV2——卡尔曼滤波(KalmanFilter)详解
要点初见：反用OpenCV3中的卡尔曼滤波器（KalmanFilter）进行运动预测
学习OpenCV——Kalman滤波
OpenCV里卡尔曼滤波的理解
官网文档：cv::KalmanFilter Class Reference
官网例子：samples/cpp/kalman.cpp
详解卡尔曼滤波原理
如何通俗并尽可能详细地解释卡尔曼滤波？