OpenCV里卡尔曼滤波的理解

转载https://blog.csdn.net/yangtrees/article/details/8075911

https://blog.csdn.net/GDFSG/article/details/50904811

我们把这5个式子可分成三大部分看：

公式和opencv对应关系的讲解：

首先对于离散控制过程的系统，其系统状态和系统测量值可进行以下表示：
X(k)： k时刻系统状态
A：     状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵
B：     控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵
U(k)： k时刻对系统的控制量
Z(k)： k时刻的测量值
H：     系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵
W(k)：系统过程噪声，为高斯白噪声，协方差为Q，对应opencv里的kalman滤波器的processNoiseCov矩阵
V(k)： 测量噪声，也为高斯白噪声，协方差为R，对应opencv里的kalman滤波器的measurementNoiseCov矩阵
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。
以上便是核心的5个公式。

第一部分：
式(1)-(2)：  预测值的计算
式(1)：        计算基于k-1时刻状态对k时刻系统状态的预测值
X(k|k-1)：   基于k-1时刻状态对k时刻状态的预测值，对应opencv里kalman滤波器的predict()输出，即statePre矩阵
X(k-1|k-1)：k-1时刻状态的最优结果，对应opencv里kalman滤波器的上一次状态的statePost矩阵
U(k)：         k时刻的系统控制量，无则为0
A：             状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵
B：             控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵

/**********************************************************************************************************************************/
式(2)：       计算X(k|k-1)对应的协方差的预测值
P(k|k-1)：   基于k-1时刻的协方差计算k时刻协方差的预测值，对应opencv里kalman滤波器的errorCovPre矩阵  (predict预测)
P(k-1|k-1)：k-1时刻协方差的最优结果，对应opencv里kalman滤波器的上一次状态的errorCovPost矩阵   （convariance协方差）
Q：            系统过程噪声协方差，对应opencv里kalman滤波器的processNoiseCov矩阵

/***********************************************************************************************************************************/

第二部分：
式(3):   增益的计算
Kg(k)：k时刻的kalman增益，为估计量的方差占总方差(估计量方差和测量方差)的比重，对应opencv里kalman滤波器的gain矩阵
H：      系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵
R：      测量噪声协方差，对应opencv里的kalman滤波器的measurementNoiseCov矩阵

/************************************************************************************************************************************/

第三部分：
式(4)--(5):  k时刻的更新

式(4)：      计算k时刻系统状态最优值
X(k|k)：     k时刻系统状态的最优结果，对应opencv里kalman滤波器的k时刻状态的statePost矩阵
Z(k)：        k时刻系统测量值
式(5)：      计算k时刻系统最优结果对应的协方差
P(k|k)：     k时刻系统最优结果对应的协方差，对应opencv里kalman滤波器的errorCovPost矩阵

./************************************************************************************************************************************/

例如：KalmanFilter KF(stateNum, measureNum, 0);

Fk : KF.transitionMatrix

Hk : KF.measurementMatrix

Qk : KF.processNoiseCov

Rk  : KF.measurementNoiseCov

Pk : KF.errorCovPost

有时也需要定义Bk : KF.controlMatrix