Kalman Filter原理简介及C++实现

本博文内容参考了北卡罗来纳大学教堂山分校的文章  An Introduction to the Kalman Filter。

目录

一、Kalman Filter简介

二、估计与观测过程

三、KF的计算起源（Computational Origins of the Filter）

四、离散KF算法（Discrete Kalman Filter Algorithm）

---

一、Kalman Filter简介

1960年R.E. Kalman（匈牙利裔美国数学家，MIT本科，哥大博士。图为卡尔曼老爷子智慧的凝视。）发表了一篇著名的文章，描述离散线性滤波问题的递归解法。随着数字计算技术的不断发展，卡尔曼滤波器已经渗透到各学科领域，尤其是在控制科学和辅助导航领域。KF的本质是一组数学方程，用递归的方式来估计过程的状态（最小化根误差的均值）。KF的强大之处有以下几点：支持对past, present, 甚至是 future states的估计，即使是在系统精度未知的情况下。

---

二、估计与观测过程

KF的目的是对系统的状态向量x进行估计，一般是列向量n*1。KF通过一个含有随机量的差分方程来对系统的状态进行估计：

其中的x_k-1是当前时刻的状态，x_k是下一时刻的状态。A是n*n转移矩阵，B是n*l控制矩阵。w是状态转移过程的噪音。

因为我们对系统的观测并不是完美的，会存在一些测量噪音。故观测方程为：

其中的H是m*n观测矩阵，将n*1状态转化为m*1观测值。同时还要加上一个观测过程的偏差v。

同时，状态转移过程噪声w和测量噪声v都服从normal probability distributions：

其中Q称为process noise covariance，R称为measurement noise covariance。这两个量会在后续内容中起到重要作用。

---

三、KF的计算起源（Computational Origins of the Filter）

首先，我们假定一个系统的真实值是无法获取的，但是我们有两个数据来源来去估计状态值：1.可以根据系统的参数（转移矩阵A、控制矩阵B）去推理下一时刻的系统状态；2.我们采用一种有误差的测量方法去测量状态值。

我们定义，根据系统参数推断出的状态叫priori state estimate，即先验状态估计(上标减号意味着还不完整，只是推理出来的值)。在priori的基础上，根据有误差的测量量进行修正过后的状态是posteriori state estimate，即后验估计。一个例子可以描述一下这个过程：假设体温是以天为单位进行变化，我今天烧37度，然后我根据经验掐指一算明天肯定38度（先验估计），但是真到了明天我拿体温计一量居然是39度，这个破体温计肯定有误差，所以第二天我简单粗暴折衷了一下烧38.5度（后验估计）。

好了，明确了这两个估计值，我们来引入两个误差：（疑问：这个真实值xk应该是永远也不明确的，为何可以直接用于计算？）

先验估计误差：

后验估计误差：

两个误差都是m*1维的列向量，现在将其转化为协方差矩阵：

先验估计误差协方差：

后验估计误差协方差：

接下来我们看一个非常重要的公式，由先验估计和观测值来得到后验估计：

计算后验估计：

其中的称为残差，表示测量值与先验估计的偏差，注意此量的维度是m*1。K是卡尔曼滤波增益矩阵，负责将m*1的测量偏差量映射成为与状态相同维度的量，以便进行加和，K的维度是n*m。其实K的作用就是在调节预测和观测的关系。

接下来的任务就是调节卡尔曼滤波器增益矩阵K的值，使得和真实的之间误差最小，即后验估计误差协方差最小。我们使用的方法就是先用减去，然后取期望，再对K求导，使倒数为0。最终可以得到K的计算公式：

K计算：

也即：

其中的R是测量过程的噪声协方差。可以对K进行定性分析：

★  如果测量非常精准，测量误差趋近于0，R趋近于0，那么上边分式简化以后右侧只剩一个,即：

★  如果预测非常精准，预测误差趋近于0，趋近于0，那么K就应该为0，不用考虑测量结果：

---

四、离散KF算法（Discrete Kalman Filter Algorithm）

Kalman Filter在某一时刻进行状态的预测，并且得到这一时刻的噪声测量反馈。因此，Kalman Filter组：可以分为两个组：time update 和 measurement update。前者负责向前推进状态和误差协方差的估计来获得先验估计。后者负责反馈，对测量结果和先验估计进行联合，得到后验估计。或者称为：predictor equations 和 corrector equations 
 

两个阶段的公式如下：

更新过程就是这样：

---

五、Kalman Filter 的 C++实现（基于OpenCV）

