kalman跟踪的实现

女爷i 2022-08-19 15:06 19阅读 0赞

卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。

其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做  
出满足最小均方误差的估计。

卡尔曼滤波器包括两个主要过程：预估与校正。

预估过程

主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；

校正过程

负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。

这样的一个过程，我们称之为预估－校正过程，对应的这种估计算法称为预估－校正算法。

卡尔曼滤波器的递归过程：  
  
1)   估计时刻k 的状态：  
          X(k) = A\*X(k-1) + B\*u(k)  
     这里,   u(k) 是系统输入  
  
2)   计算误差相关矩阵P, 度量估计值的精确程度：  
          P(k) = A\*P(k-1)\*A’+ Q  
      这里,   Q = E\{ Wj^2 \} 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简化，当作一个常数矩阵。  
  
3)   计算卡尔曼增益, 以下略去 (k),  即 P = P(k), X = X(k)：  
             K = P \*C’ \* (C \* P \* C’ + R) \-1   
       这里 R = E\{ Vj^2 \}, 是测量噪声的协方差(阵),  即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:  
             K = P \*C’ / (C \* P \* C’ + R)   
  
4)     状态变量反馈的误差量：  
             e = Z(k) – C\*X(k)  
         这里的 Z(k) 是带噪声的测量量  
  
5)    更新误差相关矩阵P  
          P = P – K \* C \* P  
  
6)   更新状态变量：  
            X =X + K\*e = X + K\* (Z(k) – C\*X(k))  
  
7)   最后的输出：  
            Y = C\*X

基本思想

利用观测数据对状态变量的预测估计进行修正，以得到状态变量的最优估计，即

最优估计＝预测估计＋修正

（1）算法是递推的，时域内设计滤波器，适用于多维随机过程的估计；   
（2）用递推法计算，不需要知道全部过去的值。用状态方程描述状态变量的动态变化规律，因此，信号可以是平稳的，也可以是非平稳的；

（3）误差准则仍为均方误差最小准则。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：  
X(k)=A X(k-1)+B U(k)+W(k)  
再加上系统的测量值：  
Z(k)=H X(k)+V(k)  
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。  
  
  
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出。  
  
  
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。  
  
  
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表covariance：

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg为卡尔曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)  
  
  
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)  
P(k|k)是后验估计误差协方差矩阵，度量估计值的精确程度

其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。  
  
式子1，2，3，4和5就是卡尔曼滤波器的5个基本公式。

kalman滤波器工作原理用图表示如下

![20151012220829654][]

kalman滤波器用于跟踪时用之前观测到的值预测下一步的状态，然后根据观测值对自己作出修正，如此不断迭代，以实现跟踪。

你可能会想，kalman滤波器到底有什么用呢，既然我每次都能观测到测量的状态值，比如速度，那我还要kalman滤波器来预测速度干嘛，而且它得到的的预测值还不一定准？我的理解是你之所以能够观测到状态值，完全是因为kalman滤波器的功劳。假设一个装有kalman滤波器的追踪器（tracker）在跟踪一个目标（target），tracker不知道target下一时刻的速度（包括大小和方向），tracker有的只是前一时刻它观测到的target的速度，现在tracker要决定下一时刻自己要往哪个方向运动，运动多快，一旦决策失误，tracker就会丢失target。那该怎么办呢？这就是kalman滤波器发挥作用的时候了，kalman滤波器根据之前的观测值告诉tracker：往12点钟方向走，速度80迈（当然也可能在三维空间中）。于是tracker按照kalman滤波器告诉它的信息执行跟踪，发现成功跟上target，于是tracker把此时它观测到的target状态反馈给kalman滤波器，以方便kalman滤波器做下一步决策。如果没有kalman滤波器，tracker胡乱走一个方向，很可能会跟踪失败。如果聪明一点的话，tracker按照之前它观测到的target的速度来前进，假设这一刻target突然转向，如果没有kalman滤波器，tracker就会离target越来越远，最终跟丢。有kalman滤波器的话，kalman滤波器会告诉tracker说，嘿，老兄，我们该转方向了。你可能会问target转向特别迅速怎么办？那就提高kalman滤波器预测和校正的频率。这样一想，其实kalman滤波器的思想很朴素。

模拟电路中稳压电源也是这种思想。

以上是我个人的理解，如有错误请大家指出。

