卡爾曼濾波器的理解，C程式碼實現，和opencv裡面KalmanFilter 的使用
阿新 • • 發佈：2019-01-03
背景：
卡爾曼濾波是一種高效率的遞迴濾波器(自迴歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計動態系統的狀態。卡爾曼濾波的一個典型例項是從一組有限的，包含噪聲的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。
這種濾波方法以它的發明者魯道夫.E.卡爾曼（Rudolph E. Kalman）命名，但是根據文獻可知實際上Peter Swerling在更早之前就提出了一種類似的演算法。
斯坦利.施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦便使用了這種濾波器。 關於這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發表。
目前,卡爾曼濾波已經有很多不同的實現.卡爾曼最初提出的形式現在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴充套件濾波器、資訊濾波器以及很多Bierman, Thornton 開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視訊或通訊裝置中廣泛存在。
從牛頓到卡爾曼
從現在開始，就要進行Kalman濾波器探討之旅了，我們先回到高一，從物理中小車的勻加速直線運動開始。
話說，有一輛質量為m的小車，受恆定的力F，沿著r方向做勻加速直線運動。已知小車在t-ΔT時刻的位移是s(t-1)，此時的速度為v(t-1)。求：t時刻的位移是s(t)，速度為v(t)？
由牛頓第二定律，求得加速度：
那麼就有下面的位移和速度關係：
如果將上面的表示式用矩陣寫在一起，就變成下面這樣：
卡爾曼濾波器是建立在動態過程之上，由於物理量（位移，速度）的不可突變特性，這樣就可以通過t-1時刻估計（預測）t時刻的狀態，其狀態空間模型為：
對比一下(1)(2)式，長得及其相似有木有：
勻加速直線運動過程就是卡爾曼濾波中狀態空間模型的一個典型應用。下面我們重點關注(2)式，鑑於研究的計算機訊號都是離散的，將(2)是表示成離散形式為：
其中各個量之間的含義是：
- x(n)是狀態向量，包含了觀測的目標（如：位移、速度）
- u(n)是驅動輸入向量，如上面的運動過程是通過受力驅動產生加速度，所以u(n)和受力有關
- A是狀態轉移矩陣，其隱含指示了“n-1時刻的狀態會影響到n時刻的狀態（這似乎和馬爾可夫過程有些類似）”
-
B是控制輸入矩陣，其隱含指示了“n時刻給的驅動如何影響n時刻的狀態”
從運動的角度，很容易理解：小車當前n時刻的位移和速度一部分來自於n-1時刻的慣性作用，這通過Ax(n)來度量，另一部分來自於現在n時刻小車新增加的外部受力，通過Bu(n)來度量。
-
w(n)是過程噪聲，w(n)~N(0,Q)的高斯分佈，過程噪聲是使用卡爾曼濾波器時一個重要的量，後面會進行分析。
計算n時刻的位移，還有一種方法：拿一把長的捲尺（嗯，如果小車跑了很長時間，估計這把卷尺就難買到了），從起點一拉，直接就出來了，設測量值為z(n)。計算速度呢？速度感測器往那一用就出來了。
然而，初中物理就告訴我們，“尺子是量不準的，物體的物理真實值無法獲得”，測量存在誤差，我們暫且將這個誤差記為v(n)。這種通過直接測量的方式獲得所需物理量的值構成觀測空間：
z(n)就是測量結果，H(n)是觀測向量，x(n)就是要求的物理量（位移、速度），v(n)~N(0,R)為測量噪聲，同狀態空間方程中的過程噪聲一樣，這也是一個後面要討論的量。大部分情況下，如果物理量能直接通過感測器測量，。
現在就有了兩種方法（如上圖）可以得到n時刻的位移和速度：一種就是通過(3)式的狀態空間遞推計算（Prediction），另一種就是通過(4)式直接拿尺子和感測器測量（Measurement）。致命的是沒一個是精確無誤的，就像上圖看到的一樣，分別都存在0均值高斯分佈的誤差w(n)和v(n)。
那麼，我最終的結果是取尺子量出來的好呢，還是根據我們偉大的牛頓第二定律推匯出來的好呢？抑或兩者都不是！
如下圖：估計量的高斯分佈和測量量的高斯分佈經過融合後為綠色的高斯分佈曲線。
稍微計算一下，通過上式求出u和σ^2，
現在令
重點注意：
其中的K增益的感官認識就是：就是估計量的方差佔總方差（包括估計方差和測量方差）的比重。
Kalman的引數中Q和R的設定非常重要，之前也提過，一般要通過實驗統計得到，它們分佈代表了狀態空間估計的誤差和測量的誤差。
其中q和r引數尤為重要，一般得通過實驗測試得到。
找兩組聲陣列測向的角度資料，對上面的C程式進行測試。一維Kalman（一維也是標量的情況，就我所知，現在網上看到的程式碼大都是使用標量的情況）和二維Kalman（一個狀態是角度值，另一個狀態是向量角度差，也就是角速度）的結果都在圖中顯示。這裡再稍微提醒一下：狀態量不要取那些能突變的量，如加速度，這點在文章“從牛頓到卡爾曼”一小節就提到過。
Matlab繪出的跟蹤結果顯示：
下面是C程式的原始碼：Kalman濾波結果比原訊號更平滑。但是有椒鹽突變噪聲的地方，Kalman濾波器並不能濾除椒鹽噪聲的影響，也會跟蹤椒鹽噪聲點。因此，推薦在Kalman濾波器之前先使用中值濾波演算法去除椒鹽突變點的影響。
#include "stdafx.h"
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <cassert>
#include <cmath>
#include <fstream>
using namespace std;
using namespace cv;
// This video stablisation smooths the global trajectory using a sliding average window
//const int SMOOTHING_RADIUS = 15; // In frames. The larger the more stable the video, but less reactive to sudden panning
const int HORIZONTAL_BORDER_CROP = 20; // In pixels. Crops the border to reduce the black borders from stabilisation being too noticeable.
// 1. Get previous to current frame transformation (dx, dy, da) for all frames
// 2. Accumulate the transformations to get the image trajectory
// 3. Smooth out the trajectory using an averaging window
// 4. Generate new set of previous to current transform, such that the trajectory ends up being the same as the smoothed trajectory
// 5. Apply the new transformation to the video
struct Trajectory
{
Trajectory() {}
Trajectory(double _x, double _y, double _a) {
x = _x;
y = _y;
a = _a;
}
// "+"
friend Trajectory operator+(const Trajectory &c1, const Trajectory &c2){
return Trajectory(c1.x + c2.x, c1.y + c2.y, c1.a + c2.a);
}
//"-"
friend Trajectory operator-(const Trajectory &c1, const Trajectory &c2){
return Trajectory(c1.x - c2.x, c1.y - c2.y, c1.a - c2.a);
}
//"*"
friend Trajectory operator*(const Trajectory &c1, const Trajectory &c2){
return Trajectory(c1.x*c2.x, c1.y*c2.y, c1.a*c2.a);
}
//"/"
friend Trajectory operator/(const Trajectory &c1, const Trajectory &c2){
return Trajectory(c1.x / c2.x, c1.y / c2.y, c1.a / c2.a);
}
//"="
Trajectory operator =(const Trajectory &rx){
x = rx.x;
y = rx.y;
a = rx.a;
return Trajectory(x, y, a);
}
double x;
double y;
double a; // angle
};
//
int main(int argc, char **argv)
{
// For further analysis
ofstream out_transform("prev_to_cur_transformation.txt");
ofstream out_trajectory("trajectory.txt");
ofstream out_smoothed_trajectory("smoothed_trajectory.txt");
ofstream out_new_transform("new_prev_to_cur_transformation.txt");
// ofstream out_trajectory("trajectory.txt");
VideoCapture cap("my.avi");
assert(cap.isOpened());
Mat cur, cur_grey;
Mat prev, prev_grey;
cap >> prev;//get the first frame.ch
prev.copyTo(cur);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
// Step 1 - Get previous to current frame transformation (dx, dy, da) for all frames
// Accumulated frame to frame transform
double a = 0;
double x = 0;
double y = 0;
// Step 2 - Accumulate the transformations to get the image trajectory
vector <Trajectory> trajectory; // trajectory at all frames
//
// Step 3 - Smooth out the trajectory using an averaging window
vector <Trajectory> smoothed_trajectory; // trajectory at all frames
Trajectory X;//posteriori state estimate
Trajectory X_;//priori estimate
Trajectory P;// posteriori estimate error covariance
Trajectory P_;// priori estimate error covariance
Trajectory K;//gain
Trajectory z;//actual measurement
double pstd = 4e-3;//can be changed 4e-3
double cstd = 0.25;//can be changed 0.25
/*他們被假設成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R
（這裡我們假設他們不隨系統狀態變化而變化）它們值的選取很重要，
一般要通過實驗統計得到，它們分佈代表了狀態空間估計的誤差和測量的誤差。。
*/
Trajectory Q(pstd, pstd, pstd);// process noise covariance
Trajectory R(cstd, cstd, cstd);// measurement noise covariance
// Step 4 - Generate new set of previous to current transform, such that the trajectory ends up being the same as the smoothed trajectory
// Step 5 - Apply the new transformation to the video
//cap.set(CV_CAP_PROP_POS_FRAMES, 0);
Mat T(2, 3, CV_64F);
//vert_border是影象穩像後大約要剪下的邊緣大小，其是與HORIZONTAL_BORDER_CROP成比例的，其實可以隨意
int vert_border = HORIZONTAL_BORDER_CROP * prev.rows / prev.cols; // get the aspect ratio correct
//VideoWriter outputVideo("compare.avi", CV_FOURCC('M', 'P', '4', '2'), 20, cvSize(prev.rows, prev.cols), 1);
IplImage*srcimg;
srcimg = &IplImage(prev);
CvVideoWriter*outputVideo;
//這裡的VideoWriter其實是可以用的，是自己定義影象cvSize(prev.rows, prev.cols)時值要與影象大小一致，否則製作的視訊沒用
outputVideo = cvCreateVideoWriter("camera.avi", CV_FOURCC('M', 'J', 'P', 'G'), 20, cvGetSize(srcimg));
int k = 1;
//獲得影象總幀數
int max_frames = cap.get(CV_CAP_PROP_FRAME_COUNT);
Mat last_T;
Mat prev_grey_, cur_grey_;
int framecoun = 0;
while (cur.data != NULL) {
cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
framecoun++;
// vector from prev to cur
vector <Point2f> prev_corner, cur_corner;
vector <Point2f> prev_corner2, cur_corner2;
vector <uchar> status;
vector <float> err;
//要記得引數的意義，200代表檢測角點的個數，0.01代表角點的質量，一般0.4，越大質量越好，30角點間的畫素距離
goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
// weed out bad matches
for (size_t i = 0; i < status.size(); i++) {
if (status[i]) {
prev_corner2.push_back(prev_corner[i]);
cur_corner2.push_back(cur_corner[i]);
}
}
// translation + rotation only 其是使用RANsc演算法，故有噪聲點也能很好的選取代表點
Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
// in rare cases no transform is found. We'll just use the last known good transform.
/*
這裡是為了防止程式崩掉，也可以用來在當檢測的點個數達不到estimateRigidTransform要求的3個點時，
進行使用上次的值，不至於崩掉
*/
if (T.data == NULL) {
last_T.copyTo(T);
}
T.copyTo(last_T);
// decompose T
double dx = T.at<double>(0, 2);
double dy = T.at<double>(1, 2);
double da = atan2(T.at<double>(1, 0), T.at<double>(0, 0));
//prev_to_cur_transform.push_back(TransformParam(dx, dy, da));
out_transform << k << " " << dx << " " << dy << " " << da << endl;
// Accumulated frame to frame transform
x += dx;//這裡是使用主運動來穩像
y += dy;
a += da;
out_trajectory << k << " " << x << " " << y << " " << a << endl;
z = Trajectory(x, y, a);
//
if (k == 1){
// intial guesses
X = Trajectory(0, 0, 0); //Initial estimate, set 0
P = Trajectory(1, 1, 1); //set error variance,set 1
}
else
{
//time update（prediction）
//記住A矩陣代表著狀態向量與測量向量之間的關係，一般一個測量向量就要有兩個狀態向量分別為:X，delt_X
//一般X與delt_X有關，delt_X與X無關，即構造矩陣時X的那一行有兩個1，delt_X的那一行只有一個1.
X_ = X; //X_(k) = X(k-1);
P_ = P + Q; //P_(k) = P(k-1)+Q;
// measurement update（correction）
K = P_ / (P_ + R); //gain;K(k) = P_(k)/( P_(k)+R );
X = X_ + K*(z - X_); //z-X_ is residual,X(k) = X_(k)+K(k)*(z(k)-X_(k));
P = (Trajectory(1, 1, 1) - K)*P_; //P(k) = (1-K(k))*P_(k);
}
//smoothed_trajectory.push_back(X);
out_smoothed_trajectory << k << " " << X.x << " " << X.y << " " << X.a << endl;
//-
// target - current diff_x是估計的主運動與現實的主運動之間的差值，這裡把其當做偏差值
double diff_x = X.x - x;//
double diff_y = X.y - y;
double diff_a = X.a - a;
dx = dx + diff_x; //進行真正的運動矯正
dy = dy + diff_y;
da = da + diff_a;
out_new_transform << k << " " << dx << " " << dy << " " << da << endl;
//
T.at<double>(0, 0) = cos(da);
T.at<double>(0, 1) = -sin(da);
T.at<double>(1, 0) = sin(da);
T.at<double>(1, 1) = cos(da);
T.at<double>(0, 2) = dx;
T.at<double>(1, 2) = dy;
Mat cur2;
warpAffine(prev, cur2, T, cur.size());
//cur2 = cur2(Range(vert_border, cur2.rows - vert_border), Range(HORIZONTAL_BORDER_CROP, cur2.cols - HORIZONTAL_BORDER_CROP));
// Resize cur2 back to cur size, for better side by side comparison
//resize(cur2, cur2, cur.size());
srcimg = &IplImage(cur2);
cvWriteFrame(outputVideo, srcimg);
// Now draw the original and stablised side by side for coolness
//把影象現實在同一個畫布上
Mat canvas = Mat::zeros(cur.rows, cur.cols * 2 + 10, cur.type());
prev.copyTo(canvas(Range::all(), Range(0, cur2.cols)));
cur2.copyTo(canvas(Range::all(), Range(cur2.cols + 10, cur2.cols * 2 + 10)));
// If too big to fit on the screen, then scale it down by 2, hopefully it'll fit :)
if (canvas.cols > 1920) {
resize(canvas, canvas, Size(canvas.cols / 2, canvas.rows / 2));
}
imshow("before and after", canvas);
waitKey(10);
//
prev = cur.clone();//cur.copyTo(prev);
cur_grey.copyTo(prev_grey);
cout << "Frame: " << k << "/" << max_frames << " - good optical flow: " << prev_corner2.size() << endl;
k++;
cap >> cur;
}
cvReleaseVideoWriter(&outputVideo);
return 1;
}
下面的是opencv裡的例程：
1個1維點的運動跟蹤，雖然這個點是在2維平面中運動，但由於它是在一個圓弧上運動，
只有一個自由度，角度，所以還是1維的。還是一個勻速運動，建立勻速運動模型，設定狀態變數x = [x1, x2] = [角度，角速度]
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <stdio.h>
using namespace std;
using namespace cv;
//計算相對視窗的座標值，因為座標原點在左上角，所以sin前有個負號
static inline Point calcPoint(Point2f center, double R, double angle)
{
return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}
static void help()
{
printf("\nExamle of c calls to OpenCV's Kalman filter.\n"
" Tracking of rotating point.\n"
" Rotation speed is constant.\n"
" Both state and measurements vectors are 1D (a point angle),\n"
" Measurement is the real point angle + gaussian noise.\n"
" The real and the estimated points are connected with yellow line segment,\n"
" the real and the measured points are connected with red line segment.\n"
" (if Kalman filter works correctly,\n"
" the yellow segment should be shorter than the red one).\n"
"\n"
" Pressing any key (except ESC) will reset the tracking with a different speed.\n"
" Pressing ESC will stop the program.\n"
);
}
int main(int, char**)
{
help();
Mat img(500, 500, CV_8UC3);
KalmanFilter KF(2, 1, 0); //建立卡爾曼濾波器物件KF
Mat state(2, 1, CV_32F); //state(角度，△角度)
Mat processNoise(2, 1, CV_32F);
Mat measurement = Mat::zeros(1, 1, CV_32F); //定義測量值
char code = (char)-1;
for (;;)
{
//1.初始化
randn(state, Scalar::all(0), Scalar::all(0.1)); //
KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1); //轉移矩陣A[1,1;0,1]
//將下面幾個矩陣設定為對角陣 （H）其一般設為1
setIdentity(KF.measurementMatrix); //測量矩陣H
// wk 是過程噪聲，並假定其符合均值為零，協方差矩陣為Qk(Q)的多元正態分佈;
setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); //系統噪聲方差矩陣Q
//vk 是觀測噪聲，其均值為零，協方差矩陣為Rk,且服從正態分佈;
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //測量噪聲方差矩陣R
setIdentity(KF.errorCovPost, Scalar::all(1)); //後驗錯誤估計協方差矩陣P
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); //x(0)初始化
for (;;)
{
Point2f center(img.cols*0.5f, img.rows*0.5f); //center影象中心點
float R = img.cols / 3.f; //半徑
double stateAngle = state.at<float>(0); //跟蹤點角度
Point statePt = calcPoint(center, R, stateAngle); //跟蹤點座標statePt
//2. 預測
Mat prediction = KF.predict(); //計算預測值，返回x'
double predictAngle = prediction.at<float>(0); //預測點的角度
Point predictPt = calcPoint(center, R, predictAngle); //預測點座標predictPt
//3.更新
//measurement是測量值
randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); //給measurement賦值N(0,R)的隨機值
// generate measurement
measurement += KF.measurementMatrix*state; //z = Vk + H*x;
double measAngle = measurement.at<float>(0);
Point measPt = calcPoint(center, R, measAngle);
// plot points
//定義了畫十字的方法，值得學習下
#define drawCross( center, color, d ) \
line(img, Point(center.x - d, center.y - d), \
Point(center.x + d, center.y + d), color, 1, CV_AA, 0); \
line(img, Point(center.x + d, center.y - d), \
Point(center.x - d, center.y + d), color, 1, CV_AA, 0)
img = Scalar::all(0);
drawCross(statePt, Scalar(255, 255, 255), 3);
drawCross(measPt, Scalar(0, 0, 255), 3);
drawCross(predictPt, Scalar(0, 255, 0), 3);
line(img, statePt, measPt, Scalar(0, 0, 255), 3, CV_AA, 0);
line(img, statePt, predictPt, Scalar(0, 255, 255), 3, CV_AA, 0);
//呼叫kalman這個類的correct方法得到加入觀察值校正後的狀態變數值矩陣
if (theRNG().uniform(0, 4) != 0)
KF.correct(measurement);
//不加噪聲的話就是勻速圓周運動，加了點噪聲類似勻速圓周運動，因為噪聲的原因，運動方向可能會改變
randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); //vk
/*這裡的是為了上面的measurement做準備，因為其是使用了下圖中的狀態方程和測量方程一起搭配使用
其中的measurement就是測量量，即Zk，*/
state = KF.transitionMatrix*state + processNoise;
imshow("Kalman", img);
code = (char)waitKey(100);
if (code > 0)
break;
}
if (code == 27 || code == 'q' || code == 'Q')
break;
}
return 0;
}
<span style="font-size:18px;">#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>
using namespace cv;
using namespace std;
const int winHeight=600;
const int winWidth=800;
Point mousePosition= Point(winWidth>>1,winHeight>>1);
//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)
{
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(1e-5)); //系統噪聲方差矩陣Q
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)
Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //初始測量值x'(0)，因為後面要更新這個值，所以必須先定義
namedWindow("kalman");
setMouseCallback("kalman",mouseEvent);
Mat image(winHeight,winWidth,CV_8UC3,Scalar(0));
while (1)
{
//2.kalman prediction
Mat prediction = KF.predict();
Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1) ); //預測值(x',y')
//3.update measurement
measurement.at<float>(0) = (float)mousePosition.x;
measurement.at<float>(1) = (float)mousePosition.y;
//4.update
KF.correct(measurement);
//draw
image.setTo(Scalar(255,255,255,0));
circle(image,predict_pt,5,Scalar(0,255,0),3); //predicted point with green
circle(image,mousePosition,5,Scalar(255,0,0),3); //current position with red
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;
}
}
}
</span>