Opencv的KalmanFilter学习笔记

本文代码主要参考

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

的鼠标跟随的那个demo，略有修改

#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <stdio.h>
#include <ctime>
#include <cstdlib>
#include <iostream>
using namespace cv;
using namespace std;

const int winHeight = 720;
const int winWidth = 1280;

Point mousePosition = Point(winWidth >> 1, winHeight >> 1);

int main(void)
{
	RNG rng;
	unsigned int seed = time(0);
	srand(seed);
	// 1.kalman filter setup
	const int stateNum = 4;	  // 状态值4×1向量(x,y,△x,△y)
	const int measureNum = 2; // 测量值2×1向量(x,y)
	KalmanFilter KF(stateNum, measureNum, 0);

	KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0,
						   0, 1, 0, 1,
						   0, 0, 1, 0,
						   0, 0, 0, 1);													  // 转移矩阵A
	setIdentity(KF.measurementMatrix);													  // 测量矩阵H
	setIdentity(KF.processNoiseCov, Scalar::all(0.0008));								  // 系统噪声方差矩阵Q
	setIdentity(KF.measurementNoiseCov, Scalar::all(1));								  // 测量噪声方差矩阵R
	setIdentity(KF.errorCovPost, Scalar::all(1));										  // 后验错误估计协方差矩阵P
	rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight > winWidth ? winWidth : winHeight); // 初始状态值x(0)
	Mat measurement = Mat::zeros(measureNum, 1, CV_32F);								  // 初始测量值x'(0)，因为后面要更新这个值，所以必须先定义

	Mat image(winHeight, winWidth, CV_8UC3, Scalar(255, 255, 255));
	// image.setTo(Scalar(255,255,255,0));

	int real_x = 100;
	int real_y = 50;
	measurement.at<float>(0) = real_x - 30 + rand() % 60;
	measurement.at<float>(1) = real_y - 20 + rand() % 40;
	mousePosition.x = measurement.at<float>(0);
	mousePosition.y = measurement.at<float>(1);
	// KF.statePre.at<float>(0) = mousePosition.x;
	// KF.statePre.at<float>(1) = mousePosition.y;

	// init value
	KF.statePost.at<float>(0) = mousePosition.x;
	KF.statePost.at<float>(1) = mousePosition.y;
	KF.statePost.at<float>(2) = 0;
	KF.statePost.at<float>(3) = 0;

	Point last_predict_pt(mousePosition.x, mousePosition.y);
	Point last_mouse_position(mousePosition.x, mousePosition.y);
	Point last_real_position(real_x, real_y);

	cout << "statePre = " << KF.statePre << endl;
	cout << "statePost = " << KF.statePost << endl;

	for (int i = 1; i < 100; i++)
	{
		// 2.kalman prediction
		Mat prediction = KF.predict();
		Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1)); // 预测值(x',y')

		// 3.update measurement
		real_x = 100 + 10 * i;
		real_y = 50 + 5 * i;
		measurement.at<float>(0) = real_x - 30 + rand() % 60;
		measurement.at<float>(1) = real_y - 20 + rand() % 40;
		mousePosition.x = measurement.at<float>(0);
		mousePosition.y = measurement.at<float>(1);

		// 4.update
		KF.correct(measurement);
		printf("x = %d, y = %d, predict_pt.x = %d, predict_pt.y = %d, real_x = %d, real_y = %d\n",
			   (int)measurement.at<float>(0), (int)measurement.at<float>(1), predict_pt.x, predict_pt.y, real_x - predict_pt.x, real_y - predict_pt.y);

		// cout << "errorCovPre = " << KF.errorCovPre<< endl;
		// cout << "gain = " << KF.gain<< endl;
		// cout << "errorCovPost = " << KF.errorCovPost<< endl;
		cout << "statePre = " << KF.statePre << endl;

		// draw
		//  circle(image,predict_pt,1,Scalar(0,255,0),3);    //predicted point with green
		//  circle(image,mousePosition,1,Scalar(0,0,255),3); //current position with red

		line(image, last_real_position, Point(real_x, real_y), Scalar(200, 200, 200), 2);
		line(image, last_mouse_position, mousePosition, Scalar(0, 255, 0), 1);
		line(image, last_predict_pt, predict_pt, Scalar(0, 0, 255), 1);

		imshow("test.jpg", image);
		waitKey(1);

		last_predict_pt = predict_pt;
		last_mouse_position = mousePosition;
		last_real_position = Point(real_x, real_y);
	}
	imwrite("result.jpg", image);
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)	#声明cmake需要的最低版本
project(Demo)	#声明cmake项目的名称

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED TRUE)

find_package(OpenCV REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})	#添加OpenCV头文件
add_executable(${PROJECT_NAME} main.cpp)	#生成可执行文件，这里我把可执行文件名称定义成项目名称
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS})	#链接OpenCV库文件到cmake项目上

最后生成的图片

灰色直线是理论真实值

绿色线是输入曲线，可以看出在随机数的影响下，波动非常之混乱

红色线是经卡尔曼滤波后预测得到的曲线，比起输入的跳动值，红色曲线非常稳定