#ifndef _KALMANFILTER_
    #define _KALMANFILTER_
    class CKalman
    {
    public:
    	CKalman(int D, int M, int C);
    	~CKalman();
    	void KalmanPredict(double * control);
    	void KalmanCorrect(double * measurement);
    	void set_transition_matrix(const double*transition_matrix);
    public:
    	int MP;  //number of measure vector dimensions 
    	int DP;  //number of state   vector dimensions 
    	int CP;  //number of control vector dimensions 
    
    	double* state_pre;            //[DP * 1]  
    	double* state_post;           //[DP * 1]  
    	double* transition_matrix;    //[DP * DP]  
    	double* control_matrix;       //[DP * CP] if (CP > 0)  
    	double* measurement_matrix;   //[MP * DP] 
    	double* process_noise_cov;    //[DP * DP] 
    	double* measurement_noise_cov;//[MP * MP] 
    	double* error_cov_pre;        //[DP * DP] 
    	double* error_cov_post;       //[DP * DP] 
    	double* gain;                 //[DP * MP] 
    };
    
    #endif

#include "stdafx.h"
    #include "kalmanfilter.h"  
    #include "Matrix.h"   
    #include<memory>
    #include<assert.h>
    using namespace std;
    
    
    CKalman::CKalman(int D, int M, int C){
    	if (D <= 0 || M <= 0){
    		
    		assert(false);// "state and measurement vectors must have positive number of dimensions! ");
    	}
    	if (C < 0)
    	{
    		assert(false);// "No Control!");
    	}
    	DP = D;
    	MP = M;
    	CP = C;
    	state_pre = new double[DP * 1];
    	memset(state_pre, 0, sizeof(double)*DP);
    	state_post = new double[DP * 1];
    	memset(state_post, 0, sizeof(*state_post));
    	transition_matrix = new double[DP * DP];
    	memset(transition_matrix, 0, sizeof(double)*DP*DP);
    	process_noise_cov = new double[DP * DP];
    	memset(process_noise_cov, 0, sizeof(double)*DP*DP);
    	measurement_matrix = new double[MP * DP];
    	memset(measurement_matrix, 0, sizeof(double)*MP*DP);
    	measurement_noise_cov = new double[MP * MP];
    	memset(measurement_noise_cov, 0, sizeof(double)*MP*MP);
    	error_cov_pre = new double[DP * DP];
    	memset(error_cov_pre, 0, sizeof(double)*DP*DP);
    	error_cov_post = new double[DP * DP];
    	memset(error_cov_post, 0, sizeof(double)*DP*DP);
    	gain = new double[DP * MP];
    	memset(gain, 0, sizeof(double)*DP*MP);
    	if (CP > 0)
    	{
    		control_matrix = new double[DP * CP];
    		memset(control_matrix, 0, sizeof(double)*DP*CP);
    	}
    }
    
    void CKalman::set_transition_matrix(const double*trans_matrix)
    {
    	for (int i = 0; i < DP*DP; i++)
    	{
    		transition_matrix[i] = trans_matrix[i];
    	}
    }
    
    CKalman::~CKalman(){
    	delete[] state_pre;
    	delete[] state_post;
    	delete[] transition_matrix;
    	if (CP >0){
    		delete[] control_matrix;
    	}
    	delete[] measurement_matrix;
    	delete[] process_noise_cov;
    	delete[] measurement_noise_cov;
    	delete[] error_cov_pre;
    	delete[] gain;
    	delete[] error_cov_post;
    }
    
    void CKalman::KalmanPredict(double * control){
    
    	CMatrix matrix;
    	/* update the state */
    	/* x'(k) = A*x(k) */
    	matrix.Mul(transition_matrix, DP, DP, state_post, DP, 1, state_pre);
    
    	if (control && CP > 0){
    		/* x'(k) = x'(k) + B*u(k) */
    		double * temp1 = new double[DP * 1];
    		matrix.Mul(control_matrix, DP, CP, control, CP, 1, temp1);
    		matrix.Add(state_pre, temp1, state_pre, DP, 1);
    		delete[] temp1;
    		temp1 = NULL;
    	}
    
    	/* update error covariance matrices */
    	/* temp2 = A*P(k) */
    	double * temp2 = new double[DP * DP];
    	matrix.Mul(transition_matrix, DP, DP, error_cov_post, DP, DP, temp2);
    
    	/* P'(k) = temp2*At + Q */
    	double * temp3 = new double[DP * DP];
    	double * temp4 = new double[DP * DP];
    	matrix.Transpose(transition_matrix, DP, DP, temp3);
    	matrix.Mul(temp2, DP, DP, temp3, DP, DP, temp4);
    	matrix.Add(temp4, process_noise_cov, error_cov_pre, DP, DP);
    	delete[] temp2;  temp2 = NULL;
    	delete[] temp3;  temp3 = NULL;
    	delete[] temp4;  temp4 = NULL;
    }
    
    void CKalman::KalmanCorrect(double * measurement){
    
    	if (!measurement){
    		assert(false);// , "Measurement is Null!!!");
    	}
    	CMatrix matrix;
    
    	/* temp1 = Ht*/
    	double * temp1 = new double[DP * MP];
    	matrix.Transpose(measurement_matrix, MP, DP, temp1);
    	/* temp2 = P'(k) * temp1 */
    	double * temp2 = new double[DP * MP];
    	matrix.Mul(error_cov_pre, DP, DP, temp1, DP, MP, temp2);
    
    	/* temp3 = H*temp2 + R */
    	double * temp3 = new double[MP * MP];
    	matrix.Mul(measurement_matrix, MP, DP, temp2, DP, MP, temp3);
    	matrix.Add(temp3, measurement_noise_cov, temp3, MP, MP);
    
    	/* temp4 = inv(temp3) */
    	double * temp4 = new double[MP * MP];
    	matrix.ContraryMatrix(temp3, temp4, MP);
    
    	/* K(k) = temp2 * temp4  */
    	matrix.Mul(temp2, DP, MP, temp4, MP, MP, gain);
    	delete[] temp1;  temp1 = NULL;
    	delete[] temp2;  temp2 = NULL;
    	delete[] temp3;  temp3 = NULL;
    	delete[] temp4;  temp4 = NULL;
    
    	//update state_post   
    	/* temp5 = z(k) - H*x'(k) */
    	double * temp5 = new double[MP * 1];
    	matrix.Mul(measurement_matrix, MP, DP, state_pre, DP, 1, temp5);
    	matrix.Sub(measurement, temp5, temp5, MP, 1);
    	/* x(k) = x'(k) + K(k)*temp5 */
    	double * temp6 = new double[DP * 1];
    	matrix.Mul(gain, DP, MP, temp5, MP, 1, temp6);
    	matrix.Add(state_pre, temp6, state_post, DP, 1);
    	delete[] temp5;  temp5 = NULL;
    	delete[] temp6;  temp6 = NULL;
    
    	//update error_cov_post   
    	/* P(k) = P'(k) - K(k)*H* P'(k) */
    	double * temp7 = new double[MP * DP];
    	double * temp8 = new double[DP * DP];
    	matrix.Mul(measurement_matrix, MP, DP, error_cov_pre, DP, DP, temp7);
    	matrix.Mul(gain, DP, MP, temp7, MP, DP, temp8);
    	matrix.Sub(error_cov_pre, temp8, error_cov_post, DP, DP);
    	delete[] temp7;  temp7 = NULL;
    	delete[] temp8;  temp8 = NULL;
    }

