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;
}