Kalman Filter

关于CAR(Context-Aware Routing)协议中预测过程涉及到的Kalman Filter，转载待看：

 

转自http://internetbuff.blog.163.com/blog/static/9425110720091501413932/

 

　　Kalman Filter是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估 计动态系统的状态。广泛应用于包含Radar、计算机视觉在内的等工程应用领域，在控制理论和控制系统工程中也是一个非常重要的课题。连同线性均方规划， 卡尔曼滤波器可以用于解决LQG(Linear-quadratic-Gaussian control)问题。卡尔曼滤波器，线性均方归化及线性均方高斯控制器，是大部分控制领域基础难题的主要解决途径。

目录

■    1     应用实例

■    2    命名和发展历史

■    3    基本动态系统模型

■    4    卡尔曼滤波器

            4.1    预测

            4.2    更新

            4.3    不变量

■    5    实例

■    6    推导

           6.1    后验估计协方差矩阵推导

           6.2    Kalman 增益推导

           6.3    后验误差协方差矩阵简化

■    7    信息滤波

■    8  非线性滤波器

           8.1   扩展Kalman 滤波

           8.2   Unscented Kalman filter

■    9   Kalman-Bucy滤波

■    10  应用

■    11  参见

■    12  参考文献

■    13  外部链接

■    1    应用实例

一个简单的应用是估计物体的位置和速度；简要描述如下：假设我们可以获取一个物体的包含噪声的一系列位置观测数据，我们可以获得此物体的精确速度和位置连续更新信息。

例如，对于雷达来说，我们关心的是跟踪目标，而目标的位置，速度，加速度的测量值是时刻含有误差的，卡尔曼滤波器利用目标的动态信息，去掉噪声影响，获取目标此刻好的位置估计(滤波)，将来位置估计(预测)，也可以是过去位置估计的(插值或平滑)

■    2    命名和发展历史

这个滤波器以它的发明者Rudolf.E.Kalman 而命名，但是在Kanlman之前，Thorvald Nicolai Thiele和Peter Swerling 已经提出了类似的算法。Stanley Schmidt 首次实现了Kalman滤波器。在一次对NASA Ames Research Center访问中，卡尔曼发现他的方法对于解决阿波罗计划的轨迹预测很有用，后来阿波罗飞船导航 电脑就使用了这种滤波器。这个滤波器可以追溯到 Swerling(1958),Kalman(1960),Kalman和Bucy(1961)发表的论文。

这个滤波器有时叫做Stratonovich-Kalman-Bucy滤波器。因为更为一般的非线性滤波器最初由Ruslan L.Stratonovich发明，而Stratonovich-Kalman-Bucy滤波器只是非线性滤波器的一个特例。事实上，1960年夏 季，Kalman和Stratonovich在一个Moscow召开的会议中相遇，而作为非线性特例的线性滤波方程，早已经由Stratonovich在 此以前发表了。

 在控制领域，Kalman滤波被称为线性二次型估计，目前,卡尔曼滤波已经有很多不同的实现，有施密特扩展滤波器、信息滤波器以及一系列的 Bierman和Thornton 发明的平方根滤波器等，而卡尔曼最初提出的形式现在称为简单卡尔曼滤波器。也许最常见的卡尔曼滤波器应用是锁相环，它在收音机、计算机和几乎全部视频或通 讯设备中广泛存在。

 ■   3    基本动态系统模型

Kalman滤波基于时域描述的线性动态系统，它的模型是Markov Chain，而Markov Chain建立在一个被高斯噪声干扰的线性算子之上。系统的状态可以用一个元素为实数的向量表示。 随着离散时间的增加，这个线性算子就会作用到当前状态之上，产生一个新的状态，并且会带入一定的噪声，同时一些已知的控制信息也会加入。同时另外一个受噪 声干扰的线性算子将产生这些隐含状态的可见输出。Kalman 滤波可以被看作为类似隐马尔科夫模型，它们的显著不同点在于：隐状态变量的取值空间是一个连续的空间，而离散状态空间则不是；另为，隐马尔科夫模型可以描 述下一个状态的一个任意分布，这也与应用于Kalman滤波器中的高斯噪声模型相反。Kalman滤波器方程和隐马尔科夫方程之间有很大的二重性，关于 Kalman 滤波方程和隐马尔科夫方程之间二重性参看Roweis and Ghahramani(1999)[4]。              

 为了从一系列的噪声观测中，应用Kalman滤波估计观测过程的内部状态。我们必须把这个过程在Kalman 滤波器的框架下建立模型， 这就意味着，对于

