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