完整的卡尔曼滤波器推导、参数分析以及C++实现（带有目标跟踪效果对比）

1. 引言

  之前看过卡尔曼滤波器的介绍，但是学得比较迷惑，只知道它有预测作用，不知道怎么去使用。这

篇博客将从理论、代码、实际效果几个方面解释卡尔曼滤波器，帮助大家在项目中使用卡尔曼滤波器。

  比较有启发的几个教程为：

卡尔曼滤波器OpenCV的具体实现：https://blog.csdn.net/gdfsg/article/details/50904811

Matlab的讲解什么是卡尔曼滤波器：https://www.zhihu.com/question/23971601

卡尔曼滤波器里的具体参数的作用：https://zhuanlan.zhihu.com/p/110107365

  接下来我将针对各大神的博客以及我个人的理解进行卡尔曼滤波器的介绍，这其中肯定存在一些错

误，希望大家批评指正，能跟大家一起讨论学习进步~

2. 算法效果

  直接附上我在跟踪器中加入卡尔曼滤波器后跟踪框的效果：

  两个图分别为原始跟踪器的跟踪结果与加上卡尔曼滤波以后的跟踪框效果，发现当加上卡尔曼滤波

以后跟踪框变得非常顺滑~~~，从视觉观感上来说看着更加丝滑了。

3. 卡尔曼滤波的迭代

  借用Matlab的例子：当一辆汽车直线匀速，可以使用GPS对其进行定位，但是测量结果存在一定误

差，所以车辆位置不够准确，但是在这个过程中，我们还知道汽车的运行速度，这可以帮助我们得到汽

车的位置的预测值。

  通过预测值与测量值的计算我们可以得到汽车位置的最优值。

3.1 问题描述

3.2 卡尔曼滤波器的计算

3.3 卡尔曼滤波器迭代过程：

/
  同学们可能对F状态矩阵和H观测矩阵有比较大的迷惑，不知道这两个矩阵到底有什么作

用，不要着急，接下来我会用例子对参数进行讲解，哪些参数需要初始化，如何初始化。

3.4 参数分析

  我们再来看看这张图，卡尔曼滤波可以用一句话总结：通过汽车预测结果的高斯分布以及测量

值的高斯分布，通过高斯相乘可以得到汽车位置的最佳值。

  如果我们有多个传感器，获得了多个位置的测量值，我们仍然可以用高斯滤波进行目标最佳位置估

计：

4. 目标跟踪中卡尔曼滤波器的简单使用

  在目标跟踪中，我想对跟踪框的中心点坐标进行卡尔曼滤波，减小跟踪框的“卡顿”现象，状态量包括

中心点x和y坐标，以及x和y的变化率。状态量X为：

  实时观测的仅观测中心点的坐标，所以观测矩阵H为：

  目标跟踪中假设目标为匀速直线运动，所以状态矩阵F为：

  Z矩阵为实际每次的测量矩阵：

  P估计量协方差，R测量协方差，Q预估过程协方差的初始化（R和Q可以自行进行初始化），在目标跟踪中我初始化为：

  针对R和Q的协方差初始化，不同的初始化有着不同的效果。如果测量协方差的越小，最终的结果更

趋向于测量的结果，反之则趋向于预测结果。这里不再推导公式了~~

（链接https://www.zhihu.com/question/23971601 第4个视频4分钟有结果的数学推导；

链接https://zhuanlan.zhihu.com/p/109859418 对R和Q协方差矩阵的推导）

5. OpenCV中卡尔曼滤波的使用

  我自己实现了一下卡尔曼滤波器，并且这个可以正常使用

//头文件
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include<iostream>
using namespace cv;
class use_kalman_filter
{
public:
	Mat x_state;
	Mat z;						//真实值
	int stateNum;					//状态值个数
	int measureNum;					//测量值个数
	void set_Q(float x);				
	void set_R(float y);			
	void set_P(float z);
	void get_F(Mat FF);
	void init();					//卡尔曼滤波器的初始化

	void correct();					//用于对预测值的更新

private:
	Mat Q;			//系统噪声方差矩阵Q
	Mat R;			//测量噪声方差矩阵R
	Mat F;			//状态转移矩阵，物理方程
	Mat H;          //观测矩阵
	Mat K;			//卡尔曼系数

	Mat P;			//预测的协方差矩阵
	Mat P_predict;  //最终的协方差矩阵
	Mat x_hat_prect;
	Mat temp;
};

#include "kalman_filter.h"

void::use_kalman_filter::set_Q(float x)
{
	Q = x*Mat::eye(stateNum, stateNum, CV_32F);
}

void::use_kalman_filter::set_R(float y)
{

	R = y*Mat::eye(measureNum, measureNum, CV_32F);

}


void::use_kalman_filter::set_P(float z)
{

	P_predict = z*Mat::eye(stateNum, stateNum, CV_32F);

}

void::use_kalman_filter::get_F(Mat FF)
{
	F = FF.clone();
}


void use_kalman_filter::init()
{
	K = Mat::zeros(stateNum, stateNum, CV_32F);
	H = Mat::zeros(measureNum, stateNum, CV_32F);
	temp = Mat::zeros(stateNum, stateNum, CV_32F);

	for (int i = 0; i < measureNum; i++)
	{
		H.at<float>(i, i) = 1;
	}

}

void::use_kalman_filter::correct()
{
	//公式均为blog中的公式

	//predict
	x_hat_prect = F*x_state;
	P = F*P_predict*F.t() + Q;

	//Update
	temp = H*P*H.t() + R;
	temp = temp.inv();
	K = P*H.t() *temp;

	x_state = x_hat_prect+ K*(z - H *x_hat_prect);     				//预测值
	P_predict = (Mat::eye(stateNum, stateNum, CV_32F) - K*H)*P;		//预测值协方差

}

#include"kalman_filter.h"
#include<iostream>

using namespace std;
use_kalman_filter KF;

void main()
{
	Mat trans;
	///=================卡尔曼滤波器的初始化===============================
	//初始化的值有状态量的数值		stateNum
	//真实测量值的个数				measureNum
	//系统噪声方差矩阵				Q
	//测量噪声方差矩阵				R
	//预测值的协方差初始化			P
	//X的初始化						x_state
	//转移矩阵（运动方程）			F

	//kalman_filter_initial
	KF.stateNum = 4;
	KF.measureNum = 2;
	KF.init();
	KF.set_Q(0.0001);
	KF.set_R(0.01);
	KF.set_P(1);
	KF.x_state = (Mat_<float>(4, 1) << 100, 100, 0, 0);


	//预测阶段
	KF.get_F((Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1));	
	KF.z = (Mat_<float>(2, 1) << 50, 90);     						//测量值
	KF.correct();

	cout<<x_state.at<float>(1)<<"   "<< x_state.at<float>(2)<<endl;		//预测值

}