每一步k  我们要定义矩阵 、 、 、 、 如下：

Kalman Filter 假设k 时刻的真实状态是从k-1时刻演化而来，符合下式

这里

■ 是作用在前一状态的状态转移模型(状态转移矩阵)

■  是作用在控制向量 上的控制输入模型(输入输出矩阵)

■ 是过程噪声，假设是均值为0的白噪声，协方差为 则：

在k时刻，假设真实状态 的观测， 满足如下公式：

                                                    

其中 是观测模型(观测矩阵)，它把真实状态映射到观测空间， 是观测噪声，假设它是均值是0，方差是 的高斯白噪声： 

Kalman Filter基本动态系统模型如图(1)所示，圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差在右下方标出。

初始状态以及每一时刻的噪声向量{x0, w1, ..., wk, v1 ... vk} 都为认为是互相独立的。实际中，真实世界中动态系统并不是严格的符合此模型。但是Kalman模型是设计在噪声过程工作的，一个近似的符合已经可以使这个 滤波器非常有用了，更多复杂模型关于Kalman Filter模型的变种，将在下述中讨论：

                                                                     图(1)

■    4    卡尔曼滤波器

Kalman Filter 是一个递归的估计，即只要获知上一时刻的状态估计和当前状态的观测就可以计算出当前状态的估计，不同于其他的估计技术，Kalman 滤波器不需要观测或/和估计的历史记录，Kalman Filter 是一个纯粹的时域滤波器，而不像低通滤波器等频域滤波器那样，需要在频域中设计，然后转换到时域中应用。

下面，代表已知从m到n-1包括m时刻的观测在n时刻的估计值

卡尔曼滤波器的状态由以下两个变量表示：

      ■ 已知k时刻以前时刻观测值，k时刻的状态估计值

      ■ 误差协方差矩阵，度量状态估计的精度程度

Kalman 滤波包括两个阶段：预测和更新；在估计阶段，滤波器应用上一状态的估计做出对当前状态的估计。在更新阶段，滤波器利用在当前状态的观测值优化预测阶段的预测值，以获的一个更精确的当前状态的估计。

4.1    预测

状态预测：

■ 

估计协方差预测：

■

4.2    更新

新息或测量余量

■ 

新息协方差

■ 

Kalman 增益

■ 

状态估计更新

■ 

状态协方差更新

■ 

使用上述公式计算 仅在最优卡尔曼增益的时候有效。使用其他增益公式要复杂一些，看见推导

4.3    不变量

如果模型准确， 和 值将准确反映最初状态的分布，那么下面所有不变量保持不变，所有估计的误差均值为0：

■ 

■ 

这里 表示 的期望，而协方差矩阵则反映的估计的协方差

■ 

■ 

■ 

 

转自http://zhuyuge0.blog.163.com/blog/static/13230361420117541511810/

 

首先我们要了解kalman滤波器的基本知识，这样在之后的介绍能起到抛砖引玉的作用，当然还是和之前笔者关于opencv的博文一样，对于 kalman的具体数学推导过程不做介绍，不过看了一下也不是很难，主要涉及到一些关于概率和线性代数的知识，有兴趣的朋友可以看下，还可以一起讨论一 下，呵呵。

好，下面进入正题，关于kalman滤波器的基本思想是若有一组强而合理的假设，给出系统的历史测量值，则可以最大化这些早前测量值的后验概率的系 统状态模型。有的朋友可能对最大化后验概率概念不了解，它就是考虑早前的模型和新测量值的不确定性，在获得测量值之后，新建立的模型是具有最高正确概率的 模型。

