文章目录
一、适用
- 适用于线性系统中。
- 用于一些无法直接测量但可以间接测量的量。
比如,火箭燃料室内的温度不能直接测量,但可以燃料室外的温度,从而据此估算燃料室内的温度。 - 用于从受误差影响的传感器测量值中估算系统的状态。
比如,GPS传感器测量结果是有误差。
二、理解
1.反馈系统
例如火箭燃料问题,要测量燃料室内的温度 ,已知火箭燃料室外的温度 和燃料的流量 。
- 燃料的流量
作为输入,通过真实模型,可以输出得到真实的燃料室内的温度
和火箭燃料室外的温度
;
通过数学模型,可以输出估计的燃料室内的温度 和火箭燃料室外的温度 。 - 比较真实的 和估计的 ,得到误差
- 用
去通过控制器K修正(增益)模型。
2.卡尔曼滤波又叫数据融合算法
-
假设你有两个传感器,测的是同一个信号。可是它们每次的读数都不太一样,怎么办?
取平均。 -
再假设你知道其中贵的那个传感器应该准一些,便宜的那个应该差一些。那有比取平均更好的办法吗?
加权平均。怎么加权?假设两个传感器的误差都符合正态分布,假设你知道这两个正态分布的方差,用这两个方差值,(此处省略若干数学公式),你可以得到一个“最优”的权重。 -
接下来,重点来了:假设你只有一个传感器,但是你还有一个数学模型。模型可以帮你算出一个值,但也不是那么准。怎么办?把模型算出来的值,和传感器测出的值,(就像两个传感器那样),取加权平均。
OK,最后一点说明:你的模型其实只是一个步长的,也就是说,知道x(k),我可以求x(k+1)。问题是x(k)是多少呢?答案:x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。这就是卡尔曼滤波。(无公式)
@cite https://www.zhihu.com/question/23971601/answer/26254459
3.意义
- 预测:
不存在神奇的深度神经网络一样推算出不可知的神奇公式,它的预测是根据你设计的公式推算出的,即 。 - 迭代:
迭代的意义只是保留上一次的状态及其协方差矩阵为下一次用。并不能修改其数学模型中的公式。 - 作用:
所以就是用来得到一个相对噪声小的目标状态值,没有什么神奇的预测还是别的作用。
三、公式推导
1.过程
- 预测:由k-1时刻的最优的状态估计
,得出k时刻的预测的状态估计
【先验估计的状态向量】
【先验估计的状态向量的协方差矩阵】 - 处理测量:由传感器得出k时刻的测量值
- 融合并更新:融合
和
,得到k时刻的最优的状态估计
【卡尔曼增益】
【最优的状态估计】
【最优的状态估计的协方差】
2.公式用到的变量
()
内对应KalmanFilter的变量
(1)定义三个列向量
- 状态向量 (state)
- 测量向量 (measurement)
- 控制向量 (control)
(2)三个矩阵
- 状态转移矩阵 (transitionMatrix)
- 控制矩阵 (controlMatrix)
- 测量矩阵 (measurementMatrix)
(3)估计的状态向量及其协方差
k-1时刻:
- 后验估计的状态向量 (statePost):k-1时刻的最优的状态估计
- 后验估计的状态向量的协方差矩阵 (errorCovPost):k-1时刻的最优的状态估计的协方差
k时刻的:
- 先验估计的状态向量 (statePre):k时刻的预测的状态估计
- 先验估计的状态向量的协方差矩阵 (errorCovPre):k时刻的预测的状态估计的协方差
- 最优的状态估计 (statePost):求出k时刻的最优的状态估计,就把statePost更新为此值
- 最优的状态估计的协方差 (errorCovPost):求出k时刻的最优的状态估计的协方差,就把errorCovPost更新为此值
(4)噪声及其协方差矩阵
- 系统噪声 :其协方差为 (processNoiseCov)
- 测量噪声 :其协方差为 (measurementNoiseCov)
(5)卡尔曼增益
- 卡尔曼增益 (gain)
3.公式意思
比如计算匀速直线运动下的角度变化。
状态
,表示角度和角度变化量
状态转移矩阵
则
。
4.对应代码部分
(1)预测predict
【先验估计的状态向量】
// 得出先验估计的状态向量statePre: X'(k) = A*X(k-1)
statePre = transitionMatrix * statePost;
// 得出先验估计的状态向量statePre的后部分:X'(k) += Bk*Uk
if (!control.empty())
statePre += controlMatrix * control;
【先验估计的状态向量的协方差矩阵】
// 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
【卡尔曼增益】
/* 这是在算括号里求逆的部分 */
// 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();
【最优的状态估计】
// 括号里的部分
// 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) - 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
详解卡尔曼滤波原理
如何通俗并尽可能详细地解释卡尔曼滤波?