A. 卡尔曼滤波理解与实现
本文为离散卡尔曼滤波算法的一 一个简明教程，从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。
XU Ruilin完成本教程主要部分的编写，WANG Xuejun完成第3节的编写，ZHU Ximin完成2.2节的编写，WEN Shuhan完成2.3节的编写，MAO Bo完成全文整理、修订和排版。
卡尔曼滤波（Kalman Filtering）及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。 鲁道夫·卡尔曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心访问时，发现他的方法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。
卡尔曼滤波尤其适用于动态系统，这种方法对于内存要求极低而运算速度快，且能够保持较好的计算精度，这使得这种方法非常适合解决实时问题和应用于嵌入式系统，也就是说，卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里，我们将逐步领会卡尔曼滤波的这些绝佳特点。
不过，现在我们先从复杂的舰艇航迹推算问题中解脱出来，从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。
假设有一辆无人车WALL-E，需要导引它从A点到达B点，共有两种手段（ 图1 ）：
显然，两种方法都有一定的误差。如果单独采用某一种方法进行定位，WALL-E在误差的影响下将无法到达B点。因此，需要将两种方法结合起来，得到一个更加精确的结果，这就是卡尔曼滤波要解决的问题。
卡尔曼滤波方法如何看待我们的问题呢？在探究这个问题之前，我们先对问题进行抽象，并用数学语言来描述我们的问题。
我们用矢量 来描述WALL-E的运动状态，这个列矢量 包括位置矢量 和速度矢量 两个分量。在WALL-E的问题上，我们现在不知道位置 和速度 的准确值，但是知道WALL-E的运动模型满足 状态方程 ，定位的方法，也即观测WALL-E运动状态的方法满足 观测方程 . 当然，我们也知道，这两种方法都存在一定的误差 ，那么我们的问题就可以转化为一个优化问题——
在这一优化问题中，目标函数是要使预测（估计）误差最小，同时约束于估计方法 和 的条件下。在卡尔曼滤波中，我们的估计原则（也就是最小化估计误差的原则）是 最小方差无偏估计 [1] ，我们将通过后面的过程分析来说明这一点。
在我们正式开始引入公式分析卡尔曼滤波问题之前，我们还必须解决一个问题------把连续的线性系统离散化，也就是将连续时域问题转化为时间序列问题。当然，目前我们只讨论线性系统的情况，关于非线性系统问题，我们有扩展卡尔曼滤波（Extended Kalman Filtering, EKF）和无迹卡尔曼滤波（Unscented Kalman Filtering, UKF）两种方法来求解。
补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为
若采样周期为 ，则从时刻 到时刻 ，有
令 ， ，则离散化的状态方程为
通过对线性系统的离散化处理，我们现在可以考虑每一个时刻WALL-E的运动状态。接下来，我们将用 来表示在 时刻运动状态的最优估计值；用 表示用 时刻对 时刻的状态预测值；用 表示对 时刻综合预测和观测两种方法的最优估计值。
在估计WALL-E位置的问题上，假定我们已经知道它是匀速直线运动，WALL-E身上还携带有一个GPS传感器可以提供它的位置信息，WALL-E在前进过程中可能会遇到一些情况，比如停止前进或是受到风的影响。
加入我们已知的是WALL-E上一个时刻的最佳估计状态，即k-1时刻的位置和速度，要求的是下一时刻即k时刻的最佳估计状态，即k时刻的位置和速度，我们可以发现有两种方法可以得到它的k时刻的状态：
一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。
另一种是通过WALL-E所携带的GPS来确定它的位置，因为GPS是测量出的就是WALL-E的实时状态，因此它比较准确。但是GPS测量k时刻的状态有两个问题，一是GPS只能测出WALL-E的位置，而测不出它的速度；二是GPS传感器测量的时候也会有仪器的误差，只能说它是比较准确的，比较接近真实值的。
那么接下来问题来了，我们如何得到k时刻WALL-E的真实状态呢？
我们将第一种方法得到的状态值称为预测值，第二种方法得到的状态值称为测量值，对汽车的最佳估计就是将这两部分信息结合起来，尽量的去逼近k时刻的真实值。
下面再深入一些思考，怎么将这两部分结合起来？
在初始时间k-1， 是WALL-E的最佳估计值，WALL-E其实可以是估计值附近的任何位置，并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间，估计的不确定性增加，用一个更大的方差表示，这是因为在时间步骤k-1和k之间，WALL-E可能收到了风力的影响，或者脚可能向前滑了一点，因此，它可能已经行进了与模型预测的距离不同的距离。
WALL-E位置的另一个信息来源来自测量，方差表示误差测量的不确定性，真正的位置同样可以是平均值附近的任何位置。
预测值和测量值，对WALL-E的最佳估计是将这两部分信息结合起来，将两个概率函数相乘得到另一个高斯函数，该估计值的方差小于先前估计值，并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。
以下，我们将进行e的运算推导
设：
则有实际目标变量的表达式：
数学模型中目标变量的表达式：
实际模型中测量变量的表达式：
数学模型中测量变量的表达式：
将目标变量的实际值和估计值相减：
将上述方程带入误差e的表达式，我们可得出误差e的解析解：
从推导结果中我们不难看出，估计值和实际值的误差随时间呈指数形式变化，当（F-KH）<1时，随着时间的推移，会无限趋近于零，也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。
在估计WALL-E位置的问题上，我们不知道位置 和速度 的准确值，但是我们可以给出一个估计区间（ 图5.a ）。卡尔曼滤波假设所有的变量是随机的且符合高斯分布（正态分布）。每个变量有一个均值 和一个方差 （ 图5.b ）。而 图5.c 则表示速度和位置是相关的。
假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，WALL-E不会走的很远。这种关系在跟踪系统状态时很重要，它给了我们更多的信息：一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。
根据数理统计知识，我们知道这种两个观测值（随机变量）之间的关系可以通过一个协方差矩阵
描述（ 图6 ）。
我们假设系统状态的分布为 高斯分布（正态分布） ，所以在 时刻我们需要两个信息：最佳预估值 及其协方差矩阵 （如式(2)所示）。
下一步，我们需要通过 时刻的状态来预测 时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎，它仅仅是对 时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。在这个过程中，位置 和速度 的变化为
我们可以通过一个状态转移矩阵 来描述这个转换关系
同理，我们更新协方差矩阵 为
到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为
引入矩阵表示为
式中 称为控制矩阵， 称为控制向量（例如加速度 ）。当然，如果没有任何外界动力影响的系统，可以忽略这一部分。
我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？如果状态只会根据系统自身特性演变，那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确，那样也不会有任何问题。但是如果有些外力我们无法预测，例如我们在跟踪一个四轴飞行器，它会受到风力影响；或者在跟踪一个轮式机器人，轮子可能会打滑，地面上的突起会使它减速。我们无法跟踪这些因素，而这些不确定事件发生时，预测方程将会失灵。因此，我们将这些不确定性统一建模，在预测方程中增加一个不确定项。
通过这种方式，使得原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点（ 图7.a ）。 可以这样描述------ 中的每个点移动到一个符合方差 的高斯分布里（ 图7.b ）。换言之，我们把这些不确定因素描述为方差为 的高斯噪声，并用 表示。这样就会产生一个新的高斯分布，方差不同，但是均值相同（ 图7.c ）。
通过对 的叠加扩展，得到完整的预测转换方程为
新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里，我们得到了一个模糊的估计范围------通过 和 描述的范围。
我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态，现在，我们要把另一种方法，也就是观测（本问题中为GPS定位）考虑进来，以进一步修正对运动状态的估计（ 图8 ）。
我们用矩阵 来描述观测方法的作用，于是有
再加入观测噪声 ，观测方程为
从控制论的角度出发，我们定义新息（也即观测值与预测值的误差）为
当然我们也知道，观测本身也会存在误差，比如本问题中的GPS定位精度仅有10m. 因此，我们用矩阵 来描述这种不确定性（ 图10 及 图11.a ）。
这时，我们新息的协方差为
现在我们需要把两种方法得到的可能性融合起来（ 图11.b ）。对于任何状态，有两个可能性：1. 传感器的观测值更接近系统真实状态；2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式，并且我们想知道两者都准确的概率值，于是我们可以通过加权来解决更相信谁的问题（ 图11.c ）。
我们现在知道，系统模型的状态预测 与对系统的状态观测 服从高斯分布，把这个问题抽象一下就是——
根据我们的一个估计准则------ 最小方差估计 ，那么这个问题可以转化为优化问题求解
求导数（差分）得
则 ，从而
当维度高于一维时，我们用矩阵来描述，有
这里的 称为 卡尔曼增益 （Kalman Gain），也就是我们在解决更信任哪种方法时的偏向程度。
如果我们从两个独立的维度估计系统状态，那么根据系统模型的预测为
通过传感器的观测为
我们结合着两种方法得到
由 可知，卡尔曼增益为
将 约去（ 中也含有 项），得
此时的卡尔曼增益实际为
我们最后再来验证一下 估计的无偏性 ——
这里我们设 时刻的真值为 ，由于
由于 （ 从初值而来的无偏传递性 ）可知 ，即卡尔曼滤波满足无偏估计准则。显然，其中要求系统噪声和观测噪声是不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，卡尔曼滤波的估计结果是有偏的。
到这里，我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设
假设一 和 都是零均值高斯白噪声，也即 ，
假设二 与 无关，也即
假设三 系统初值 的均值和方差已知，且 与 均不相关。
以及5个基本方程 方程一 状态预测
方程二 协方差预测
方程三 卡尔曼增益
B. 什么是滤波算法
卡尔曼滤波器（Kalman Filter）是一个最优化自回归数据处理算法（optimal recursive data processing algorithm）。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Kолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。
现设线性时变系统的离散状态防城和观测方程为：
X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)
Y(k) = H(k)·X(k)+N(k)
其中
X(k)和Y(k)分别是k时刻的状态矢量和观测矢量
F(k,k-1)为状态转移矩阵
U(k)为k时刻动态噪声
T(k,k-1)为系统控制矩阵
H(k)为k时刻观测矩阵
N(k)为k时刻观测噪声
则卡尔曼滤波的算法流程为：
预估计X(k)^= F(k,k-1)·X(k-1)
计算预估计协方差矩阵
C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
Q(k) = U(k)×U(k)'
计算卡尔曼增益矩阵
K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
R(k) = N(k)×N(k)'
更新估计
X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]
计算更新后估计协防差矩阵
C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'
X(k+1) = X(k)~
C(k+1) = C(k)~
C. surf算法C语言编写，要做嵌入式开发，不要C++和基于OPENCV的
surf借鉴了sift中简化近似的思想，将DOH中的高斯二阶微分模板进行了近似简化，使得模板对图像的滤波只需要进行几个简单的加减法运算，并且，这种运算与滤波模板的尺寸有关。实验证明surf算法较sift算法在运算速度上要快3倍左右。
1积分图像
surf算法中要用到积分图像的概念。借助积分图像，图像与高斯二阶微分模板的滤波转化为对积分图像的加减运算。积分图像（IntegralImage）的概念是由viola和Jones提出来的，而将类似积分图像用于盒子滤波是由Simard等人提出。
积分图像中任意一点（i,j）的值为ii(i,j)为原图像左上角到任意点（i,j）相应的对角线区域灰度值的总和即：
公式中，I(x`,y`)表示原图像中点（i`,j`）的灰度值，ii（x,y）可以由下面两公式迭代计算得到：
公式中，S（x,y）表示一列的积分，且S（i,-1）=0,ii(-1,j)=0.求积分图像，只需对原图像的所有像素素进行一遍扫描。下面的代码为c++语言的实现
pOutImage[0][0]=pInImage[0][0];
for(intx=1,x<nWidth;i++)
{
pOutImage[x][0]=pInImage[x-1][0]+pInImage[x][0];
}
for(inty=1;y<nHeight;y++)
{
intnSum=0;
for(intx=0;x<nWidth;x++)
{
nSum=pInImage[x][y];
pOutImage[x][y]=pInImage[x][y-1]+nSum;
}
}
如图表示，在求取窗口w内的像元灰度和时，不管窗口W的大小如何，均可利用积分图像的4个对应点（i1,j1）（i2,j2）（i3,j3）（i4,j4）的值计算的到。也就是说，求取窗口W内的像元灰度和与窗口的尺寸是无关的。窗口W内的像元的灰度和为
Sum（W）=ii(i4,j4)-ii(i2,j2)-ii(i3,j3)+ii(i1,j1)
下面看以截图，相信都可以看懂
关于矩形区域内像素点的求和应该是一种简单重复性运算，采用这种思路总体上提高了效率。为什么这么说呢？假设一幅图片共有n个像素点，则计算n个位置的积分图总共的加法运算有n-1次（注意：可不是次哦，要充分利用递推思想），将这些结果保存在一个跟原图对应的矩阵M中。当需要计算图像中某个矩形区域内的所有像素之和是直接像查表一样，调出A,B,C,D四点的积分图值，简单的加减法（注意只需要三次哦）即可得到结果。反之，如果采用naive的方式直接在原图像中的某个矩形区域内求和，你想想，总共可能的矩形组合有多少？！！且对于一幅图像n那是相当大啊，所以2^n
那可是天文数字，而且这里面绝大部分的矩形有重叠，重叠意味着什么？在算求和的时候有重复性的工作，其实我们是可以有效的利用已经计算过的信息的。这就是积分图法的内在思想：它实际上是先计算n个互不重叠（专业点说是不相交）的矩形区域内的像素点求和，充分利用这些值（已有值）计算未知值，有点类似递推的味道...这就完全避免了重复求和运算。
这样就可以进行2种运算：
（1）任意矩形区域内像素积分。由图像的积分图可方便快速地计算图像中任意矩形内所有像素灰度积分。如下图2.3所示，点1的积分图像ii1的值为(其中Sum为求和):
ii1=Sum（A）
同理，点2、点3、点4的积分图像分别为：
ii2=Sum(A)+Sum(B);ii3=Sum(A)+Sum(C);ii4=Sum(A)+Sum(B)+Sum(C)+Sum(D);
矩形区域D内的所有像素灰度积分可由矩形端点的积分图像值得到:
Sum(D)=ii1+ii4-(ii2+ii3)(1)
(2)特征值计算
矩形特征的特征值是两个不同的矩形区域像素和之差，由(1)式可以计算任意矩形特征的特征值，下面以图2.1中特征原型A为例说明特征值的计算。
如图2.4所示，该特征原型的特征值定义为:
Sum（A）-Sum（B）
根据（1）式则有：Sum(A)=ii4+ii1-(ii2+ii3);Sum(B)=ii6+ii3-(ii4+ii5);
所以此类特征原型的特征值为:
(ii4-ii3)-(ii2-ii1)+(ii4-ii3)-(ii6-ii5)
另示：运用积分图可以快速计算给定的矩形之所有象素值之和Sum(r)。假设r=(x,y,w,h),那么此矩形内部所有元素之和等价于下面积分图中下面这个式子:
Sum(r)=ii(x+w,y+h)+ii(x-1,y-1)-ii(x+w,y-1)-ii(x-1,y+h)
由此可见，矩形特征特征值计算只与此特征端点的积分图有关，而与图像坐标值无关。对于同一类型的矩形特征，不管特征的尺度和位置如何，特征值的计算所耗费的时间都是常量，而且都只是简单的加减运算。其它类型的特征值计算方法类似。
D. 图像处理之双边滤波算法
双边滤波是一种非线性的滤波方法，是结合图像的空间邻近度和像素值相似度的一种折中处理，同时考虑空域信息和灰度相似性，达到保边去噪的目的，具有简单、非迭代、局部处理的特点。之所以能够达到保边去噪的滤波效果是因为滤波器由两个函数构成：
一个函数是像素欧式距离决定滤波器模板的系数，另一个是由像素的灰度差值决定滤波器模板的系数。
其综合了高斯滤波器（Gaussian Filter）和α-截尾均值滤波器（Alpha-Trimmed mean Filter）的特点。高斯滤波器只考虑像素间的欧式距离，其使用的模板系数随着和窗口中心的距离增大而减小；Alpha截尾均值滤波器则只考虑了像素灰度值之间的差值，去掉α%的最小值和最大值后再计算均值。
双边滤波器使用二维高斯函数生成距离模板，使用一维高斯函数生成值域模板。
双边滤波器中，输出像素的值依赖于邻域像素的值的加权组合，其公式如下：
其中(k,l)为模板窗口的中心坐标；(i,j)为模板窗口的其他系数的坐标；σd为高斯函数的标准差。 使用该公式生成的滤波器模板和高斯滤波器使用的模板是没有区别的。
值域模板系数的生成公式如下：
其中，函数f(x,y)表示要处理的图像，f(x,y)表示图像在点(x,y)处的像素值；(k,l)为模板窗口的中心坐标；(i,j)为模板窗口的其他系数的坐标；σr为高斯函数的标准差。
将上述两个模板相乘就得到了双边滤波器的模板,其公式如下：