技術標籤：基本演算法視覺里程計計算機視覺卡爾曼濾波演算法矩陣演算法

在工程活動中運用KF或者EKF，常常需要判斷當前觀測值的可靠程度。若用一個不太可靠的觀測值更新模型，往往會產生不太好的結果。

那麼，有沒有一種方式，用來評估觀測值對於模型是否可靠呢？答案：有。

以卡爾曼濾波為例。首先回顧一下卡爾曼濾波的5個方程。如下圖所示。

給定觀察值Z。似然估計值的計算方式如下：

其中，

,如下圖中綠框所示。

. 如下圖紅框標記所示。

C++程式碼：用opencv實現，變數Sk（上圖紅框的值）、i_k（上圖綠框的值）、Sk_inv(為Sk的逆矩陣）均為矩陣。

	float sigma = sqrt(cv::determinant(Sk));
	float likehood_prod = cv::exp(-0.5*cv::trace(i_k.t()*Sk_inv*i_k).val[0]) / (sigma*cv::pow(CV_PI + CV_PI, i_k.rows*0.5));

 

當進行KF模型選擇時，若各個模型觀測量的維度不同，但是是來著於同一組資料。在工程上可以考慮將程式碼中的i_k.rows去掉，以此驗證模型是否work。

	float sigma = sqrt(cv::determinant(Sk));
	float likehood_prod = cv::exp(-0.5*cv::trace(i_k.t()*Sk_inv*i_k).val[0]) / (sigma*cv::pow(CV_PI + CV_PI, 0.5));