kalman filter的三条假设也有必要说一下：1、被建模型系统是线性的，即k时刻系统状态可以用某个矩阵与k-1时刻的乘机表达

2、影响测量的噪声是白噪声，即噪声与时间不相关；

3、噪声的本质是高斯分布的，即只用均值和协方差就可以准确为幅值建模。（以上知识可以参考于刘opencv learning）

下面开始使用kalman滤波器，在此之前，我们必须了解CvKalman的结构，还有cvCreateKalman、cvKalmanPredict、cvKalmanCorrect的使用方法和实现的作用（可以参照下面的代码示例），这些基本的就不详细介绍。

看了某位仁兄的博文，我发现他对kalman使用总结很到位，下面笔者在他的基础之上重新说明下Kalman滤波器的使用步骤：

 在这些过程中，第一步相对来说最复杂，我们光光利用cvcreatekalman建立滤波器是不够的，必须对其初始化，包括对CvKlaman中 每个矩阵进行初始化（除了error_cov_pre和gain无需初始化，因为在刚开始时我们当然不会知道先验错误的相关矩形，而对于gain来说，它 的计算需要用到error_cov_pre，所以gain（增益矩阵）也无需初始化），包括对transition_matrix（转移矩 阵）,control_matrix（控制矩阵）,measurement_matrix（测量矩阵）,process_noise_cov（处理噪声相 相关矩阵）,measurement_noise_cov（测量噪声相关矩阵）,error_cov_post（后验错误矩阵），进行初始化。

下面采用一个简单的例子来讲解：

 1 #include <cv.h>
 2 #include <cxcore.h>
 3 #include <highgui.h>
 4 #include <cmath>
 5 #include <vector>
 6 #include <iostream>
 7 using namespace std;
 8 const int winHeight=600;
 9 const int winWidth=800;
10 CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);
11 //mouse event callback
12 void mouseEvent(int event, int x, int y, int flags, void *param )
13 {
14  if (event==CV_EVENT_MOUSEMOVE) {
15   mousePosition=cvPoint(x,y);
16  }
17 }
18 int main (void)
19 {
20  //1.kalman filter setup
21  const int stateNum=4;
22  const int measureNum=2;
23  CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)
24  CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
25  CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)
26  CvRNG rng = cvRNG(-1);//这个函数也许有的朋友不知道，初始化随机数生成器的状态
27  float A[stateNum][stateNum] ={//transition matrix传递矩阵
28   1,0,1,0,
29   0,1,0,1,
30   0,0,1,0,
31   0,0,0,1
32  };
33  memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));
34  cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );//cvRealScalar是用value初始化val[0],0初始化val[1],val[2],val[3]
35  cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));//cvSetIdentity是初始化带单位的矩阵，第二个参数是赋给对角线的值
36  cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));
37  cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));
38  //initialize post state of kalman filter at random
39 
40  cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));
41  CvFont font;//用随机数填充数组并更新RNG状态，CV_RAND_UNI表示均匀分布，随后两个参数表示随机数的上下界
42  cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);
43  cvNamedWindow("kalman");
44  cvSetMouseCallback("kalman",mouseEvent);
45  IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
46  while (1){
47   //2.kalman prediction
48   const CvMat* prediction=cvKalmanPredict(kalman,0);
49   CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);
50   //3.update measurement
51   measurement->data.fl[0]=(float)mousePosition.x;
52   measurement->data.fl[1]=(float)mousePosition.y;
53   //4.update
54   cvKalmanCorrect( kalman, measurement );  
55   //draw 
56   cvSet(img,cvScalar(255,255,255,0));
57   cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
58   cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red
59   char buf[256];
60   sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
61   cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
62   sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
63   cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));
64   
65   cvShowImage("kalman", img);
66   int key=cvWaitKey(3);
67   if (key==27){//esc   
68    break;   
69   }
70  }      
71  cvReleaseImage(&img);
72  cvReleaseKalman(&kalman);
73  return 0;
74 }

 

至于运行结果就不赋图了，很多博文都有这段代码的运行结果。

掌握这段程序之后我们可以再去读于刘书中的kalman滤波器跟踪一个旋转点的程序，也没有什么难点了。