卡爾曼濾波
文章目錄
- 卡爾曼濾波
-
- 卡爾曼濾波簡介
- Dynamic model（實時系統）
- 目标跟蹤分析
- 卡爾曼濾波五大更新方程
-
- 時間更新
- 狀态更新
- 代碼實作——小球跟蹤
- 參考資料：
卡爾曼濾波簡介
卡爾曼濾波是一種高效率的遞歸濾波器（自回歸濾波器），它能夠從一系列不完全及包含噪聲的測量中，估計動态系統的位置。
卡爾曼濾波的一個典型執行個體是從一組有限的、包含噪聲的對物體位置的觀察序列（可能有偏差）預測出物體的位置坐标以及速度。
卡爾曼濾波算法的核心是動态調整參數。
Dynamic model（實時系統）
假設有一系列觀測值 y 1 , y 2 , . . . , y t − 2 , y t − 1 , y t y_{1},y_{2},...,y_{t-2},y_{t-1},y_{t} y1,y2,...,yt−2,yt−1,yt，組成了一個set（集合），set資料順序不可以互換，而series可以互換，互換後得到的結果是一樣的。
Dynamic model（state-space model（狀态空間模型））的中心思想是，觀測值 y t − 2 , y t − 1 , y t y_{t-2},y_{t-1},y_{t} yt−2,yt−1,yt之間的關系絕對不是互相獨立的，是通過對應的隐狀态 x t − 2 ， x t − 1 , x t x_{t-2}，x_{t-1},x_{t} xt−2，xt−1,xt聯系起來。隐狀态之間都有一個機率，這個機率的意義是已知前一個隐狀态的值，下一個隐狀态的出現的值的機率。當所有隐狀态的值都已經得知了，所有的觀測都變成互相獨立了。
其中的綠色箭頭的機率（transition prob）是 P ( x t ∣ x t − 1 ) P(x_{t}|x_{t-1}) P(xt∣xt−1)，表示根據前面的狀态 x t − 1 x_{t-1} xt−1，現在狀态 x t x_{t} xt的機率是多少；紅色的箭頭的機率（measurement prob）是 P ( y t ∣ x t ) P(y_{t}|x_{t}) P(yt∣xt)代表，已知對應隐狀态的值，觀測值的機率。如果要完全的去定義一個Dynamic model，還需要知道一個初始機率（initial prob） P ( x 1 ) P(x_{1}) P(x1)，三個機率可以完全描述一個state-space model。
目标跟蹤分析
根據質點坐标和速度，系統的狀态變量為 x t = ( x , y , v x , v y ) T x_{t}=(x,y,v_{x},v_{y})^{T} xt=(x,y,vx,vy)T，這些量是直接觀測得到。系統的觀測變量為 y t = ( x ‾ , y ‾ ) y_{t}=(\overline{x},\overline{y}) yt=(x,y)。
由速度公式，令 Δ t = 1 \Delta t=1 Δt=1可得到狀态轉移矩陣為：
A = [ 1 , 0 , 1 , 0 0 , 1 , 0 , 1 0 , 0 , 1 , 0 0 , 0 , 0 , 1 ] A=\left[\begin{matrix} 1,0,1,0\\ 0,1,0,1\\ 0,0,1,0\\ 0,0,0,1\end{matrix}\right] A=⎣⎢⎢⎡1,0,1,00,1,0,10,0,1,00,0,0,1⎦⎥⎥⎤
H = [ 1 , 0 , 0 , 0 0 , 1 , 0 , 0 ] H=\left[\begin{matrix} 1,0,0,0\\0,1,0,0\\ \end{matrix}\right]\ H=[1,0,0,00,1,0,0]
卡爾曼濾波五大更新方程
有時間更新和狀态更新：
時間更新
x ‾ t = A x t − 1 + B u t − 1 \overline{x}_{t}=Ax_{t-1}+Bu_{t-1} xt=Axt−1+But−1 (1)
通過上一時刻的狀态最優值和要施加的控制量來預測目前狀态。由隻是求取平均值，高斯噪聲均值為0，是以可以省去 ω \omega ω這一項。
x ‾ t \overline{x}_{t} xt：目前時刻的估計值
A A A：目前時刻的狀态轉移矩陣
x t − 1 x_{t-1} xt−1：上一時刻的最優估計值
B B B：目前時刻的控制矩陣
u t u_{t} ut：目前時刻的控制量
P ‾ t = A P t − 1 A T + Q \overline{P}_{t}=AP_{t-1}A^{T}+Q Pt=APt−1AT+Q (2)
除了預測均值之外，還需要預測值的協方差來計算Kalman增益。由上一次的誤差協方差矩陣 P k − 1 P_{k-1} Pk−1和過程噪聲 Q Q Q預測新的誤差 P ‾ k \overline{P}_{k} Pk。
P k − 1 P_{k-1} Pk−1：上一時刻預測值的協方差矩陣
Q Q Q：目前時刻測量值的噪聲矩陣
狀态更新
K t = P ‾ t C T ( C P ‾ t C T + R ) − 1 K_{t}=\overline{P}_{t}C^{T}(C\overline{P}_{t}C^{T}+R)^{-1} Kt=PtCT(CPtCT+R)−1 (3)
根據預測值的協方差 P ‾ t \overline{P}_{t} Pt，測量值和狀态的比例系數 C C C，測量值的協方差 R R R計算Kalman增益 K t K_{t} Kt。
x t = x ‾ t + K t ( y t − C x ‾ t ) x_{t}=\overline{x}_{t}+K_{t}(y_{t}-C\overline{x}_{t}) xt=xt+Kt(yt−Cxt) (4)
這一步是Kalman Filter的精華部分，現在已經有了對狀态的預測值和協方差同時也收集到了狀态的測量值，此時可以通過Kalman增益來計算狀态估計值，進行校正更新。增益越大，表明越相信測量值。
P t = ( I − K t C ) P ‾ t P_{t}=(I-K_{t}C)\overline{P}_{t} Pt=(I−KtC)Pt (5)
為了進行下一次疊代，需要計算目前狀态的協方差。
Kalman Filter的算法中，從初始狀态開始，不斷計算目前狀态的均值和方差來疊代，直至系統結束。
卡爾曼濾波有以下兩種作用。第一，用作單種資料濾波，那麼将資料作為測量量創數模型，卡爾曼濾波通過上一次的值估計出下一次的值，然後将此次的估計值和測量值分别取一定的權重（卡爾曼增益），進而求出這次的最優質。第二，多資料的融合，将一種資料作為測量量，另一種資料作為估計值進行融合。
代碼實作——小球跟蹤
// Kalman_F.cpp: 定義控制台應用程式的入口點。
#include "stdafx.h"
#include <video/tracking.hpp>
#include <highgui/highgui.hpp>
#include <cmath>
#include <vector>
#include <stdio.h>
#include <iostream>
using namespace cv;
using namespace std;
const int winHeight = 600;
const int winWidth = 800;
Point move_mouse = Point(winWidth >> 1, winHeight >> 1); //右移一位，相當于除2
//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param)
{
if (event == CV_EVENT_MOUSEMOVE) //滑動滑鼠
{
move_mouse = Point(x, y);
}
}
int main()
{
//1.kalman filter setup
const int stateNum = 4; //狀态數，（x,y,dx,dy）
const int measureNum = 2; //觀測量（坐标值）
KalmanFilter kalman(stateNum, measureNum, 0);
/* KalmanFilter::KalmanFilter(intdynamParams, intmeasureParams, int controlParams = 0, inttype = CV_32F)
dynamParams:dimentionality of the state; measureParams:dimentionality of the measurement
controlParams:dimentionality of the control vector; type:Type of the created matrices that should be CV_32F orCV_64F */
Mat processNoise(stateNum, 1, CV_32F);
Mat measureNoise(stateNum, 1, CV_32F);
Mat measurement(measureNum, 1, CV_32F);
float A[stateNum][stateNum] = {//transition matrix
1,0,1,0,
0,1,0,1,
0,0,1,0,
0,0,0,1
}; //二維就是四個，三維就是六個
memcpy(kalman.transitionMatrix.data, A, sizeof(A)); //拷貝
cout << kalman.transitionMatrix << endl;
//定義矩陣
//setIdentity: 縮放的機關對角矩陣，給參數矩陣對角線賦相同值
setIdentity(kalman.measurementMatrix);
setIdentity(kalman.processNoiseCov, Scalar::all(1e-5));
setIdentity(kalman.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(kalman.errorCovPost, Scalar::all(1));
//initialize post state of kalman filter at random
randn(kalman.statePost, Scalar::all(0), Scalar::all(0.1)); //随機生成一個矩陣，期望是0，标準差為0.1;
namedWindow("kalman", 1);
setMouseCallback("kalman", mouseEvent);
Mat img(500, 500, CV_8UC3);
while (1)
{
//2.kalman prediction
Mat prediction = kalman.predict();
Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1));
//3.update measurement
measurement.at<float>(0) = (float)move_mouse.x;
measurement.at<float>(1) = (float)move_mouse.y;
//4.update
kalman.correct(measurement);
//5.draw
img = Scalar::all(0);
circle(img, predict_pt, 4, CV_RGB(0, 255, 0), 2);//predicted point with green
circle(img, move_mouse, 4, CV_RGB(255, 0, 0), 2);//current position with red
//定義字型，顯示字
putText(img, "Green: Predicted Point", cvPoint(10, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
putText(img, "Red: Current Point", cvPoint(10, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
imshow("kalman", img);
int key = cvWaitKey(3);
if (key == 27)
{
break;
}
}
system("pause");
return 0;
}
參考資料：
徐亦達博士關于卡爾曼濾波的公開課，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html
CSDN，再談卡爾曼濾波–五大公式，https://me.csdn.net/woaizgw)https://blog.csdn.net/woaizgw/article/details/73648578
CSDN，卡爾曼濾波算法的深入了解，https://blog.csdn.net/L_smartworld/article/details/82145013
![無人駕駛2：卡爾曼濾波行人狀态估計[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)