卡尔曼滤波之观测值的似然估计

    在工程活动中运用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));