调用

// kalman-filter.cpp : 定义控制台应用程序的入口点。
    //
    
    #include "stdafx.h"
    #include"kalmanfilter.h"
    #include<time.h>
    #include<cstdlib>
    #include <cv.h>  
    #include <cxcore.h>  
    #include <highgui.h> 
    
    using namespace std;
    
    
    const int winHeight = 600;
    const int winWidth = 800;
    
    
    
    CvPoint mousePosition = cvPoint(winWidth >> 1, winHeight >> 1);
    
    //mouse event callback  
    void mouseEvent(int event, int x, int y, int flags, void *param)
    {
    	if (event == CV_EVENT_MOUSEMOVE) {
    		mousePosition = cvPoint(x, y);
    	}
    }
    
    
    int _tmain(int argc, _TCHAR* argv[])
    {
    	time_t t;
    	srand(time(&t));
    
    	//1.kalman filter setup  
    	const int stateNum = 4;
    	const int measureNum = 2;
    	CKalman kf(stateNum, measureNum, 0);//state(x,y,detaX,detaY) 
    	double A[] = {//transition matrix  
    		1, 0, 1, 0,
    		0, 1, 0, 1,
    		0, 0, 1, 0,
    		0, 0, 0, 1
    	};
    	kf.set_transition_matrix(A);
    	
    	kf.measurement_matrix[0]  = 1;//for x,x is an observation
    	kf.measurement_matrix[5] =  1;//for y,y is an observation
    
    	kf.measurement_noise_cov[0] = 1e1;
    	kf.measurement_noise_cov[3] = 1e1;
    
    	kf.process_noise_cov[0] = 1e-1;//diag
    	kf.process_noise_cov[5] = 1e-1;
    	kf.process_noise_cov[10] = 1e-1;
    	kf.process_noise_cov[15] = 1e-1;
    	
    	int nn = winHeight > winWidth ? winWidth : winHeight;
    	kf.state_post[0] = double(rand()) / RAND_MAX*nn;//x
    	kf.state_post[1] = double(rand()) / RAND_MAX*nn;//y    
    	kf.state_post[2]=rand();//deltaX
    	kf.state_post[3]=rand();//deltaY
    	
    	kf.error_cov_post[0]  = 1;//diag
    	kf.error_cov_post[5] = 1;
    	kf.error_cov_post[10] = 1;
    	kf.error_cov_post[15] = 1;
    
    
    	CvFont font;
    	cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, 1);
    
    	cvNamedWindow("kalman");
    	cvSetMouseCallback("kalman", mouseEvent);
    	IplImage* img = cvCreateImage(cvSize(winWidth, winHeight), 8, 3);
    
    
    	while (1){
    		//2.kalman prediction  
    		kf.KalmanPredict(NULL);
    		CvPoint predict_pt = cvPoint((int)kf.state_pre[0], (int)kf.state_pre[1]);
    
    		//3.update measurement  
    		
    		double measure[2];
    		measure[0] = double(mousePosition.x);
    		measure[1] = double(mousePosition.y);
    		//4.update  
    		kf.KalmanCorrect(measure);
    		
    
    
    
    		//draw   
    		cvSet(img, cvScalar(255, 255, 255, 0));
    		cvCircle(img, predict_pt, 5, CV_RGB(0, 255, 0), 3);//predicted point with green  
    		cvCircle(img, mousePosition, 5, CV_RGB(255, 0, 0), 3);//current position with red  
    		char buf[256];
    		sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y);
    		cvPutText(img, buf, cvPoint(10, 30), &font, CV_RGB(0, 0, 0));
    		sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y);
    		cvPutText(img, buf, cvPoint(10, 60), &font, CV_RGB(0, 0, 0));
    
    		cvShowImage("kalman", img);
    		int key = cvWaitKey(3);
    		if (key == 27){//esc     
    			break;
    		}
    	}
    
    	cvReleaseImage(&img);
    	
    	return 0;
    
    }

