KF、EKF、UKF在传感器融合当中的应用

“不必遗憾。若是美好，叫做精彩。若是糟糕，叫做经历。”

 

 

2018年的最后一天

希望今年所有的遗憾，都是明年惊喜的铺垫！

致所有努力的人～

 

将KF、EKF、UKF在自动驾驶传感器融合当中的应用整理下吧：

卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）、无损卡尔曼滤波（UKF）

 

首先要搞清楚两个问题：1. 为什么要用KF、EKF、UKF？

                                            2. 什么是KF、EKF、UKF？

先从最简单的KF说起吧～

由于传感器本身的局限性以及自然界存在的噪声影响，导致我们依据传感器获取的任何测量结果都是有误差的，也就是我们通常无法精确的知道物体当前的状态。因此我们为了估计一个事物的状态，往往会去测量它，但是我们不能完全相信我们的测量，因为我们的测量是不精准的，它往往会存在一定的噪声，这个时候就需要在传感器测量结果的基础上，对目标的运动状态进行跟踪估计，以此来保证障碍物的位置、速度等信息不会发生突变而且是准确的。

KF、EKF以及UKF则是几种结合预测（先验分布）和测量更新（似然）的状态估计算法。这些算法能够应用于目标状态估计（目标可以是行人，车辆、GPS数据等）。
 

具体介绍KF的例程，网上的资料太多了，在此就不细说了，其实总的来说就3部分：初始化，预测，测量更新。

推荐两个网址：1. 知乎的陈光大神：https://zhuanlan.zhihu.com/p/45238681

                            2. AdamShan博客专家：https://blog.csdn.net/AdamShan/article/details/78248421

核心公式：

 对于KF来讲，比较简单，而且Opencv里也有对应的cv::KalmanFilter模块，可以直接使用，其实它内部实现也是根据以上核心共式来封装的；当然也可以自己实现，下面就分别两种方法给出代码：

1.使用opencv里封装好的写的一个.hpp

/**************************************************************************
 * @author z.h
 * @date 2018.12.28
 * usage:kalman_filter_cv.hpp //opencv
 *
 **************************************************************************/

#ifndef KALMAN_FILTER_CV_H
#define KALMAN_FILTER_CV_H

#include <opencv2/opencv.hpp>

class kalman_filter_cv
{
public:
	kalman_filter_cv();
	~kalman_filter_cv();

	void _init(std::vector<float> v, int _mode)
	{
		if(_mode == 4)
		{
			state_num = 4;
			measure_num = 2;

			kf.init(state_num, measure_num, 0);
			kf.transitionMatrix = (cv::Mat_<float>(state_num, state_num) << 
				1, 0, 1, 0, 
				0, 1, 0, 1, 
				0, 0, 1, 0, 
				0, 0, 0, 1);

			setIdentity(kf.measurementMatrix, cv::Scalar::all(1));
			setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-5));
			setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1));
			setIdentity(kf.errorCovPost, cv::Scalar::all(1));

			kf.statePost.at<float>(0, 0) = v[0];
			kf.statePost.at<float>(1, 0) = v[1];
			kf.statePost.at<float>(2, 0) = 0;
			kf.statePost.at<float>(3, 0) = 0;
		}
		else if(_mode == 8)
		{
			state_num = 8;
			measure_num = 4;

			kf.init(state_num, measure_num, 0);

			kf.transitionMatrix = (cv::Mat_<floa