卡爾曼濾波matlab_卡爾曼濾波(kalaman Filter)

學習參考：

卡爾曼濾波器的原理以及在matlab中的實作

Opencv實作Kalman濾波器

opencv中的KF源碼分析

Opencv-kalman-filter-mouse-tracking

了解：

假設：一個小車距離左側某一物體k時刻的真實位置狀态

,而位置狀态觀測值為

,則小車的線性動态系統可表示為：

位置狀态的系統預測值：

位置狀态的觀測值：

其中F為系統狀态轉移矩陣，B為控制輸入矩陣，H為系統狀态觀測矩陣，

為系統預測過程中的噪聲（其均值為0，協方差為Q），

為系統測量過程中的噪聲（其均值為0，協防方差為R）.

則卡拉曼濾波的思想就是利用Kalaman增益修正預測值

使其逼近真實值

來計算最優估計值

。

Kalaman步驟：

1. 通過上一時刻的估計值預測目前狀态的估計值

   

2. 計算先驗誤差估計協方差矩陣

   

3. 計算此時卡爾曼增益

   

4. 更新估計值

   

5. 計算後驗誤差估計協方差

   

6. 

   ,循環1-5。

Kalaman優點：

• 時域濾波器，不需變換到頻域設計
• 一種遞歸估計，不需要記錄曆史觀測值和估計值

Kalaman的實作：

• kalaman.hpp

namespace cv{


class CV_EXPORTS_W KalmanFilter
{
public:
    //預設構造函數：無論構造函數是自動生成的還是使用者定義的，它都是可以調用的構造函數，無需提供任何參數。
    CV_WRAP KalmanFilter();
    //完整構造函數
    CV_WRAP KalmanFilter(int dynamparams,int measureparamsint, int controlParams=0, int type=CV_32F);  
    //dynamparams:狀态次元
    //measureparamsint：觀測次元
    //controlParams：控制次元
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);

    //計算預測狀态
    CV_WRAP const Mat& predict(const Mat& control=Mat());  
    //依據觀測值更新預測狀态 
    CV_WRAP const Mat& correct(const Mat& measurement);  
  
    Mat statePre;           //!< 預測狀态 (x'(k)): x(k)=A*x(k-1)+B*u(k)  
    Mat statePost;          //!< 糾正狀态 (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  
    Mat transitionMatrix;   //!< 狀态轉換矩陣 (F)  
    Mat controlMatrix;      //!< 控制矩陣(B) (not used if there is no control)  
    Mat measurementMatrix;  //!< 觀測矩陣(H)  
    Mat processNoiseCov;    //!< 處理過程噪聲協方差矩陣 (Q)  
    Mat measurementNoiseCov;//!< 觀測噪聲協方差矩陣 (R)  
    Mat errorCovPre;        //!< 先驗誤差估計協方差矩陣 (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  
    Mat gain;               //!< 卡爾曼增益矩陣 (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  
    Mat errorCovPost;       //!< 後驗誤差估計協方差矩陣 (P(k)): P(k)=(I-K(k)*H)*P'(k)  
      
    // temporary matrices  
    Mat temp1;  
    Mat temp2;  
    Mat temp3;  
    Mat temp4;  
    Mat temp5;  
};
}
           

• kalaman類的實作

namespace cv
{
KalmanFilter::KalmanFilter() {}
    //dynamparams:狀态次元
    //measureparamsint：觀測次元
    //controlParams：控制次元
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{
    init(dynamParams, measureParams, controlParams, type);
}
 
void KalmanFilter::init(int DP, int MP, int CP, int type)
{
    CV_Assert( DP > 0 && MP > 0 );
    CV_Assert( type == CV_32F || type == CV_64F );
    CP = std::max(CP, 0);
 
    statePre = Mat::zeros(DP, 1, type);//狀态向量次元DP X 1
    statePost = Mat::zeros(DP, 1, type);
    transitionMatrix = Mat::eye(DP, DP, type);// 狀态轉移矩陣F的大小為DP X DP
 
    processNoiseCov = Mat::eye(DP, DP, type);// 處理過程噪聲協方差矩陣Q的大小為DP X DP
    measurementMatrix = Mat::zeros(MP, DP, type);//觀測矩陣H的大小為MP X DP
    measurementNoiseCov = Mat::eye(MP, MP, type);// 觀測噪聲協方差矩陣R大小為MP X MP
 
    errorCovPre = Mat::zeros(DP, DP, type); // 先驗誤差估計協方差矩陣P'大小為DP X DP
    errorCovPost = Mat::zeros(DP, DP, type);// 後驗誤差估計協方差矩陣P大小為DP X DP
    gain = Mat::zeros(DP, MP, type); // 卡爾曼增益矩陣
 
    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type);// 控制矩陣B
    else
        controlMatrix.release();
 
