【OpenCV学习系列】之KalmanFilter

opencv

OpenCV: 开源计算机视觉库

项目地址：https://gitcode.com/gh_mirrors/opencv31/opencv

免费下载资源

AutoSleep

2630人浏览 · 2022-01-10 20:16:07

AutoSleep · 2022-01-10 20:16:07 发布

　　说实话卡尔曼滤波博客网页有很多博主见解，足以说明其应用广泛与热点。本人也看到许多对于卡尔曼滤波算法的介绍，受益颇多。应用卡尔曼滤波算法在目标跟踪领域已经有很长时间了，写此篇博客主要为了给自己加深印象，如果还能够对阅读博客的你有一丝帮助我已经知足。下面我会结合OpenCV开源代码对KalmanFilter的流程给予一定的介绍。

卡尔曼滤波流程

　　相信看过卡尔曼滤波理论都清楚，整个卡尔曼滤波就是一个不断递推计算【预测】=》【更新】=》【预测】=》【更新】… 这个过程，不过想要详细解释每个过程都做了什么，还是需要定义一下计算公式与声明。

状态空间方程如下：

状态方程

$x_k={A_k}{x_k}+{B_k}{u_k}+w_k$

测量方程

$z_k={H_k}{x_k}+v_k$

相关参数解释： $x_k$ 是状态向量， $z_k$ 是测量向量， $A_k$ 是状态转移矩阵， $u_k$ 是控制向量， $B_k$ 是控制矩阵， $w_k$ 是系统误差（噪声）， $H_k$ 是测量矩阵， $v_k$ 是测量误差（噪声）。其中 $w_k$ 与 $v_k$ 都为高斯分布。

$w_k$ 的高斯分布 $N(0, Q_k)$

$v_k$ 的高斯分布 $N(0, R_k)$

1. 那么预侧阶段状态值：

$x^{'}_{k|k-1}={A_k}{x^{'}_{k-1|k-1}}+{B_k}{u_k}$

那么预侧阶段最小均方误差：

$P_{k|k-1}={A_k}{P_{k-1|k-1}}A^{T}_{k}+{Q_k}$

2. 更新阶段测量误差：

