卡尔曼滤波 -- 从推导到应用
前言
卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方差为目的而推导出的几个递推数学等式,也可以从贝叶斯推断的角度来推导。
本文将分为两部分：
第一部分，结合例子，从最小均方差的角度，直观地介绍卡尔曼滤波的原理，并给出较为详细的数学推导。
第二部分，通过两个例子给出卡尔曼滤波的实际应用。其中将详细介绍一个匀加速模型，并直观的对比系统状态模型的建立对滤波的影响。
第一部分
先看一个对理解卡尔曼滤波能起到作用的的笑话:
一片绿油油的草地上有一条曲折的小径,通向一棵大树.一个要求被提出:从起点沿着小径走到树下.
“很简单.” A说,于是他丝毫不差地沿着小径走到了树下.
现在，难度被增加了：蒙上眼。
“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树旁。“唉，好久不练，生疏了。” （只凭自己的预测能力）
“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地歪歪扭扭的走到了树旁。“唉，这个 GPS 没做好，漂移太大。”（只依靠外界有很大噪声的测量）
“我来试试。” 旁边一也当过特种兵的拿过 GPS, 蒙上眼，居然沿着小径很顺滑的走到了树下。（自己能预测+测量结果的反馈）
“这么厉害！你是什么人?”“卡尔曼 ! ”
“卡尔曼？！你就是卡尔曼？”众人大吃一惊。
“我是说这个 GPS 卡而慢。”
此段引用自 highgear 的 《授之以渔：卡尔曼滤波器...大泄蜜...》 (点击可跳转到该网页)
这个小笑话很有意思的指出了卡尔曼滤波的核心，预测+测量反馈，记住这种思想。
-----------------------------------------------------------分割线-----------------------------------------------------------------------
在介绍卡尔曼滤波前，简单说明几个在学卡尔曼过程中要用到的概念。即什么是协方差，它有什么含义，以及什么叫最小均方差估计，什么是多元高斯分布。如果对这些有了了解，可以跳过，直接到下面的分割线。
均方差：它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方差等于每个样本的误差平方再乘以该样本出现的概率的和。
方差：方差是描述随机变量的离散程度，是变量离期望值的距离。
注意两者概念上稍有差别，当你的样本期望值就是真实值时，两者又完全相同。最小均方差估计就是指估计参数时要使得估计出来的模型和真实值之间的误差平方期望值最小。
两个实变量之间的协方差：
它表示的两个变量之间的总体误差，当Y=X的时候就是方差。下面说说我对协方差的通俗理解，先抛去公式中的期望不谈，即假设样本X,Y发生的概率就是1，那么协方差的公式就变成了：
这就是两个东西相乘，马上联想到数值图像里的相关计算。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。协方差矩阵只不过就是元素多了组成了矩阵，其中协方差矩阵的对角线就是方差，具体公式形式请见wiki。
其实，这种相乘的形式也有点类似于向量投影，即两个向量的内积。再远一点，联想到傅里叶变换里频谱系数的确定，要确定一个函数f(x)在某个频率w上的频谱，就是<f(x),cos(wt)>,< ,>表示向量内积，通俗的讲是将f(x)投影到cos(wt)上，要讲清傅里叶的本质需要另写一篇博文，这里提到这些只是觉得有益于对知识的相互理解。
高斯分布：概率密度函数图像如下图,四条曲线的方差各不相同,方差决定了曲线的胖瘦高矮。（图片来源：*)
多元高斯分布：就是高斯分布的低维向高维的扩展，图像如下。
对应多元高斯分布的公式也请自行谷歌，以前高斯公式中的方差也变成了协方差，对应上面三张图的协方差矩阵分别如下：
注意协方差矩阵的主对角线就是方差，反对角线上的就是两个变量间的协方差。就上面的二元高斯分布而言，协方差越大，图像越扁，也就是说两个维度之间越有联系。
-----------------------------------------------------------分割线---------------------------------------------------------------------
这部分每讲一个数学性的东西，接着就会有相应的例子和直观的分析帮助理解。
首先假设我们知道一个线性系统的状态差分方程为
其中x是系统的状态向量，大小为n*1列。A为转换矩阵，大小为n*n。u为系统输入，大小为k*1。B是将输入转换为状态的矩阵，大小为n*k。随机变量w为系统噪声。注意这些矩阵的大小，它们与你实际编程密切相关。
看一个具体的匀加速运动的实例。
有一个匀加速运动的小车，它受到的合力为 ft , 由匀加速运动的位移和速度公式，能得到由 t-1 到 t 时刻的位移和速度变化公式：
该系统系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。控制输入变量为u，也就是加速度，于是有如下形式:
所以这个系统的状态的方程为：
这里对应的的矩阵A大小为 2*2 ,矩阵B大小为 2*1。
貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。并且，即使你建立了较为准确的模型，只要你在某一步有误差，由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），以至于最后得到的估计结果完全不能用了。回到最开始的那个笑话，如果那个完全凭预测的特种兵在某一步偏离了正确的路径，当他站在错误的路径上（而他自己以为是正确的）做下一步预测时，肯定走的路径也会错了，到最后越走越偏。
既然如此，我们就引进反馈。从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。
测量值当然是由系统状态变量映射出来的，方程形式如下：
注意Z是测量值，大小为m*1(不是n*1，也不是1*1，后面将说明），H也是状态变量到测量的转换矩阵。大小为m*n。随机变量v是测量噪声。
同时对于匀加速模型，假设下车是匀加速远离我们，我们站在原点用超声波仪器测量小车离我们的距离。
也就是测量值直接等于位移。可能又会问，为什么不直接用测量值呢？测量值噪声太大了，根本不能直接用它来进行计算。试想一个本来是朝着一个方向做匀加速运动的小车，你测出来的位移确是前后移动（噪声影响），只根据测量的结果，你就以为车子一会往前开一会往后开。
对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q,R为噪声变量的协方差矩阵。
看到这里自然要提个问题，为什么噪声模型就得服从高斯分布呢？请继续往下看。
对于小车匀加速运动的的模型，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0。测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
那么：
Q中，叠加在速度上系统噪声方差为0.01，位移上的为0，它们间协方差为0，即噪声间没有关联。
理论预测（先验）有了，测量值（后验）也有了，那怎么根据这两者得到最优的估计值呢？首先想到的就是加权，或者称之为反馈。
我们认定是预测（先验）值，
其中，称之为残差，也就是预测的和你实际测量值之间的差距。如果这项等于0，说明预测和测量出的完全吻合。这种反馈递推的形式又让我联想到数值分析里用来求解线性方程组时的一种迭代方法，Gauss-Seidel迭代法，有兴趣的可以看看。
现在的关键就是求取这个K。这时最小均方差就起到了作用，顺便在这里回答为什么噪声必须服从高斯分布，在进行参数估计的时候，估计的一种标准叫最大似然估计，它的核心思想就是你手里的这些相互间独立的样本既然出现了，那就说明这些样本概率的乘积应该最大（概率大才出现嘛）。如果样本服从概率高斯分布，对他们的概率乘积取对数ln后，你会发现函数形式将会变成一个常数加上样本最小均方差的形式。因此，看似直观上很容易理解的最小均方差理论上来源就出于那里（详细过程还请自行谷歌，请原谅，什么都讲的话就显得这边文章没有主次了）。
先看估计值和真实值间误差的协方差矩阵，提醒一下协方差矩阵的对角线元素就是方差，求这个协方差矩阵，就是为了利用他的对角线元素的和计算得到均方差.
这里请注意ek是向量，它由各个系统状态变量的误差组成。如匀加速运动模型里，ek便是由位移误差和速度误差，他们组成的协方差矩阵。表示如下：
其中，Serr代表位移误差，Verr代表速度误差,对角线上就是各自的方差。
把前面得到的估计值代入这里能够化简得：
(1)式
同理，能够得到预测值和真实值之间误差的协方差矩阵：
注意到系统状态x变量和测量噪声之间是相互独立的。于是展开（1）式可得：
最后得到：
继续展开：
接下来最小均方差开始正式登场了，回忆之前提到的，协方差矩阵的对角线元素就是方差。这样一来，把矩阵P的对角线元素求和，用字母T来表示这种算子，他的学名叫矩阵的迹。
最小均方差就是使得上式最小，对未知量K求导,令导函数等于0，就能找到K的值。
注意这个计算式K，转换矩阵H式常数，测量噪声协方差R也是常数。因此K的大小将与预测值的误差协方差有关。不妨进一步假设，上面式子中的矩阵维数都是1*1大小的，并假设H=1，不等于0。那么K可以写成如下：
所以越大，那么K就越大，权重将更加重视反馈,如果
等于0，也就是预测值和真实值相等，那么K=0，估计值就等于预测值（先验）。
将计算出的这个K反代入Pk中，就能简化Pk，估计协方差矩阵Pk的：
因此递推公式中每一步的K就计算出来了，同时每一步的估计协方差也能计算出来。但K的公式中好像又多了一个我们还未曾计算出来的东西,他称之为预测值和真实值之间误差的协方差矩阵。它的递推计算如下：
请先注意到预测值的递推形式是：
由于系统状态变量和噪声之间是独立，故可以写成：
由此也得到了的递推公式。因此我们只需设定最初的
,就能不断递推下去。
这里总结下递推的过程，理一下思路：
首先要计算预测值、预测值和真实值之间误差协方差矩阵。
有了这两个就能计算卡尔曼增益K，再然后得到估计值，
最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。
至此，卡尔曼滤波的理论推导到此结束。还有一些如实际应用中状态方程建立不正确，预测结果会怎样等这样的细节问题，以及一些总结留到第二部分讨论。
第二部分
这部分主要是通过对第一部分中提到的匀加速小车模型进行位移预测。
先来看看状态方程能建立准确的时候，状态方程见第一部分分割线以后内容，小车做匀加速运动的位移的预测仿真如下。
- clc
- clear all
- close all
- % 初始化参数
- delta_t=0.1; %采样时间
- t=0:delta_t:5;
- N = length(t); % 序列的长度
- sz = [2,N]; % 信号需开辟的内存空间大小 2行*N列 2:为状态向量的维数n
- g=10; %加速度值
- x=1/2*g*t.^2; %实际真实位置
- z = x + sqrt(10).*randn(1,N); % 测量时加入测量白噪声
- Q =[0 0;0 9e-1]; %假设建立的模型 噪声方差叠加在速度上 大小为n*n方阵 n=状态向量的维数
- R = 10; % 位置测量方差估计，可以改变它来看不同效果 m*m m=z(i)的维数
- A=[1 delta_t;0 1]; % n*n
- B=[1/2*delta_t^2;delta_t];
- H=[1,0]; % m*n
- n=size(Q); %n为一个1*2的向量 Q为方阵
- m=size(R);
- % 分配空间
- xhat=zeros(sz); % x的后验估计
- P=zeros(n); % 后验方差估计 n*n
- xhatminus=zeros(sz); % x的先验估计
- Pminus=zeros(n); % n*n
- K=zeros(n(1),m(1)); % Kalman增益 n*m
- I=eye(n);
- % 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
- for k = 9:N %假设车子已经运动9个delta_T了，我们才开始估计
- % 时间更新过程
- xhatminus(:,k) = A*xhat(:,k-1)+B*g;
- Pminus= A*P*A'+Q;
- % 测量更新过程
- K = Pminus*H'*inv( H*Pminus*H'+R );
- xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));
- P = (I-K*H)*Pminus;
- end
- figure
- plot(t,z);
- hold on
- plot(t,xhat(1,:),'r-')
- plot(t,x(1,:),'g-');
- legend('含有噪声的测量', '后验估计', '真值');
- xlabel('Iteration');
绿线为真实值，蓝色的为噪声很大的测量值，红线为估计值。由此可以看出卡尔曼滤波确实相当犀利，提供了一个顺滑的最优的估计。并请注意代码中，特地使得估计是从第9个才开始预测,就像雷达跟踪一样，假设一开始我们没有发现这个东西，它已经运行了一段时间，我们在雷达测量和自己预测后得到估计结果，从图中可看出效果确实很好。
但这里请注意图像中画红圈部分，由于一开始你预测值为0，而实际上不是（它已经运动9个时间间隔了），所以估计出的效果不好。在这里回忆前面讨论过的K值大小和估计的关系，既然预测不准，那么一开始我就先相信测量呗。这就涉及估计值误差协方差初值的选取。在第一部分中我们知道卡尔曼增益K与预测误差协方差矩阵正相关，由第一部分推导知道预测误差协方差阵
：
它又和估计误差协方差矩阵有关，在Q，A确定的情况下，
和
成正比。所以如果我们给的
初值大的话，那么递推第一步中计算出的卡尔曼增益K就大。K大意味着更相信测量值。
修改初值P=[2 0;0 2],估计图像如下，可以看到初始估计明显改进了。（两幅图中，测量值相同，只改变了P）。这幅图中红色水平线那部分是前9个时间段，你还没开始雷达追踪，所以是水平的为0。
好了，到第二个问题，当状态方程建立不正确的又会怎样呢？实际应用中很多时候我们不能建立正确的状态方程。
我们假设建立的状态方程如下：
转换矩阵A,B,H都等于1.这个模型明显是不正确的。
注意这个时候的系统噪声，就不单单只是系统内部产生的，还包括你建立状态方程的不正确性。你建立的越不正确，根据你模型进行的预测就不正确，从这个角度来说，相当于你的噪声增大了。所以这个时候系统噪声W的方差应该增大。理解这一点，对改进实际估计效果有好处。接下来通过对比不同的W方差值设定给出对比，贴出这部分仿真如下。
- clc
- clear
- close all
- % 初始化参数
- delta_t=0.1;
- t=0:delta_t:5;
- g=10;%加速度值
- n_iter = length(t); % 序列的长度
- sz = [n_iter, 1]; % 信号需开辟的内存空间大小
- x=1/2*g*t.^2;
- x=x';
- z = x + sqrt(10).*randn(sz); % 测量时加入测量白噪声
- Q = 0.9; % 过程激励噪声方差
- %注意Q值得改变 待会增大到2，看看效果。对比看效果时，修改代码不要改变z的值
- R = 10; % 测量方差估计，可以改变它来看不同效果
- % 分配空间
- xhat=zeros(sz); % x的后验估计
- P=zeros(sz); % 后验方差估计
- xhatminus=zeros(sz); % x的先验估计
- Pminus=zeros(sz); % 先验方差估计
- K=zeros(sz); % Kalman增益
- % 估计的初始值
- xhat(1) = 0.0;
- P = 1.0;
- for k = 2:n_iter %
- % 时间更新过程
- xhatminus(k) = xhat(k-1);
- Pminus(k) = P(k-1)+Q;
- % 测量更新过程
- K(k) = Pminus(k)/( Pminus(k)+R );
- xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
- P(k) = (1-K(k))*Pminus(k);
- end
- figure
- plot(t,z);
- hold on
- plot(t,xhat,'r-')
- plot(t,x,'g-');
- legend('含有噪声的测量', '后验估计', '真值');
- xlabel('Iteration');
图(a)
图(b)
两个图中测量值是一样的，只是第二图中将系统噪声方差Q增大到2。对比可以看出，特别是图像后半段，图(b)比图(a)效果更接近真实值。
至此，从推导到应用接近尾声了，但我在这里还有一个问题就是，你随便给的x的预测初值，模型建立也不正确，kalman filter 竟然依然这么犀利，那么他收敛性怎么证明呢？写这文章的时候，笔者没有看详细的数学证明，但是由前面说到的kalman filter和数值分析里递推求解方程组时用的Gauss-Seidel 迭代法，两者真的很相近，于是我直观的认为卡尔曼的收敛性和Gauss-Seidel 一样。Gauss-Seidel迭代法里权重的选取能使得递推收敛真实值，因此卡尔曼滤波里增益K的每次计算就是卡尔曼收敛的重要保证。
最后再说一句个人可能不正确观点，抛砖迎玉，卡尔曼滤波最后收敛得到的方程就是维纳滤波，卡尔曼滤波是一步一步递推然后收敛到真实值，维纳滤波是直接计算出估计值，不是一步一步的结果，但两者都是最小均方差的思想在里面。因此，我在学卡尔曼的时候，想到数字图像里的维纳滤波，直观的想到，维纳滤波能做到的，卡尔曼应该也能做到。但这我也没去验证，倒是确实有这方面的论文。
全文完
（转载请注明作者和出处：http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）
reference：
1.Greg Welch & Gary Bishop. << An Introduction to the Kalman Filter >>
2.Tony Lacey. << Tutorial:The Kalman Filter >>.
3.Ramsey Faragher. << Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation >>
4.highgear . 《授之以渔：卡尔曼滤波器...大泄蜜...》
5.很多概念定义来自*