    temp1.create(DP, DP, type);
    temp2.create(MP, DP, type);
    temp3.create(MP, MP, type);
    temp4.create(MP, DP, type);
    temp5.create(MP, 1, type);
}
 
const Mat& KalmanFilter::predict(const Mat& control)
{
    // 更新預測狀态 e: x'(k) = F*x(k)
    statePre = transitionMatrix*statePost;
    //判斷是否添加控制，是否添加控制矩陣影響更新的預測狀态
    if( !control.empty() )
        // x'(k) = x'(k) + B*u(k)
        statePre += controlMatrix*control;
 
    // temp1 = F*P(k)
    temp1 = transitionMatrix*errorCovPost;
 
    // 更新先驗誤差估計協方差矩陣P'(k) = temp1*F_T + Q
	// gemm(a,b,n,c,m,ans,flags): ans = n*a*b+m*c
    // flags=GEMM_2_T:b transposes
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
 
    // handle the case when there will be measurement before the next predict.
    //a.copyTo(b):得到與a一樣的b，兩者可進行互不相關的操作。
    statePre.copyTo(statePost);
    errorCovPre.copyTo(errorCovPost);
 
    return statePre;
}
 
const Mat& KalmanFilter::correct(const Mat& measurement)
{
    // K(k) = p'(k)*H_T*inv(H*p'(k)*H_T+R)
    // temp2 = F*P'(k)
    temp2 = measurementMatrix * errorCovPre;
 
    // temp3 = temp2*H_T + R
    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
    //
    // solve(MA,MB,MX,CV_LU) 可以求AX=B for X 等價于X=A-1 *B
    // temp4 = inv(temp3)*temp2 = K(k)_T
	solve(temp3, temp2, temp4, DECOMP_SVD);
 
    // K(k)
	// 轉置過來得到真正的K
    gain = temp4.t();
 
    // temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;
 
    // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;
 
    // P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;
 
    return statePost;
}
}
           

• demo.cpp

#include "opencv2/video/tracking.hpp"
#include <iostream>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;
using namespace std;
void on_mouse(int e, int x, int y, int d, void *ptr)
{
	Point*p = (Point*)ptr;
	p->x = x;
	p->y = y;
}

int main()
{
	// 初始化
	int stateSize = 4;  // [x, y, v_x, v_y]
	int measSize = 2;   // [z_x, z_y] 
	int contrSize = 0;  // no control input
	unsigned int F_type = CV_32F;//or CV_64F

	KalmanFilter KF(stateSize, measSize, contrSize, F_type);
	Mat state(stateSize, 1, F_type);  // [x, y, v_x, v_y] 
	Mat meas(measSize, 1, F_type);    // [z_x, z_y] 
	setIdentity(KF.transitionMatrix);
	setIdentity(KF.measurementMatrix);
	setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
	setIdentity(KF.errorCovPost, Scalar::all(.1));


	Mat display_image(600, 800, CV_8UC3);
	namedWindow("Mouse Kalman");

	char ch = 0;
	double ticks = 0;
	Point mousePos;
	vector<Point> mousev, kalmanv;

	while (ch != 'q' && ch != 'Q')
	{
		//預測
		state = KF.predict(); 

		Point predictPt(state.at<float>(0), state.at<float>(1));
	
		// 滑鼠觀測
		setMouseCallback("Mouse Kalman", on_mouse, &mousePos);
		meas.at<float>(0) = mousePos.x;
		meas.at<float>(1) = mousePos.y;

		// >更新
		Mat estimated = KF.correct(meas);

		Point statePt(estimated.at<float>(0), estimated.at<float>(1));
		Point measPt(meas.at<float>(0), meas.at<float>(1));
		char mousep[20];
                sprintf(mousep, "(%0.2f,%0.2f)", meas.at<float>(0), meas.at<float>(1));
                putText(display_image, "mouse point:",Point(0,10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );
                putText(display_image, mousep, Point(120,10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false);
		char prept[20];
                sprintf(prept, "(%0.2f,%0.2f)", state.at<float>(0), state.at<float>(1));
                putText(display_image, "pre point:",Point(0,50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );
                putText(display_image, prept,Point(120,50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );
 

		imshow("Mouse Kalman", display_image);
		display_image = Scalar::all(0);
		//push_back 将新元素移動拷貝到vector裡
		mousev.push_back(measPt);
		kalmanv.push_back(statePt);

		//以點坐标畫園
		circle(display_image,statePt,5,(255,0,0),1);
		circle(display_image,measPt,5,(0,0,255),1);

		//畫出運動軌迹
		for (int i = 0; i < mousev.size() - 1; i++)
			line(display_image, mousev[i], mousev[i + 1], Scalar(0, 0, 255), 1);

		for (int i = 0; i < kalmanv.size() - 1; i++)
			line(display_image, kalmanv[i], kalmanv[i + 1], Scalar(255, 0, 0), 1);


		ch = cv::waitKey(10);
	}
}