关于Kalman滤波器的理解

写在本文之前，有两个重要的思想需贯穿脑海之中：  

 1. 没有绝对准确，只有更为接近的准确  
 2. 滤波即为加权  关于1，这个不做多说。  

关于2，对信号的滤波即是对离散序列的加权，传统的低通滤波器可以理解为高频权值为0（或接近0），而低频的权值为1，此时便可实现通低频阻高频的效果。同理，高通、带通滤波器可以理解为对不同频段的信号进行加权后获得想要的信号。  

———-  

 1 基本概念

 1.1 一些概念

设X、Y为随机变量，μ、ν分别为X、Y的期望。x、y分别为X、Y的真实值，x^、y^分别为x、y的估计值，则有：

误差：真实值与估计值的差，即e=x−x^
方差：方差是描述随机变量与其期望值的离散程度，即σ2X=E(X−E(X))
协方差：描述两个变量间的相关性，cov(X,Y)=E[(X−μ)(Y−ν)] 。其中，X与Y的不相关是独立的必要不充分条件，但是对于X、Y正态分布时二者独立是不相关的充要条件。
协方差矩阵：若X为m维向量X=(X1,X2,…,Xm)，其中Xk中含一组数据；Y为n维向量Y=(Y1,Y2,…,Yn)，其中Yk中含一组数据,则X与Y的协方差应表示为mxn维协方差矩阵：

cov(X,Y)=E((X−μ)(Y−ν)T)=⎡⎣⎢⎢⎢⎢⎢E[(X1−μ1)(Y1−ν1)]E[(X2−μ2)(Y1−ν1)]⋮E[(Xm−μm)(Y1−ν1)]E[(X1−μ1)(Y2−ν2)]E[(X2−μ2)(Y2−ν2)]⋮E[(Xm−μm)(Y2−ν2)]⋯⋯⋱⋯E[(X1−μ1)(Yn−νn)]E[(X2−μ2)(Yn−νn)]⋮E[(Xm−μm)(Yn−νn)]⎤⎦⎥⎥⎥⎥⎥

