Kalman濾波器的詳細使用和講解

這兩天一直在研究Kalman濾波器，剛開始開的時候對代碼很不理解，但是沒辦法，誰叫哥就是學這個的呢，所以硬着頭皮死看代碼，以及看了一些關於對kalman濾波器的使用的博文，現在已經大致掌握kalman濾波器的使用步驟。(轉自http://zhuyuge0.blog.163.com/blog/static/13230361420117541511810/)

首先我們要了解kalman濾波器的基本知識，這樣在之後的介紹能起到拋磚引玉的作用，當然還是和之前筆者關於opencv的博文一樣，對於kalman的具體數學推導過程不做介紹，不過看了一下也不是很難，主要涉及到一些關於概率和線性代數的知識，有興趣的朋友可以看下，還可以一起討論一下，呵呵。

好，下面進入正題，關於kalman濾波器的基本思想是若有一組強而合理的假設，給出系統的歷史測量值，則可以最大化這些早前測量值的後驗概率的系統狀態模型。有的朋友可能對最大化後驗概率概念不瞭解，它就是考慮早前的模型和新測量值的不確定性，在獲得測量值之後，新建立的模型是具有最高正確概率的模型。

kalman filter的三條假設也有必要說一下：1、被建模型系統是線性的，即k時刻系統狀態可以用某個矩陣與k-1時刻的乘機表達

2、影響測量的噪聲是白噪聲，即噪聲與時間不相關；

3、噪聲的本質是高斯分佈的，即只用均值和協方差就可以準確爲幅值建模。（以上知識可以參考於劉opencv learning）

下面開始使用kalman濾波器，在此之前，我們必須瞭解CvKalman的結構，還有cvCreateKalman、cvKalmanPredict、cvKalmanCorrect的使用方法和實現的作用（可以參照下面的代碼示例），這些基本的就不詳細介紹。

看了某位仁兄的博文，我發現他對kalman使用總結很到位，下面筆者在他的基礎之上重新說明下Kalman濾波器的使用步驟：

在這些過程中，第一步相對來說最複雜，我們光光利用cvcreatekalman建立濾波器是不夠的，必須對其初始化，包括對CvKlaman中每個矩陣進行初始化（除了error_cov_pre和gain無需初始化，因爲在剛開始時我們當然不會知道先驗錯誤的相關矩形，而對於gain來說，它的計算需要用到error_cov_pre，所以gain（增益矩陣）也無需初始化），包括對transition_matrix（轉移矩陣）,control_matrix（控制矩陣）,measurement_matrix（測量矩陣）,process_noise_cov（處理噪聲相相關矩陣）,measurement_noise_cov（測量噪聲相關矩陣）,error_cov_post（後驗錯誤矩陣），進行初始化。

下面採用一個簡單的例子來講解：

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <cmath>
#include <vector>
#include <iostream>
using namespace std;
const int winHeight=600;
const int winWidth=800;
CvPoint mousePosition=cvPoint(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=cvPoint(x,y);
}
}
int main (void)
{
//1.kalman filter setup
const int stateNum=4;
const int measureNum=2;
CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)
CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)
CvRNG rng = cvRNG(-1);//這個函數也許有的朋友不知道，初始化隨機數生成器的狀態
float A[stateNum][stateNum] ={//transition matrix傳遞矩陣
  1,0,1,0,
  0,1,0,1,
  0,0,1,0,
  0,0,0,1
};
memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));//void *memcpy(void *dest, const void *src, size_t n);從源src所指的內存地址 // 的起始位置開始拷貝n個字節到目標dest所指的內存地址的起始位置中
cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );//cvRealScalar是用value初始化val[0],0初始化val[1],val[2],val[3]
cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));//cvSetIdentity是初始化帶單位的矩陣，第二個參數是賦給對角線的值
cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));
cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));
//initialize post state of kalman filter at random

cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));
CvFont font;//用隨機數填充數組並更新RNG狀態，CV_RAND_UNI表示均勻分佈，隨後兩個參數表示隨機數的上下界
cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);//我的博客上有相關文章可以參考
cvNamedWindow("kalman");
cvSetMouseCallback("kalman",mouseEvent);
IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
while (1){
  //2.kalman prediction
  const CvMat* prediction=cvKalmanPredict(kalman,0);
  CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);
  //3.update measurement
  measurement->data.fl[0]=(float)mousePosition.x;
  measurement->data.fl[1]=(float)mousePosition.y;
  //4.update
  cvKalmanCorrect( kalman, measurement );
  //draw
  cvSet(img,cvScalar(255,255,255,0));
  cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
  cvCircle(img,mousePosition,5,CV_RGB(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);
  cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
  sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
  cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));

  cvShowImage("kalman", img);
  int key=cvWaitKey(3);
  if (key==27){//esc
   break;
  }
}
cvReleaseImage(&img);
cvReleaseKalman(&kalman);
return 0;
}