2

Kalman滤波器数学原理与应用

 2 years ago
source link: https://changkun.de/blog/posts/kalman-filter-principle/
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.

Kalman滤波器数学原理与应用

Published at:

2013-08-29

  |  

Reading: 2059 words ~5min

  |  

PV/UV: 7/6

前段时间做Tracking，了解到Kalman滤波，感觉很不错。wikipedia上内容有点多，整理过后发现只有五个核心公式。

Kalman滤波器不断把协方差递归，估算出最优值，并且保留了上一时刻的协方差（Kalman增益），因此它的运行很快，可以随不同的时刻而改变他的值。 首先，我们要引入一个离散控制过程系统，该系统可以用一个线性随机微分方程来描述：

X(k)=AX(k−1)+BU(k)+W(k)X(k)=AX(k−1)+BU(k)+W(k)

加上系统的测量值：

Z(k)=HX(k)+V(k)Z(k)=HX(k)+V(k)

上面式子中，X(k)X(k)是kk时刻的系统状态，U(k)U(k)是kk时刻对系统的控制量。ABAB是系统参数，对于多模型系统，他们为矩阵。Z(k)Z(k)是kk时刻的测量值，HH是测量系统的参数，对于多测量系统，HH为矩阵。

W(k)W(k)和V(k)V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声他们的协方差分别为QQ，RR（假设不随系统状态变化而变化）。 对于满足上面条件（线性随机微分系统，过程和测量都是高斯白噪声），Kalman滤波器是最优的信息处理器。下面则用其自身和自身的协方差来估计系统的最优化输出。 首先，要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统上一状态而预测出现在状态：

X(k|k−1)=AX(k−1|k−1)+BU(k)X(k|k−1)=AX(k−1|k−1)+BU(k)

上式中，X(k|k−1)X(k|k−1)是利用上一状态预测的结果，X(k−1|k−1)X(k−1|k−1)是上一状态最优的结果，U(k)U(k)为现在状态的控制量，如果没有控制量，它可以为0。 至此，系统结果已经更新，但对于X(k|k−1)X(k|k−1)的协方差还未更新，用CovCov表示协方差则：

Cov(k|k−1)=ACov(k−1|k−1)AT+QCov(k|k−1)=ACov(k−1|k−1)AT+Q

上式中，Cov(k|k−1)Cov(k|k−1)是X(k|k−1)X(k|k−1)对应的协方差，Cov(k−1|k−1)Cov(k−1|k−1)是X(k−1|k−1)X(k−1|k−1)对应的协方差，ATAT表示AA的转置，QQ是系统过程的协方差。上面两个式子为对系统的预测。 当有了现在状态的预测结果，我们再收集现在状态的预测值。结合预测值和测量值，我们可以得到现在状态kk的最优化估计值X(k|k)X(k|k)：

X(k|k)=X(k|k−1)+Kg(k)(Z(k)−HX(k|k−1))X(k|k)=X(k|k−1)+Kg(k)(Z(k)−HX(k|k−1))

其中，KgKg为Kalman增益：

Kg=Cov(k|k−1)HT(HCov(k|k−1)HT+R(k))−1Kg=Cov(k|k−1)HT(HCov(k|k−1)HT+R(k))−1

于此，已经得到了k状态下的最优估算值X(k|k)X(k|k)。但是为了要另Kalman滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)X(k|k)的协方差：

Cov(k|k)=(E−KgH)Cov(k|k−1)Cov(k|k)=(E−KgH)Cov(k|k−1)

其中，EE为单位矩阵。当系统进入k+1k+1状态时，Cov(k|k)Cov(k|k)就是第二个公式的，因此，可以自回归下去。

Kalman滤波器作为一个很好的预测模型，可以根据现有情况来预测未来情况，在做Tracking中则变得很有用处。对于一个对象SS的Tracking，SS的pos在域中漂移，利用Tracking可以使得pos平滑稳定。 在OpenCV中提供了CvKalman结构，但是使用起来仍然需要建立模型，前几天我根据上述原理对整个过程进行了封装：

//
//  CvKalmanPoint.h
//  Renovation
//
//  Version History: 
//		Created by 欧 长坤 on 2013-08-17 
//
//  Description：
//		//2013-08-17
//		创建CvKalmanPoint对象并使用
//		cvKalmanPositionPrediction方法来返回Kalman滤波预测点。
//  
//  Copyright (c) 2013年 欧 长坤. All rights reserved.
//
//
#ifndef CVKALMANPOINT_H__
#define CVKALMANPOINT_H__

