卡尔曼滤波器原理说明

一、原理
1.通过观测一个运动系统的输入输出，综合考虑运动误差（假定是高斯分布）和观测误差（假定为高斯分布），来预测系统的真实状态：

图1 误差融合的示意图
二、参数说明
卡尔曼滤波器共有6个参数，其中有4个是初始化后即固定不变参数，两个是描述系统状态的参数：

'A:',kalman.transitionMatrix（转化矩阵，将当前时刻的Xk转化为下一时刻的Xk+1，初始化后不变）
'H:',kalman.measurementMatrix（观测矩阵，将系统状态X转化为观测值的矩阵，初始化后不变）
'Q:',kalman.processNoiseCov（过程误差协方差矩阵，表示在系统运行过程时的误差程度，值越高表示误差越大，初始化后不变）
'R:',kalman.measurementNoiseCov（观测误差协方差矩阵，表示观测值的误差程度，值越高表示越不信任观测值，初始化后不变）
'P:',kalman.errorCovPost（状态协方差矩阵，用于描述系统状态中各个变量的相关性，根据系统状态不断更新）
'X:',kalman.statePost（状态矩阵，根据系统状态不断更新）
其中X和P是表示系统状态的参数，每预测和更新时都会更新一次。

三、实例分析
一个小车在直线上行驶，这时我们使用kalmanFilter对它进行预测。
首先，描述小车的状态，我们使用【距离原点的长度Position，小车运行速度velocity】两个变量，其中Position是可以通过测量的时候得到的，velocity则是一个隐藏的变量，这样的话：

其中k代表时间。
第二步：预测小车下一刻的位置和状态，由于知道了小车的位置和速度，所以可以预测下一刻的位置，那么如何做到这个呢，就需要我们的转化矩阵A：

即：

这里的Fk就是参数定义里的A，由上面的定义得知P是X的协方差矩阵，所以由：

得到：

但是系统的运行总是不会是完全精准的，所以，小车的位置可能会偶然向前一些或者向后一些，这里的误差用高斯分布矩阵Q来描述，例如知道小车制动性能很好，那么误差会较低，可以设Q为波峰很高分布很窄的高斯分布。那么，加上Q的话，公式则会变成：

至此，我们的两个需要变化的参数已经更新完成，这里的Xk和Pk就是由k-1时刻通过系统状态推测出来的k时刻的小车的位置速度以及对应的协方差矩阵，其实还有可能会有外力作用，这时需要加上控制变量C，但是为了系统简洁，这里假设没有外力作用。
第三步：更正小车的状态，这时我们知道了预测的k时刻的X和P，但是小车的速度不是一尘不变的，在k时刻我们使用测量仪器进行测量，测量结果为zmeasured，这里的z为小车的测量距离，但是我们预测出来的状态X有两个变量，这里就需要H矩阵来进行转化，我们预测的结果是：

这里的Zexcepted是预测出来的，预测时的系统误差高斯分布矩阵Q，预测的可能位置分布如下图中的青色线，同时我们测量结果为Zmeasured，测量结果是使用测量仪器测量出来的，也有一定的误差，这里的误差使用高斯分布矩阵R来表示，测量仪器一般较为精准，所以测量误差的高斯分布较为集中，波峰高范围小，这里的实际测量位置分布如下图的粉红色线。将这两个分布进行相乘便得到我们更正后的小车运行系统的状态了，也就是波峰最小，范围最小的绿色线，这个绿色线的分布就是所谓的卡尔曼增益K：

这个过程使用公式表示如下：

其中代表高斯分布过程误差协方差矩阵与高斯分布测量误差协方差矩阵与状态协方差矩阵，代表矫正后系统状态，代表预测时系统状态，为测量出的系统状态。
至此系统状态和协方差矩阵两个参数均已矫正更新。

四、代码详解

void KalmanFilter::init(int DP, int MP, int CP, int type)
{
	statePre = Mat::zeros(DP, 1, type);   //预测状态
	statePost = Mat::zeros(DP, 1, type);   //当前状态X
	transitionMatrix = Mat::eye(DP, DP, type);   //转化矩阵A
	processNoiseCov = Mat::eye(DP, DP, type);   //过程误差矩阵Q
	measurementMatrix = Mat::zeros(MP, DP, type);  //观测转化矩阵H
	measurementNoiseCov = Mat::eye(MP, MP, type);  //观测误差协方差矩阵R
	errorCovPre = Mat::zeros(DP, DP, type);   //预测状态协方差矩阵
	errorCovPost = Mat::zeros(DP, DP, type);  //当前状态协方差矩阵P
	gain = Mat::zeros(DP, MP, type);  //卡尔曼增益K，计算时自动生成
	if (CP > 0)
	controlMatrix = Mat::zeros(DP, CP, type);  //控制变量矩阵，暂用不到
	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)
{
	// update the state: x'(k) = A*x(k)
	statePre = transitionMatrix * statePost;
	
	if (!control.empty())
	// x'(k) = x'(k) + B*u(k)
	statePre += controlMatrix * control;
	
	// update error covariance matrices: temp1 = A*P(k)
	temp1 = transitionMatrix * errorCovPost;
	
	// P'(k) = temp1*At + Q
	gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
	
	// handle the case when there will be measurement before the next predict.
	statePre.copyTo(statePost);
	errorCovPre.copyTo(errorCovPost);
	
	return statePre;
}

const Mat &KalmanFilter::correct(const Mat &measurement)
{
	// temp2 = H*P'(k)
	temp2 = measurementMatrix * errorCovPre;
	// temp3 = temp2*Ht + R
	gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
	// temp4 = inv(temp3)*temp2 = Kt(k)
	solve(temp3, temp2, temp4, DECOMP_SVD);
	// 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;
}