${y_k^{'}}=z_k-H_{k}x^{'}_{k|k-1}$

2.1 测量协方差：

$S_k=H_{k}P_{k|k-1}H^{T}_{k}+R_{k}$

2.2 卡尔曼增益：

$K_k=P_{k|k-1}H^{T}_{k}S_{k}^{-1}$

2.2 卡尔曼修正状态值：

$x_{k|k}^{'}=x_{k|k-1}^{'}+K_{k}y_{k}^{'}$

2.3 修正最小均方误差：

$P_{k|k}=(I-K_{k}H_{k})P_{k|k-1}$

上述公式与OpenCV代码对齐：

	cv::Mat statePre;           //!< 预侧状态值 predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
	cv::Mat statePost;          //!< 修正状态值 corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
	cv::Mat transitionMatrix;   //!< 状态转移矩阵 state transition matrix (A)
	cv::Mat controlMatrix;      //!< 控制矩阵 control matrix (B) (not used if there is no control)
	cv::Mat measurementMatrix;  //!< 测量矩阵 measurement matrix (H)
	cv::Mat processNoiseCov;    //!< 系统误差 process noise covariance matrix (Q)
	cv::Mat measurementNoiseCov;//!< 测量误差 measurement noise covariance matrix (R)
	cv::Mat errorCovPre;        //!< 先验误差 priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
	cv::Mat gain;               //!< 卡尔曼增益 Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
	cv::Mat errorCovPost;       //!< 后验修正误差 posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

卡尔曼滤波代码示例

想必上述吧啦吧啦一大堆其实都是基本的流程，大家在别的博客也见到类似流程。下面我们从代码部分来进行一定的分析与讲解：首先我把OpenCV的KalmanFilter类提取出来，方便调试【代码几乎一致没有什么改动】。

头文件部分：

#ifndef _KALMAN_FILTER_X_
#define _KALMAN_FILTER_X_

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"

class  KalmanFilterX
{
public:
	KalmanFilterX();
	/** @overload
	@param dynamParams Dimensionality of the state.
	@param measureParams Dimensionality of the measurement.
	@param controlParams Dimensionality of the control vector.
	@param type Type of the created matrices that should be CV_32F or CV_64F.
	*/
	KalmanFilterX(int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F);

	/** @brief Re-initializes Kalman filter. The previous content is destroyed.

	@param dynamParams Dimensionality of the state.
	@param measureParams Dimensionality of the measurement.
	@param controlParams Dimensionality of the control vector.
	@param type Type of the created matrices that should be CV_32F or CV_64F.
	 */
	void init(int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F);

	/** @brief Computes a predicted state.

	@param control The optional input control
	 */
	const cv::Mat& predict(const cv::Mat& control = cv::Mat());

	/** @brief Updates the predicted state from the measurement.

	@param measurement The measured system parameters
	 */
	const cv::Mat& correct(const cv::Mat& measurement);

	cv::Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
	cv::Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
	cv::Mat transitionMatrix;   //!< state transition matrix (A)
	cv::Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
	cv::Mat measurementMatrix;  //!< measurement matrix (H)
	cv::Mat processNoiseCov;    //!< process noise covariance matrix (Q)
	cv::Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
	cv::Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
	cv::Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
	cv::Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

	// temporary matrices
	cv::Mat temp1;
	cv::Mat temp2;
	cv::Mat temp3;
	cv::Mat temp4;
	cv::Mat temp5;
}; 
#endif

源码实现部分：


#include "KalmanFilterX.h"

KalmanFilterX::KalmanFilterX() 
{}

KalmanFilterX::KalmanFilterX(int dynamParams, int measureParams, int controlParams, int type)
{
	init(dynamParams, measureParams, controlParams, type);
}

void KalmanFilterX::init(int DP, int MP, int CP, int type)
{
	CV_Assert(DP > 0 && MP > 0);
	CV_Assert(type == CV_32F || type == CV_64F);
	CP = std::max(CP, 0);

	statePre = cv::Mat::zeros(DP, 1, type);
	statePost = cv::Mat::zeros(DP, 1, type);
	transitionMatrix = cv::Mat::eye(DP, DP, type);

	processNoiseCov = cv::Mat::eye(DP, DP, type);
	measurementMatrix = cv::Mat::zeros(MP, DP, type);
	measurementNoiseCov = cv::Mat::eye(MP, MP, type);

	errorCovPre = cv::Mat::zeros(DP, DP, type);
	errorCovPost = cv::Mat::zeros(DP, DP, type);
	gain = cv::Mat::zeros(DP, MP, type);

	if (CP > 0)
		controlMatrix = cv::Mat::zeros(DP, CP, type);
	else
		controlMatrix.release();

	temp1.create(DP, DP, type);
	temp2.create(MP, DP, type);
	temp3.create(MP, MP, type);
	temp4.create(MP, DP, type);
	temp5.create(MP, 1, type);
}

const cv::Mat& KalmanFilterX::predict(const cv::Mat& control)
{
	// update the state: x'(k) = A*x(k)
	statePre = transitionMatrix * statePost;

	if (!control.empty())
		// x'(k) = x'(k) + B*u(k)
		statePre += controlMatrix * control;

	// update error covariance matrices: temp1 = A*P(k)
	temp1 = transitionMatrix * errorCovPost;

	// P'(k) = temp1*At + Q

	double alpha = 1.0;
	double beta = 1.0;
	// errorCovPre = alpha * temp1.t() * transitionMatrix + beta * processNoiseCov
	gemm(temp1, transitionMatrix, alpha, processNoiseCov, beta, errorCovPre, cv::GEMM_2_T);

	// handle the case when there will be measurement before the next predict.
	statePre.copyTo(statePost);
	errorCovPre.copyTo(errorCovPost);

	return statePre;
}

const cv::Mat& KalmanFilterX::correct(const cv::Mat& measurement)
{
	// temp2 = H*P'(k)
	temp2 = measurementMatrix * errorCovPre;

	double alpha = 1.0;
	double beta = 1.0;
	// temp3 = temp2*Ht + R

	// temp3 = alpha * temp2.t() * measurementMatrix + beta * measurementNoiseCov
	gemm(temp2, measurementMatrix, alpha, measurementNoiseCov, beta, temp3, cv::GEMM_2_T);

	// temp4 = inv(temp3)*temp2 = Kt(k)
	solve(temp3, temp2, temp4, cv::DECOMP_SVD);

	// K(k)
	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;

	return statePost;
}

测试函数部分：

#include <stdio.h>
#include <iostream>
#include <vector>
#include "KalmanFilterX.h"

// 打印矩阵数据
void printMatrix(const cv::Mat& matrix);

int main(void)
{
	// 1.kalman filter setup
	const int stateNum = 4;                                      // 状态值4×1向量(x,y,△x,△y)
	const int measureNum = 2;                                    // 测量值2×1向量(x,y)	
	KalmanFilterX KFX(stateNum, measureNum, 0);                  // 控制矩阵参数默认为0  控制矩阵B 

	std::vector<cv::Point> measureValue;
	measureValue.emplace_back(cv::Point(1,4));
	measureValue.emplace_back(cv::Point(2,7));
	measureValue.emplace_back(cv::Point(3,10));
	measureValue.emplace_back(cv::Point(4,13));
	measureValue.emplace_back(cv::Point(5,16));
	measureValue.emplace_back(cv::Point(6,19));
	measureValue.emplace_back(cv::Point(7,22));
	measureValue.emplace_back(cv::Point(8,25));
	measureValue.emplace_back(cv::Point(9,28));
	measureValue.emplace_back(cv::Point(10,31));

	cv::Mat_<float> transitionmatrix = (cv::Mat_<float>(4, 4) << 1, 0, 1, 0, 
		                                                         0, 1, 0, 1,
		                                                         0, 0, 1, 0, 
		                                                         0, 0, 0, 1);

	KFX.transitionMatrix = transitionmatrix;                     // 转移矩阵 A
	setIdentity(KFX.measurementMatrix);                          // 测量矩阵H
	setIdentity(KFX.processNoiseCov, cv::Scalar::all(1e-5));     // 系统噪声方差矩阵Q
	setIdentity(KFX.measurementNoiseCov, cv::Scalar::all(1e-1)); // 测量噪声方差矩阵R
	setIdentity(KFX.errorCovPost, cv::Scalar::all(1));           // 后验错误估计协方差矩阵P
	
    // 初始状态值x(0)
	// setIdentity(KFX.statePost, cv::Scalar::all(1)); 

	// 初始测量值x'(0)，因为后面要更新这个值，所以必须先定义
	cv::Mat measurement = cv::Mat::ones(measureNum, 1, CV_32F);   

	for(int i=0; i < measureValue.size(); ++i)
	{
		// 2.kalman prediction
		cv::Mat prediction = KFX.predict(); // 输出是下一时刻的状态值KFX.statePost(k+1)
		cv::Point predict_pt = cv::Point(prediction.at<float>(0), prediction.at<float>(1));   // 预测值(x',y')

		// 3.update measurement
		measurement.at<float>(0) = (float)measureValue[i].x;
		measurement.at<float>(1) = (float)measureValue[i].y;

		// 4.update
		KFX.correct(measurement);  // 经过KFX.correct()之后，最终的结果应该是KFX.statePost

		//printMatrix(KFX.transitionMatrix);    // A
		//std::cout << "---A---" << std::endl;
		//printMatrix(KFX.measurementMatrix);   // H
		//std::cout << "---H---" << std::endl;  
		//printMatrix(KFX.processNoiseCov);     // Q
		//std::cout << "---Q---" << std::endl;
		//printMatrix(KFX.measurementNoiseCov); // R
		//std::cout << "---R---" << std::endl;
		//printMatrix(KFX.gain);                // K
		//std::cout << "---K---" << std::endl;
		//printMatrix(KFX.errorCovPre);         // errorCovPre
		//std::cout << "---errorCovPre---" << std::endl;
		//printMatrix(KFX.errorCovPost);        // errorCovPost
		//std::cout << "---errorCovPost---" << std::endl;

		std::cout << "predict (x, y): " << predict_pt.x << " , " << predict_pt.y << std::endl;
		std::cout << "measureValue (x, y): " << measureValue[i].x << " , " << measureValue[i].y << std::endl;
		std::cout << "KFX.statePost (x, y): " << KFX.statePost.at<float>(0) << " , " << KFX.statePost.at<float>(1) << std::endl;
	}

	return 0;
}

void printMatrix(const cv::Mat& matrix)
{
	std::cout << "--------------beigin---------------" << std::endl;

	for (size_t i = 0; i < matrix.rows; ++i)
	{
		for (size_t j = 0; j < matrix.cols; ++j)
		{
			std::cout << " " << matrix.at<float>(i, j);
		}
		std::cout << std::endl;
	}

	std::cout << "--------------end----------------" << std::endl;
}

对上述代码进行简单说明一下：

measureValue为人为构造测量值列表进行模拟测量值序列；
转移矩阵A设置成为匀速模型；
卡尔曼滤波里面的statePre为预测下一时刻的输出，statePost为对当前时刻测量值与预测值的纠正后输出；

<============ loop 0 ===============>
predict (x, y): 0 , 0
measureValue (x, y): 1 , 4
KFX.statePost (x, y): 0.952381 , 3.80952
<============ loop 1 ===============>
predict (x, y): 1 , 5
measureValue (x, y): 2 , 7
KFX.statePost (x, y): 1.92983 , 6.84211
<============ loop 2 ===============>
predict (x, y): 2 , 9
measureValue (x, y): 3 , 10
KFX.statePost (x, y): 2.9572 , 9.92218
<============ loop 3 ===============>
predict (x, y): 3 , 12
measureValue (x, y): 4 , 13
KFX.statePost (x, y): 3.97266 , 12.9603
<============ loop 4 ===============>
predict (x, y): 4 , 15
measureValue (x, y): 5 , 16
KFX.statePost (x, y): 4.98126 , 15.9793
<============ loop 5 ===============>
predict (x, y): 5 , 18
measureValue (x, y): 6 , 19
KFX.statePost (x, y): 5.98641 , 18.9896
<============ loop 6 ===============>
predict (x, y): 6 , 21
measureValue (x, y): 7 , 22
KFX.statePost (x, y): 6.98971 , 21.9956
<============ loop 7 ===============>
predict (x, y): 7 , 24
measureValue (x, y): 8 , 25
KFX.statePost (x, y): 7.99195 , 24.9993
<============ loop 8 ===============>
predict (x, y): 8 , 28
measureValue (x, y): 9 , 28
KFX.statePost (x, y): 8.99354 , 28.0016
<============ loop 9 ===============>
predict (x, y): 9 , 31
measureValue (x, y): 10 , 31
KFX.statePost (x, y): 9.9947 , 31.0031

　　从上述结果可以看到，每次预测下一时刻与测量值存在一定的误差，但是经过纠正之后的statePost能够很好的降低测量值误差同时也能够降低预测的误差随着不断递推迭代进行。

　　在目标跟踪里面，如果目标丢失情况下，我们是否使用卡尔曼滤波向前预测一帧，这个时候获取的应该是statePre的预估值。如果目标跟踪时刻，预估状态与测量值关联成功，我们经过卡尔曼滤波以后保存在追踪列表的值应该为statePost的值。

小结

　　如何在理解卡尔曼滤波基础上有效调节相关卡尔曼参数，可以参考一下博客的参考链接。同时，我将知乎上面对卡尔曼滤波理解较为直白的一种解释截图贴在下面：希望对你有进一步理解。【注：下载代码单步调试一下你会知道卡尔曼滤波计算整个流程是如何的，便于你加速理解卡尔曼滤波】

在这里插入图片描述

参考

如何通俗并尽可能详细地解释卡尔曼滤波？

How a Kalman filter works, in pictures

OpenCV–卡尔曼滤波(KalmanFilter)详解【转载】

卡尔曼滤波中关键参数的调整

深度解析卡尔曼滤波在IMU中的使用

GitHub 加速计划 / opencv31 / opencv

77.37 K

55.71 K

下载

OpenCV: 开源计算机视觉库

最近提交(Master分支：2 个月前 )

c3747a68 Added Universal Windows Package build to CI. 2 天前

9b635da5 - 2 天前

GitCode 开源社区

旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。

更多推荐

[转载]在Windows环境下安装GNU Radio

转自：在Windows环境下安装GNURadio_恐弱智_新浪博客GNU Radio是用Python开发的，大部分开源的工程能够在Linux环境下运行良好，而Windows下却运行的很勉强，而且安装配置都很复杂。GNU Radio算是个例外了，不光提供了Windows的二进制安装，还有比较详细的说明。我是Python小白，所以折腾了好久才弄好，特意记录下来，免得以后再装还折腾。GNU Radio的

GitCode 开源社区

centOS 8 使用dnf安装Docker

DNF是什么？CentOS 8使用YUM软件包管理器版本v4.0.4。现在，该版本使用DNF(已删除YUM)。DNF是软件包管理器。它会在Linux发行版上安装，执行更新并删除软件包。使用DNF安装Docker跳过具有损坏依赖性的程序包一个有效的解决方案是使您的CentOS 8系统使用以下--nobest命令安装最符合条件的版本：sudo dnf install docker...

GitCode 开源社区

定时同步数据库表(mysql+linux+crontab)

sync.sh里面的参数需要改变，ip/username/password/database/tablesync.sh#!/bin/sh# Please change the IP and password of the data source db.# Then change the table name.filename=/home/nington/db/$(date +%Y-%m