#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>

class CvKalmanPoint
{
public:

	// 参数
	int stateNum;
	int measureNum;

	// 屏幕分辨率
	int windowHeight;
	int windowWidth;

	CvKalman* kalman;
	CvMat* process_noise;
	CvMat* measurement;
	CvRNG rng;

	// 鼠标坐标
	CvPoint mousePosition;

	// 初始化
	CvKalmanPoint(void);
	~CvKalmanPoint(void);

	// 预测卡尔曼点
	CvPoint cvKalmanPositionPrediction(void);

};

#endif

下面则是构造函数和cvKalmanPositionPrediction方法的具体实现：

#include "CvKalmanPoint.h"
#include <windows.h>
#include <winuser.h>

CvKalmanPoint::CvKalmanPoint(void)
{

	windowWidth  = GetSystemMetrics(SM_CXSCREEN);
	windowHeight = GetSystemMetrics(SM_CYSCREEN); 

	stateNum  	  = 4;		// 状态数，包括(x,y,dx,dy)坐标及速度（每次移动的距离）
	measureNum	  = 2;		// 实际量，能看到的是坐标值，当然也可以自己计算速度，但是没有必要。 
	kalman	 	  = cvCreateKalman( stateNum, measureNum, 0 );	// state(x,y,detaX,detaY)  
	process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
	measurement	  = cvCreateMat( measureNum, 1, CV_32FC1 );		// measurement(x,y) 
	rng			  = cvRNG(-1);	// 64位长整数种子，初始化rng

	/*
		// 变换矩阵
		┌ x'┐  ┌1 0 1 0┐┌ x┐
		│ y'│= │0 1 0 1││ y│
		│dx'│  │0 0 1 0││dx│
		└dy'┘  └0 0 0 1┘└dy┘
		==> x'  = x + dx
		==> y'  = y + dy
		==> dx' = dx
		==> dy' = dy
	*/
	// 4 == stateNum
	float transformMatrix[4][4] = {  
		1,0,1,0,  
		0,1,0,1,  
		0,0,1,0,  
		0,0,0,1  
	};

	memcpy( 
		kalman->transition_matrix->data.fl, 
		transformMatrix, 
		sizeof(transformMatrix) 
	);

	// 把数组中除了行数与列数相等以外的所有元素的值都设置为0，行数与列数相等的元素的值都设置为1
	cvSetIdentity( 
		kalman->measurement_matrix,
		cvRealScalar(1) 
	);  
	cvSetIdentity(
		kalman->process_noise_cov,
		cvRealScalar(1e-5)
	);  
	cvSetIdentity(
		kalman->measurement_noise_cov,
		cvRealScalar(1e-1)
	);  
	cvSetIdentity(
		kalman->error_cov_post,
		cvRealScalar(1)
	);

	// 初始化后的随机态卡尔曼滤波 
	cvRandArr(
		&rng,
		kalman->state_post,
		CV_RAND_UNI,
		cvRealScalar(0),
		cvRealScalar(windowHeight>windowWidth?windowWidth:windowHeight)
	);
}
CvKalmanPoint::~CvKalmanPoint(void)
{

}

CvPoint CvKalmanPoint::cvKalmanPositionPrediction(void)
{
	// 卡尔曼预测
    const CvMat* prediction = cvKalmanPredict(kalman,0);  
    CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);  

    // 测量校准  
    measurement->data.fl[0] = (float)mousePosition.x;  
    measurement->data.fl[1] = (float)mousePosition.y;  

    // 更新  
    cvKalmanCorrect(kalman, measurement);       

	//printf("预测位置:(%d,%d)\n",predict_pt.x,predict_pt.y);
	//printf("实际位置:(%d,%d)\n",mousePosition.x,mousePosition.y);

	return predict_pt;
}

使用方法：

CvPoint measure;
……
CvKalmanPoint kalman_point;
kalman_point.mousePosition.x = measure.x;
kalman_point.mousePosition.y = measure.y;
measure = kalman_point.cvKalmanPositionPrediction();

report

Recommend

About Joyk


Aggregate valuable and interesting links.
Joyk means Joy of geeK