为什么卡尔曼滤波只能用于解决线性高斯系统

[Kalman Filter(卡尔曼滤波)的推导][Kalman Filter]

# 中，介绍了卡尔曼滤波几个公式的证明，后验概率密度要是高斯型的这一假设用在了这个计算结果的证明过程里了。同时说明了卡尔曼滤波的实际目的： #

#   1）预测结果是真值的无偏估计 #

# 2）各个变量的方差最小 #

在这篇证明中，作者提到了[ http://www.aticourses.com/kalman\_filter.pdf][http_www.aticourses.com_kalman_filter.pdf]  这篇推导，并指出证明的内容主要来源于此。具体内容请读者自己翻阅。

[今天小结一下卡尔曼滤波][Link 1]

[卡尔曼滤波 -- 从推导到应用][--]

[Kalman滤波器从原理到实现][Kalman]

[对Kalman(卡尔曼)滤波器的理解][Kalman 1]

[20151012220829654]: /images/20220731/4670b145382944c4a32ad9d790cd3e96.png
[Kalman Filter]: http://blog.csdn.net/tudouniurou/article/details/6277520
[http_www.aticourses.com_kalman_filter.pdf]: http://www.aticourses.com/kalman_filter.pdf
[Link 1]: http://blog.csdn.net/tudouniurou/article/details/6277512
[--]: http://blog.csdn.net/heyijia0327/article/details/17487467
[Kalman]: http://blog.csdn.net/xiahouzuoxin/article/details/39582483
[Kalman 1]: http://blog.csdn.net/lanbing510/article/details/40936343