对于X自身而言，其协方差矩阵(或称为方差，详见[wiki-协方差矩阵](https://zh.wikipedia.org/wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE%E7%9F%A9%E9%98%B5 “wiki-协方差矩阵”))：

cov(X,X)=E((X−μ)(X−μ)T)=⎡⎣⎢⎢⎢⎢⎢E[(X1−μ1)(X1−μ1)]E[(X2−μ2)(X1−μ1)]⋮E[(Xm−μm)(X1−μ1)]E[(X1−μ1)(X2−μ2)]E[(X2−μ2)(X2−μ2)]⋮E[(Xm−μm)(X2−μ2)]⋯⋯⋱⋯E[(X1−μ1)(Xn−μn)]E[(X2−μ2)(Xn−μn)]⋮E[(Xm−μm)(Xn−μn)]⎤⎦⎥⎥⎥⎥⎥

可以从上式可以看出，矩阵对角为X列向量的方差。若X服从正态分布，其为对角矩阵。（后面可将X联想为状态向量，X1、X2联想为位移向量和速度向量）

误差的协方差矩阵：若X为m维向量X=(X1,X2,…,Xm)，其中Xk中含一组数据，误差为m维向量e，则其误差的协方差矩阵为：

P=cov(e,e)=E[eeT]

可知，其对角元素元素之和（或称迹）即为均方差。

 注：此处十分重要，将状态变量的误差协方差矩阵与状态变量的最小均方差估计联系起来。

 1.2 最小均方误差估计

均方误差：它是”误差”的平方的，也就是多个样本的时候，均方差等于每个样本的误差平方再乘以该样本出现的概率的和，即

MSE=E[(x−x^)2]=E[e2]

若X为m维向量X=(X1,X2,…,Xm)，其中Xk中含一组数据，则X的均方误差：

MSE=E[(x−x^)T(x−x^)]=E[eTe]=tr(E[eeT])=∑i=1mE[e2i]

注意，这里的e为m维向量，X的均方误差即为误差平方和的期望值，或误差平方的期望之和，这即是误差协方差矩阵的迹。

最小均方差（MMSE）：对于变量\mathrm{X}，其最小均方误差即为：

MMSE=MSEmin=E[(x−x^)T(x−x^)]=E[eTe]=tr(E[eeT])=∑i=1mE[e2i]

最小均方误差估计：最小均方误差估计即最优估计，即寻找合适的估计函数x^=c(x)来估计x，使上式最小。
（1） X为一维变量时，该估计函数为：

x^=E[X]

其证明详见参考文献，大致过程是通过MSEmin对x^求导，令导函数为0，即可求得x^ 。    
（2） 当X存在先验条件Y时，该估计函数为：

x^=E[X|Y=y]

此时，该估计函数即为X在Y条件下的条件期望。其证明详见参考文献，证明过程同上。
（3） 当X存在先验条件Y，且X、Y为m为向量时，该估计函数为：

x^(x1,x1,⋅⋅⋅,xm)=E[X|Y1=y1,Y2=y2,⋅⋅⋅,Ym=ym]

 1.3 状态空间  

根据牛顿运动规律，物体的运动方程大体归结为化成如下形式：

md2xdt2+2βdxdt+kx=f(t)

  
化成以下形式：  

x¨−a1x˙−a0x=f(t)

  
再写成方程组形式：  

{x˙=0⋅x+1⋅x˙+0⋅f(t)x¨=a0x+a1x˙+1⋅f(t)

  
其矩阵形式：  

(x˙x¨)=(0a01a1)( xx˙)+( 0 1)f(t)

  
化简为：  

x˙=Fx+Gf

  
如果在方程中加入过程噪声，则有：  

x¨=a1x˙+a0x+f(t)+w(t)

  
得到类似矩阵：  

(x˙x¨)=(0a01a1)( xx˙)+( 0 1)f(t)+( 0 1)w(t)

  
简化为：  

x˙=Fx+Gf+w

  
其中，F：传输矩阵； x：状态矢量； G：控制矩阵； f：控制矢量； w：过程噪声  

在这里，状态向量x=（x,x˙）包含位移和速度的信息。推广到多维空间，状态向量为x=（x1,x2,...,xn;x1˙,x2˙,...,xn˙），这种方法称为状态空间描述法。  

---

 2 Kalman滤波器

 2.1 前提条件  

- 线性系统  
- 系统噪声和测量噪声服从高斯分布  

 2.2 系统模型  

卡尔曼滤波建立在线性代数和隐马尔可夫模型上，k时刻的状态在(k-1)时刻的基础上递推过来，系统模型由预测空间模型和观测空间模型组成。

预测空间模型：  

xk=Axk−1+Buk−1+wk−1

  
其中，  
- xk是k时刻的预测状态向量，xk−1是(k-1)时刻的状态向量  
- uk−1是(k-1)时刻的控制输入向量
- A是k时刻状态传输矩阵，与系统本身特性相关，其隐含指示(k-1)时刻的状态会影响到k时刻的状态  
- B是k时刻控制输入矩阵，其隐含指示(k-1)时刻的驱动输入会影响到k时刻的状态  
- wk是过程噪声，服从正态分布，wk∼N(0,Qk)，Qk为其协方差矩阵  

观测空间模型：  

zk=Hxk+vk

  
其中，  
- zk是k时刻的观测状态向量  
- H是k时刻的观测矩阵，把真实状态空间映射成观测空间  
- vk是观测噪声，服从正态分布，vk∼N(0,Rk)，Rk为其协方差矩阵  

分析上述两个状态方程，不拿看出，在k时刻的预测状态向量xk（或观测状态向量zk）是在第(k-1)时刻的状态上推导而来（马尔科夫模型），并且都在状态更新的过程中带入新的高斯噪声。

那么问题来了，既然既可以通过预测，亦可以通过观测，来获知系统在k时刻的状态，但是二者都存在误差（噪声），那么二者哪个更可靠呢？这时，我们自然而然的想到了的加权的思想，通过对二者的加权获得k时刻的准确的状态。  通过以下图片可以帮我们更好地理解，小车距出发地的距离可以通过预测模型和测量模型获得，且二者都服从正态分布，都存在误差。通过对二者加权，得到我们想要的较为准确的状态模型（图中绿色表示），但是，这个模型也存在误差，服从正态分布。  
  
  
  
  

综上，kalman滤波即是对预测模型和观测模型进行加权处理，获取更加接近实际值新的状态估计值。实际上，kalman滤波即是利用观测模型的残差来修正预测模型（最优估计），同时计算残差的权值。  

 2.3 递推方程

kalman滤波器用于估计离散时间过程的状态变量x∈Rn，这个离散时间过程由以下离散随机差分方程描述：

设x−k∈Rn(-代表先验，^代表估计)为在已知第 k步以前状态情况下第 k 步的先验状态估计。x^k∈Rn为已知测量变量zk时第k步的后验状态估计。由此定义先验估计误差和后验估计误差：

e−k=xk−x^−k,ek=xk−x^k

先验估计误差的协方差为：

P−k=E[e−k(e−k)T]

后验估计误差的协方差为：

Pk=E[ek(ek)T]

那么，对于预测和测量状态空间方程，即为：
预测空间模型-预测值（先验估计）：

x^−k=Ax^k−1+Bu^k−1

观测空间模型-测量值（测量值的预测）：  

z^k=Hx^−k

 实际测量值为：zk=Hx^−k+vk=z^k+vk

Kalman滤波器：
这时，要在测量值的基础上再次更新先验估计，可得到后验估计，得到最优估计x^k：

x^k=x^−k+Kk(zk−Hx^−k)

上式构造了kalman滤波器的表达式，先验估计x^−k和加权的预测残差(zk−Hx^−k)线性的构成了后验状态估计x^k。

 (1) xk 的概率原型。这个可以见参考文献，来源于贝叶斯规则：x^k的更新取决于在已知先前的测量变量zk的情况下x^k的先验估计x^−k的概率分布。在已知 zk的情况下，x^k的分布可写为：

p(xk|zk)−N(x^k,Pk)

即为正态分布。

 (2) x^k的推导。关于x^k的形式，这里应该是利用了反馈的原理（如下面这张图），或是最优估计的原理，在知乎上看到有说用极大似然估计可以推导，但是，具体推导还不是很清楚。在论文中还看到过一种是将预测和测量的正态分布相乘求联合正态分布，联合正态分布即为后验状态估计。

 (3) Kk值的推导。既然x^k是xk的最优估计，那么xk的误差协方差矩阵的迹tr[Pk](Pk的计算见下面)最小即xk的均方误差最小。此时，将后验估计x^k的表达式和误差协方差矩阵Pk的表达式联立，即可求得tr[Pk]，对其求导，导函数为零时求得Kk

Kk=P−kHT(HP−kHT+R)−1=P−kHTHP−kHT+R

还有另外一种表达形式，这里是对k时刻的测量值和（k-1）时刻状态估计值进行加权

 2.4 离散Kalman滤波器算法

卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计某一时刻的过程状态，然后以测量变量（含噪声）的方式获得反馈。

因此卡尔曼滤波器可分为两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈，它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估－校正算法。

离散卡尔曼滤波时间更新方程

{x^−k=Ax^k−1+Buk−1P−k=APk−1AT+Q

时间更新方程主要获取由预测方程预测的k时刻的状态x^−k，为x^k的先验状态估计；此时，其误差的协方差矩阵为P−k。

 上式中，存在未知参数A, B, P−k, Pk−1, Q
(1) 关于A。这是和状态向量相关的矩阵，具体见举例
(2) 关于B。这是驱动矩阵，和外部输入驱动有关，具体见举例
(3) 关于 P−k。这是k时刻的先验状态估计的误差协方差矩阵，与预测方程的噪声有关，是根据（k-1）时刻的误差协方差矩阵Pk−1递推而来（Pk−1一般是给定的初值），利用P−k的表达式 P−k=E[e−k(e−k)T]即可求得。
(4) 关于Pk−1。（k-1）时刻的误差协方差矩阵（Pk−1一般是给定的初值）
(5) 关于Q。过程噪声（预测误差）wk−1的协方差矩阵，在预测的过程中产生。

离散卡尔曼滤波状态更新方程

⎧⎩⎨⎪⎪Kk=P−kHT(HP−kHT+R)−1x^k=x^−k+Kk(zk−Hx^−k)Pk=(I−KkH)P−k

状态更新方程在预测的先验估计x^−k上，同测量值zk进行融合，更新校正先验状态估计，得到k时刻的后验状态估计x^k；此时，递推出后验状态估计的误差协方差矩阵Pk，由此可以得到MMSE的条件，求得Kk

 上式中，存在未知参数Kk, H, R, zk, Pk
(1) 关于Kk。残差（真实测量值与基于先验状态估计的测量值（估计量））的增益，可以看做测量反馈的增益
(2) 关于H。这是观测矩阵，和状态向量有关，具体见举例
(3) 关于R。测量噪声（测量误差）vk−1的协方差矩阵，在测量的过程中产生。 
(4) 关于zk。k时刻的真是测量值。
(5) 关于Pk。k时刻的后验估计的误差协方差矩阵。

因此，通过时间更新和测量更新的不断递推，便可得到比较准确的状态向量估计值。其中，若初始过程噪声wk和测量噪声vk为一确定值，不断递推后状态向量误差协方差矩阵Pk和卡尔曼增益Kk会收敛并保持常量。

———  

 3 程序  

 3.1 Matlab程序  

% kalman filter% x(k+1) = Fk * x(k) + Wk; 预测模型% y(k)   = Hk * x(k) + Vk; 观测模型N = 365;Fk = [1];               % 状态转移矩阵               X  = zeros(N,1);        % 初始化状态变量W  = 12*randn(N,1);     % 构造过程噪声X(1) = 100;             % 初始状态                    for k = 2:N    % 状态方程    X(k) = Fk * X(k-1) + W(k-1);endHk = [2];               % 观测矩阵                Y  = zeros(N,1);        % 初始化观测变量V  = 20*randn(N,1);     % 构造观测噪声for k = 1:N    % 观测方程    Y(k) = Hk * X(k) + V(k);   endQ  = cov(W);            % 过程噪声协方差；    R  = cov(V);            % 观测噪声协方差；          Xupdate = zeros(N,1);   Xupdate(1) = Y(1);      % 初始化第一个，也就是观测到的第一个 Pupdate(1) = 0;for k = 2:N    % 五大核心方程    Xpredict(k) = Fk * Xupdate(k-1);    Ppredict(k) = Fk * Pupdate(k-1) * Fk' + Q;    K = Ppredict(k) * Hk * (Hk * Ppredict(k) * Hk' + R).^-1;    Xupdate(k) = (1 - K * Hk) * Xpredict(k) + K * Y(k);    Pupdate(k) = (1 - K * Hk) * Ppredict(k);endplot(1:N,X,'B','linewidth',2);hold on;plot(1:N,Y,'K','linewidth',2);hold on;plot(1:N,Xupdate,'R','linewidth',2);hold on;plot(1:N,abs(Xupdate-X),'m','linewidth',2);hold off;legend('真值','观测值','滤波值','误差')

 3.2 C程序  

———-  

参考  

- https://www.zhihu.com/question/23971601  
- http://blog.csdn.net/xiahouzuoxin/article/details/39582483  
- https://zhuanlan.zhihu.com/p/21889676  
- https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2  
- https://zh.wikipedia.org/wiki/%E6%9C%80%E5%B0%8F%E4%BA%8C%E4%B9%98%E6%B3%95
- http://www.jianshu.com/p/2768642e3abf
- http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
- Ramsey Faragher. Understanding the Basis of the Kalman Filter. 2012
- Greg Welch, Gary Bishop. An Introduction to the Kalman Filter. 2001
- Don Johnson. Minimum Mean Squared Error Estimators. 2004
- R.E.Kalman. A new approach to linear filtering and prediction problems. 1960