本程序共有两路轨迹：自动生成的直线轨迹 和 实时鼠标数据。

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>
#include <iostream>

using namespace cv;
using namespace std;
const int winHeight = 700;
const int winWidth = 1200;

KalmanFilter CreateKF(double sys_noise);
Point Kalman_Predict(KalmanFilter KF);
Point mousePosition = Point(winWidth >> 1, winHeight >> 1);
bool Match_straegy_bin(Point pridict_1, Point pridict_2, Mat measurement_A, Mat measurement_B);

//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param)
{
	if (event == CV_EVENT_MOUSEMOVE) {
		mousePosition = Point(x, y);
	}
}

int main(void)
{
	int i = 0;
	const int measureNum = 2;
	Point track[2][5000];
	Point predict_pt, predict_pt_1;	// 预测用的点

	KalmanFilter KF = CreateKF(1e-5);	// 创建一个Kalman Filter
	KalmanFilter KF_1 = CreateKF(1e-6);

	// 生成假数据
	Point data[2][800];
	for (int i = 0; i < 800; i++)
		data[0][i] = Point(i + 50, 200);
	for (int j = 0; j < 800; j++)
		data[1][j] = Point(j + 50, 200);
	
	//初始测量值x'(0)，因为后面要更新这个值，所以必须先定义
	Mat measurement_A = Mat::zeros(measureNum, 1, CV_32F);                           
	Mat measurement_B = Mat::zeros(measureNum, 1, CV_32F);

	// 新建一个窗口界面
	namedWindow("kalman");
	setMouseCallback("kalman", mouseEvent);
	Mat image(winHeight, winWidth, CV_8UC3, Scalar(0));

	for(int k=100;k<500;k++)
	{
		//1.kalman prediction	
		predict_pt = Kalman_Predict(KF);
		predict_pt_1 = Kalman_Predict(KF_1);

		//2.update measurement
		//顺序版本的数据
		measurement_A.at<float>(0) = data[0][k].x;
		measurement_A.at<float>(1) = data[0][k].y;
		//鼠标+规整数据
		measurement_B.at<float>(0) = (float)mousePosition.x;
		measurement_B.at<float>(1) = (float)mousePosition.y;

		//使用匹配策略
		bool match_1_A = Match_straegy_bin(predict_pt, predict_pt_1, measurement_A, measurement_B);
		if (match_1_A)
		{
			KF.correct(measurement_A);
			KF_1.correct(measurement_B);
		}
		else
		{
			KF.correct(measurement_B);
			KF_1.correct(measurement_A);
		}

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

		track[0][i] = predict_pt;
		track[1][i] = predict_pt_1;
		

		// 绘制轨迹
		for (int j = 0; j < i; j++)
		{
			circle(image, track[0][j], 3, Scalar(0, 0, 0), 1);
			circle(image, track[1][j], 3, Scalar(225, 225, 0), 1);
		}

		i = i + 1;

		char buf[256];
		sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y);
		putText(image, buf, Point(10, 30), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);

		sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y);
		putText(image, buf, cvPoint(10, 60), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);

		imshow("kalman", image);
		int key = waitKey(3);
		if (key == 27) {//esc   
			break;
		}
	}
	return 0;
}

KalmanFilter CreateKF(double sys_noise)		// 创建一个Kalman滤波器
{
	RNG rng;
	//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(sys_noise));                            //系统噪声方差矩阵Q 1e-5
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));                        //测量噪声方差矩阵R
	setIdentity(KF.errorCovPost, Scalar::all(1));                                  //后验错误估计协方差矩阵P
	rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight > winWidth ? winWidth : winHeight);   //初始状态值x(0)
	return KF;
}

Point Kalman_Predict(KalmanFilter KF)
{
	Mat prediction = KF.predict();
	Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1));   //预测值(x',y')
	return predict_pt;
}

bool Match_straegy_bin(Point pridict_1, Point pridict_2, Mat measurement_A, Mat measurement_B)
{
	//检验预测1和A是否匹配
	double delt_1, delt_2;
	delt_1 = sqrt(pow((measurement_A.at<float>(0) - pridict_1.x), 2) + pow((measurement_A.at<float>(1) - pridict_1.y), 2));
	delt_2 = sqrt(pow((measurement_B.at<float>(0) - pridict_1.x), 2) + pow((measurement_B.at<float>(1) - pridict_1.y), 2));
	if (delt_1 < delt_2)
		return true;
	else
		return false;
}