卡尔曼滤波形象解释及c++实现

前言：

      Kalman60年代把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。

一、直白解释(经典）

       我一直有一个愿望，就是把抽象的理论具体化，用最直白的方式告诉大家--不提一个生涩的词，不写一个数学公式，像讲故事一样先把道理说明白，需要知道细节的同学可以自己去查所有需要知道的一切。因为学习的过程告诉我，最难的其实是最初和这个理论和应用背景亲和的过程--这些理论它究竟是做什么的，又是怎么做到的。可惜我们能看到的关于这些理论的资料大多数都是公式的堆砌并且假定我们明白许多“基本的道理”，其实这些“基本的道理”往往是我们最难想象和超越的。以卡尔曼滤波为例，让我们尝试一种不同的学习方法。

      相信所有学习卡尔曼滤波的同学首先接触的都是状态方程和观测方程，学过控制系统的同学可能不陌生，否则，先被那两个看起来好深奥的公式给吓跑了，关键是还不知道他们究竟是干什么的，什么是状态，什么是观测。。。。。。如果再看到后面的一大串递归推导增益，实在很晕很晕，更糟糕的是还没整明白的时候就已经知道卡尔曼滤波其实已经不够使了，需要extended kalmanfilter 和particle filter了。。。

       其实我们完全不用理会这些公式。先来看看究竟卡尔曼滤波是做什么的，理解了卡尔曼滤波，下面的就顺其自然了。

用一句最简单的话来说，卡尔曼滤波是来帮助我们做测量的，大家一定不明白测量干嘛搞那么复杂？测量长度拿个尺子比一下，测量温度拿温度表测一下不就完了嘛。的确如此，如果你要测量的东西很容易测准确，没有什么随机干扰，那真的不需要劳驾卡尔曼先生。但在有的时候，我们的测量因为随机干扰，无法准确得到，卡尔曼先生就给我们想了个办法，让我们在干扰为高斯分布的情况下，得到的测量均方误差最小，也就是测量值扰动最小，看起来最平滑。

      还是举例子最容易明白。我最近养了只小兔子，忍不住拿小兔子做个例子嘻嘻。

      每天给兔子拔草，看她香甜地吃啊吃地，就忍不住关心一下她的体重增长情况。那么我们就以小兔子的体重作为研究对象吧。假定我每周做一次观察，我有两个办法可以知道兔子的体重，一个是拿体重计来称：或许你有办法一下子就称准兔子的体重（兽医通常都有这办法），但现在为了体现卡尔曼先生理论的魅力，我们假定你的称实在很糟糕，误差很大，或者兔子太调皮，不能老实呆着，弹簧秤因为小兔子的晃动会产生很大误差。尽管有误差，那也是一个不可失去的渠道来得到兔子的体重。还有一个途径是根据书本上的资料，和兔子的年龄，我可以估计一下我的小兔子应该会多重，我们把用称称出来的叫观察量，用资料估计出来的叫估计值，无论是观察值还是估计值显然都是有误差的，假定误差是高斯分布。现在问题就来了，按照书本上说我的兔子该3公斤重，称出来却只有2.5公斤，我究竟该信哪个呢？如果称足够准，兔子足够乖，卡尔曼先生就没有用武之地了呵呵，再强调一下是我们的现状是兔兔不够乖，称还很烂呵呵。在这样恶劣的情景下，卡尔曼先生告诉我们一个办法，仍然可以估计出八九不离十的兔兔体重，这个办法其实也很直白，就是加权平均，把称称出来的结果也就是观测值和按照书本经验估算出来的结果也就是估计值分别加一个权值，再做平均。当然这两个权值加起来是等于一的。也就是说如果你有0.7分相信称出来的体重，那么就只有0.3分相信书上的估计。说到这里大家一定更着急了，究竟该有几分相信书上的，有几分相信我自己称的呢？都怪我的称不争气，没法让我百分一百信赖它，还要根据书上的数据来做调整。好在卡尔曼先生也体会到了我们的苦恼，告诉我们一个办法来决定这个权值，这个办法其实也很直白，就是根据以往的表现来做决定，这其实听起来挺公平的，你以前表现好，我就相信你多一点，权值也就给的高一点，以前表现不好，我就相信你少一点，权值自然给的低一点。那么什么是表现好表现不好呢，表现好意思就是测量结果稳定，方差很小，表现不好就是估计值或观测值不稳定，方差很大。想象你用称称你的哦兔子，第一次1公斤第二次10公斤，第三次5公斤，你会相信你的称吗，但是如果第一次3公斤第二次3.2公斤，第三次2.8公斤，自然我就相信它多一点，给它一个大的权值了。

      有了这个权值，下面的事情就很好办了。很显然卡尔曼先生是利用多次观察和估计来达到目的的，我们也只能一步一步地调整我们的观察和估计值，来渐渐达到准确的测量，所以整个算法是递归的，需要多次重复调整的。调整的过程也很简单，就是把实测值（称出来的体重）和估计值（书上得来的体重）比较一下，如果估计值比测量值小，那就把估计值加上他们之间的偏差作为新的估计值，当然前面要加个系数，就是我们前面说的加权系数，这个地方我要写个公式，因为很简单就能说明白

      比如我们的观查值是Z，估计值是X， 那么新的估计值就应该是 Xnew  =  X  + K ( Z-X），从这个公式可以看到，如果X估计小了，那么新的估计值会加上一个量K ( Z-X）, 如果估计值大了,大过Z了，那么新的估计值就会减去一个量K ( Z-X），这就保证新的估计值一定比现在的准确，一次一次递归下去就会越来越准却了，当然这里面很有作用的也是这个K，也就是我们前面说的权值，书上都把他叫卡尔曼增益。。。(Xnew  =  X  + K ( Z-X） = X ×（1-K) + KZ ，也就是说估计值X的权值是1-k，而观察值Z的权值是k，究竟k 取多大，全看估计值和观察值以前的表现，也就是他们的方差情况了）

     发现把一个问题讲明白还真不是件容易的事情，谁听明白了我佩服谁，因为我已经把自己讲糊涂了哈。

     顺便就把extended kalman filter和particle filter提一下，因为高斯模型有时不适用，于是有了extended kalman filter，而particle filter是用于多个对象的，比如除了兔子我还有只猫，他们的体重有一个联合概率模型，每一个对象就是一个particle。无论是卡尔曼滤波还是particle滤波，都是概率分布传递的过程，卡尔曼传递的是高斯分布，particle filter 传递的是高斯混合分布，每一个峰代表一个动物在我们的例子。

 二、实现的算法步骤

一步Kalman滤波程序：

对n维线性动态系统与m维线性观测系统
状态方程：X_k = (A_k,k-1)*X_k-1 + W_k-1
观测方程：Y_k = H_k*X_k + V_k
k = 1,2,...
 X_k为n维状态向量，              Y_k为m维观测向量。
(A_k,k-1)(nxn维)为状态转移阵，   H_k(nxm维)为观测矩阵
W_k为n维状态噪声向量，一般假设为高斯白噪声，且均值为0，协方差为Q_k
V_k为m维观测噪声向量，一般假设为高斯白噪声，且均值为0，协方差为R_k

Kalman滤波问题就是在已知k个观测向量Y_0,Y_1,...,Y_k和初始状态向量估计X_0，及其估计误差协方差阵P_0，以及Q_k,R_k等情况下，递推估计各个x_k及其噪声，估计协方差阵P_k的问题。具体计算公式如下：

P_k|k-1 = (A_k,k-1 )* P_k-1 * (A_k,k-1)^T + Q_k-1 //状态预估计协方差矩阵
K_k = P_k|k-1 * H_k^T * [H_k * P_k|k-1 * H_k^T + R_k]^{-1}
X_k = A_k,k-1 * X_k-1 + K_k * [Y_k - H_k * A_k,k-1 * X_k-1] //更新状态向量估计
P_k = (I-K_k*H_k)*P_k|k-1 //更新状态估计误差协方差矩阵
其中：
K_k(nxm维)为Kalman增益矩阵
Q_k(nxn维)为W_k协方差阵
R_k(mxm维)为V_k协方差阵
P_k(nxn维)为估计误差协方差阵

一步Kalman滤波函数参数：
n：     整型变量，动态系统的维数（状态量个数）
m：     整型变量，观测系统的维数（观测量个数）
A：     双精度2维数组，nxn，系统状态转移矩阵
H：     双精度2维数组，mxn，观测矩阵
Q：     双精度2维数组，nxn，模型噪声W_k的协方差矩阵
R：     双精度2维数组，mxm，观测噪声V_k的协方差矩阵
y_k：   双精度2维数组，mx1，观测向量序列
x_k：   双精度2维数组，nx1，状态向量估计量序列。输入x_k-1，返回x_k
P_k：   双精度2维数组，nxn，存放误差协方差阵P_k-1。返回时存放更新的估计误差协
方差阵P_k
输出：  状态向量估计x_k, 状态误差协方差P_k

三、程序表示

下面贴C++程序实现：

本人已经标记的非常清楚了，请大家欣赏：

//各种矩阵声明   /* 状态误差协方差矩阵 */Qk = new float[4 * 4];   // 状态阵协方差for (int y = 0; y < 4; y++)for (int x = 0; x < 4; x++)Qk[y * 4 + x] = 0.0;for (int y = 0; y < 4; y++)Qk[y * 4 + y] = 1.0;  /* 对角矩阵的协方差都为1.0，且相互独立 */// 观测阵协方差(观测噪声)Rk = new float[2 * 2];  // 观测阵协方差for (int y = 0; y < 2; y++)for (int x = 0; x < 2; x++)Rk[y * 2 + x] = 0.0;for (int y = 0; y < 2; y++)Rk[y * 2 + y] = 1.0;  /* 协方差都为1.0，且相互独立 */Pk = new float[4 * 4];   // 状态测量误差协方差for (int y = 0; y < 4; y++)for (int x = 0; x < 4; x++)Pk[y * 4 + x] = 0.0;for (int y = 0; y < 4; y++)Pk[y * 4 + y] = 10.0;  /* 初始协方差都为10，且相互独立 */Am = new float[4 * 4];     // 状态转移阵for (int y = 0; y < 4; y++)for (int x = 0; x < 4; x++)Am[y * 4 + x] = 0.0;for (int y = 0; y < 4; y++)Am[y * 4 + y] = 1.0;   /* 对角为1 */Am[0 * 4 + 2] = DeltaT;Am[1 * 4 + 3] = DeltaT;Hm = new float[2 * 4];     // 观测for (int y = 0; y < 2; y++)for (int x = 0; x < 4; x++)Hm[y * 4 + x] = 0.0;Hm[0 * 4 + 0] = 1.0; Hm[1 * 4 + 1] = 1.0;yk = new float[2];   // 观测量，排列顺序：x, yyk[0] = (float)x0;yk[1] = (float)y0;/* 初始化运动速度、位置、半窗宽高、尺度变化速度等状态量 */xk = new float[4]; // 状态量，排列顺序：x, y, xv, yvxk[0] = (float)x0;xk[1] = (float)y0;xk[2] = 0.0;xk[3] = 0.0;///////////////////////////////////////////////////核心代码int Kalman(int n, int m, float * A, float * H, float * Q, float * R,float * y_k, float * x_k, float * P_k){float * Ax, *PH, *P, *P_kk_1, temp, *K_k;float * yHAx, *KH, *I;double * HPHR;int x, y, i;int invRval;P = new float[n*n];P_kk_1 = new float[n*n];/* 1.状态误差协方差的预测 P_k|k-1 */for (y = 0; y < n; y++)  /* A_k,k-1*P_k-1 */for (x = 0; x < n; x++){temp = 0;for (i = 0; i < n; i++)temp += A[y*n + i] * P_k[i*n + x]; P[y*n + x] = temp;}for (y = 0; y < n; y++)  /* (A_k,k-1*P_k-1)*A_k,k-1^T+Q_k-1 */for (x = 0; x < n; x++){temp = 0;for (i = 0; i < n; i++)temp += P[y*n + i] * A[x*n + i];//A为状态转移阵的转置P_kk_1[y*n + x] = temp + Q[y*n + x];}/* 2.求增益矩阵 K_k */PH = new float[n*m];for (y = 0; y < n; y++) /* P_k|k-1*H_k^T */for (x = 0; x < m; x++){temp = 0;for (i = 0; i < n; i++)temp += P_kk_1[y*n + i] * H[x*m + i];PH[y*m + x] = temp;//nxm维数}HPHR = new double[m*m]; /* 求H_k*(P_k|k-1*H_k)^T+R_k */for (y = 0; y < m; y++)for (x = 0; x < m; x++){temp = 0;for (i = 0; i < n; i++)temp += H[y*n + i] * PH[i*m + x];//mxm维数HPHR[y*m + x] = temp + R[y*m + x];//mxm维数}invRval = brinv(HPHR, m); /* 求逆 */if (invRval == 0){delete[] P;delete[] P_kk_1;delete[] PH;delete[] HPHR;return(0);}K_k = new float[n*m]; /* 求K_k */for (y = 0; y < n; y++)for (x = 0; x < m; x++){temp = 0;for (i = 0; i < m; i++)temp += PH[y*m + i] * (float)HPHR[i*m + x];//n*m x m*nK_k[y*m + x] = temp;//n*n}/* 3.求状态的估计 x_k */Ax = new float[n];for (y = 0; y < n; y++) /* A_k,k-1 * x_k-1 */{temp = 0;for (i = 0; i < n; i++)temp += A[y*n + i] * x_k[i];Ax[y] = temp;//n*1}yHAx = new float[m];for (y = 0; y < m; y++) /* y_k - H_k*(A_k,k-1)*x_k-1 */ {temp = 0;for (i = 0; i < n; i++)temp += H[y*n + i] * Ax[i];//m*n x n*1=m*1yHAx[y] = y_k[y] - temp;//m*1}for (y = 0; y < n; y++)   /* 求x_k */{temp = 0;for (i = 0; i < m; i++)temp += K_k[y*m + i] * yHAx[i];//n*m x m*1x_k[y] = Ax[y] + temp;//n*1}/* 4.更新误差的协方差 P_k */KH = new float[n*n];for (y = 0; y < n; y++)for (x = 0; x < n; x++){temp = 0;for (i = 0; i < m; i++)temp += K_k[y*m + i] * H[i*n + x];//n*m xm*n=n*nKH[y*n + x] = temp;//n*n}I = new float[n*n];for (y = 0; y < n; y++)for (x = 0; x < n; x++)I[y*n + x] = (float)(x == y ? 1 : 0);//对角线为1的n*n矩阵for (y = 0; y < n; y++)   /* I - K_k*H_k */for (x = 0; x < n; x++)I[y*n + x] = I[y*n + x] - KH[y*n + x];//n*nfor (y = 0; y < n; y++)  /* P_k = ( I - K_k*H_k ) * P_k|k-1 */for (x = 0; x < n; x++){temp = 0;for (i = 0; i < n; i++)temp += I[y*n + i] * P_kk_1[i*n + x];//n*n x n*n=n*nP_k[y*n + x] = temp;}delete[] P;delete[] P_kk_1;delete[] PH;delete[] HPHR;delete[] K_k;delete[] Ax;delete[] yHAx;delete[] KH;delete[] I;return(1);}

最后卡尔曼滤波求出的是更新的状态矩阵x_k以及状态误差协方差矩阵P_k。

opencv也有卡尔曼滤波程序，有空再分析。

四、参考资料

1.http://www.cnblogs.com/jason-jiang/archive/2007/01/13/619643.html

2.http://www.zhihu.com/question/22422121/answer/22877882