卡尔曼滤波原理及简单程序（《学习opencv-中文版》）

1、kalman滤波器：最优化自回归数据处理算法。

自回归模型：根据前一次的表现，来预测接下来的情况，他们存在一种线性关系。

2、Kalman滤波器的三个重要假设：a.被建模的系统是线性关系。b.影响测量的噪声属于白噪声（噪声与时间无关）。c.噪声的本质是高斯分布（即正态分布）。

a假设的意思是k时刻的系统状态（state）可以用某个矩阵（转换矩阵F）与k-1时刻的系统状态的乘积表示。b.c假设说明噪声是高斯白噪声，即噪声与时间无关，且只用均值和协方差就可以为幅值建模。

高斯白噪声中的高斯是指概率分布是正态函数，而白噪声是指它的二阶矩不相关，一阶矩为常数，是指先后信号在时间上不相关性。

那么，一阶矩和二阶矩分别是指什么呢？

一阶矩：均值，    二阶矩：方差，    三阶矩：偏度，     四阶矩：峰度

3、kalman的5个公式：

（1）x_k  = F  x_k-1   +  B U_k   +   w_k

（2）P_k(-)  =  F  P_k-1   F(T) + Q_k-1 // 获得误差协方差

（3）K_k = P_k(-)  H_k(-)  ( H_k  P_k(-)   H_k(T)  +  R_k)(-1)

（4）x_k =   x_k(-)  + K_k  (   z_k(-)  - H_k  x_k(-)  )

（5）P_k = (  I -  K_k  H_k ) P_k(-)

z_k = H_k  x_k + v_k

x_k是一个现在状态元素的n维向量，即实际量（state vector dimensions）

F是状态转换矩阵（state transition matrix），对应x_k的维度，为一个n*n的矩阵

U_k的作用是允许外部控制施加于系统，由表示输入控制的c维向量组成

B是一个联系输入控制和状态改变的n*c矩阵。

w_k为过程噪声（process noise）,w_k的元素具有高斯分布（正态分布）N（0, 方差）， 那么，协方差矩阵Q（process_noise_cov）为n*n矩阵

z_k为测量值，m维向量(measurement vectore dismensions),是x_k加上了测量噪声

H_k是m*n的矩阵，为测量系统的参数

v_k是测量噪声，具有高斯分布（正态分布）N（0, 方差）， 那么，测量噪声协方差矩阵R（measurement_noise_cov）为m*m矩阵

P_k是误差协方差

K_k 为卡尔曼增益

x_k(-) 为预测值

x_k =   x_k(-)  + K_k  (   z_k(-)  - H_k  x_k(-)  )该公式说明了实际值、预测的值、测量值三者之间的关系

#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
#include "ml.h"


#define phi2xy(mat) cvPoint(cvRound(img->width/2 + img->width/3 * cos(mat->data.fl[0])),cvRound(img->height / 2 - img->width/3 * sin(mat->data.fl[0])))




int main(void)
{
cvNamedWindow("Kalman", 1);


//  创建一个随机数产生器
CvRandState rng;
cvRandInit(&rng, 0, 1, -1, CV_RAND_UNI);


// 一幅用于绘制的图像
IplImage* img = cvCreateImage(cvSize(500, 500), 8, 3);


// 创建卡尔曼滤波器结构
CvKalman* kalman = cvCreateKalman(2, 1, 0); // 2(速度、角速度)， 1(只能测量车辆的位置), 0(无控制量)


// 用随机数初始化状态
CvMat* x_k = cvCreateMat(2, 1, CV_32FC1); // 状态x_k
cvRandSetRange(&rng, 0, 0.1, 0);
rng.disttype = CV_RAND_NORMAL;
cvRand(&rng, x_k);


// 过程噪声
CvMat* w_k = cvCreateMat(2, 1, CV_32FC1);


// 测量量，只有一个：位置
CvMat* z_k = cvCreateMat(1, 1, CV_32FC1);
cvZero(z_k);


// 转换矩阵，描述模型的k和k+1步的参数
const float F[4] = {1, 1, 0, 1}; // 2*2矩阵
memcpy(kalman->transition_matrix->data.fl, F, sizeof(F));




// 初始化卡尔曼滤波的其他参数
cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1)); // 测量矩阵H
cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5)); // 过程噪声
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1)); // 测量噪声
cvSetIdentity(kalman->error_cov_post, cvRealScalar(1)); // 后验误差协方差


// 选择随机数初始状态
cvRand(&rng, kalman->state_post);


while(1)
{
// 预测点的位置
const CvMat* y_k = cvKalmanPredict(kalman, 0); // 控制量为0

// 产生测量值
cvRandSetRange(&rng, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0);
cvRand(&rng, z_k);
cvMatMulAdd(kalman->measurement_matrix, x_k, z_k, z_k); // 广义矩阵的乘法


// 画点
cvZero(img);
cvCircle(img, phi2xy(z_k), 4, CV_RGB(255, 255, 0)); // 测量值
cvCircle(img, phi2xy(y_k), 4, CV_RGB(255, 255, 255)); // 预测值
cvCircle(img, phi2xy(x_k), 4, CV_RGB(255, 0, 0)); // 实际值


// 校正卡尔曼滤波的状态
cvKalmanCorrect(kalman, z_k);




// 产生过程噪声
cvRandSetRange(&rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0);
cvRand(&rng, w_k);
cvMatMulAdd(kalman->transition_matrix, x_k, w_k, x_k);

cvShowImage("Kalman", img);
// 按"Esc"键退出程序
if (cvWaitKey(100) == 27)
{
break;
}

}
return 0;
}

参考：http://www.cnblogs.com/xmphoenix/p/3634536.html