【OpenCV学习系列】之KalmanFilter

最新推荐文章于 2023-04-19 20:34:20 发布

AutoXTruck

最新推荐文章于 2023-04-19 20:34:20 发布

阅读量2.5k

点赞数 2

分类专栏：自动驾驶技术专栏文章标签： opencv 计算机视觉图像处理

本文链接：https://blog.csdn.net/Small_Munich/article/details/112631741

版权

自动驾驶技术专栏专栏收录该内容

32 篇文章 70 订阅

订阅专栏

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