卡曼滤波推导
主要参考文献:MIT Kalman filter tutorial
ps: 有错误请指出。
1. 卡曼滤波推导的前言
首先，我接下来的推导是基于离散系统的（非连续系统）。
其次，卡曼滤波成立的条件是默认当前研究的离散系统是一个马尔可夫过程，即等同基于这个假设：当前的状态只与上一时刻的状态相关。马尔可夫过程是一类随机过程，它的原始模型是马尔可夫链。马尔可夫过程是研究离散事件动态系统状态空间的重要方法，它的数学基础是随机过程。
1.1. 离散系统状态空间（State Space）
状态空间是控制系统里的一个专业名词。状态空间代表着一个系统可以用矩阵来描述，输入输出可以用向量来描述以及系统状态更新可以用线性代数来描述，这一点在下方会以公式来说明。状态空间使得单输入单输出，多输入单输出或者多输入多输出的系统都可以有一个统一的描述方法。
1.2. 系统描述
如果一个离散系统是马尔可夫过程，那么系统的可以用状态空间来表述：
... ... (1)
... ... (2)
这里的
是一个nx1的向量，描述k+1时刻的系统状态量，而
描述的是k时刻的，
是一个nxn的矩阵，是状态传递矩阵。状态传递矩阵是要你基于对系统的了解，把以及每一元素之间的关系描述出来。
是nx1的向量，被称作为k时刻的过程噪声。这个过程噪声是系统与生俱来的，在状态更新时需要加上去（ps:噪声和误差的区别：噪声是指自然存在的，误差或估算误差产生于人类的信息不足或者对系统的理解不足）。这里我们需要假设这个噪声是时间上平稳的，也就是噪声在任何一段时间里都可以以相同的方法描述。例如，多数情况下我们都把这个噪声描述为高斯分布，且这个高斯分布的均值和方差都不随时间而变化。同时，我们通常假设这个噪声是白噪声。至于为什么我也想问。
是一个mx1的向量，是k时刻的观测量，而H矩阵是一个mxn的矩阵，是状态观测转换矩阵。这个转换矩阵是用来描述与里每一元素之间的关系。
是mx1的向量，被称作k时刻的观测噪声。同样的，我们假设观测噪声是时间上平稳的，是白噪声。还有，与过程噪声无相关性。
2. 卡曼滤波推导
2.1. 卡曼滤波的目标min(T[E(
)])
卡曼滤波其实本质上是一个均方误差减少器（mean squared error minimizer）。
让我们把我们对状态量的估计设为
，那么估计的误差： = - （nx1矩阵）。
那么卡曼滤波的任务就是减少估计的误差, 数学上表现为：min(E(
)+E(
)+...+E(
) ... ... (3)
(1) 也等于min(E(++...+)) = min(T[E()]) (T[ ]为求矩阵的迹)
让我们把 E() 设为
。我们把这个nxn的矩阵称作t时刻的状态估算误差协方差矩阵。
2.2. 更新状态量估算
根据对系统的了解，我们获得卡曼滤波器上一帧时刻对目前时刻的状态量的估计
(具体公式为
，详见章节2.5 和3) , 我们也称其为目前时刻的未验估计（prior estimate）。那么状态量的更新公式为：
= 
... ... (4)
此公式是被这样设定的，没有推导。
其中
我们称为新息
, 
称为卡曼增益。整一个卡曼滤波最重要就这个卡曼增益对状态量估算更新的帮助。而卡曼增益怎么算，我们会在下文讨论。
2.3. 计算卡曼增益
卡曼增益的设置就是为了完成对的减少。我们可以把的迹对的偏导等于0，即![\frac{dT[P_k]}{dK_k} = 0](http://img.e-com-net.com/image/info8/5306a97e45744a8fba1fbab03771b7e1.gif){kind=small}
，来求得什么对应着的迹的最小值。
那么的迹和之间的关系式是什么？推导如下：
首先，把(2)带入(4)，得到： = 
那么
那么![P_k=E[e_ke_k^T]](http://img.e-com-net.com/image/info8/204dd8bdc9414a309ad977ba481fdd71.gif){kind=small}
![=E[[(I-K_kH)(x_k-\widehat{x}_k')-K_kv_k][(I-K_kH)(x_k-\widehat{x}_k')-K_kv_k]^T]](http://img.e-com-net.com/image/info8/5c5788494baa4a96b4b76373e4a8b45c.gif){kind=small}
... ... (5)
因为
是先验估计的误差，而是是观测噪声，他们是uncorrelated，所以他们乘积的期望值为0。
(5) = ^T+K_kE[v_kv_k^T]K_k^T](http://img.e-com-net.com/image/info8/8c6413dfd1af40e4a1448223e05384dc.gif){kind=small}
因为当中的![E[(x_k-\widehat{x}_k')(x_k-\widehat{x}_k')^T]](http://img.e-com-net.com/image/info8/f479391fcc72499abc634ce25ef65ec3.gif){kind=small}
等于协方差的先验估计
，同时把![E[v_kv_k^T]](http://img.e-com-net.com/image/info8/c6e5be9c4b2b4276909b22567f1fbfb0.gif){kind=small}
设为 
矩阵（nxn矩阵）
那么
... ... (6)
让我们展开上式（6）,我们得到 
... ... (7)
对上式（7）,我们左右取矩阵的迹：![T[P_k] = T[P_k' - K_kHP_k' - P_k'H^TK_k^T+K_k(HP_k'H^T+R)K_k^T]](http://img.e-com-net.com/image/info8/f4fdc5b3f07e4506951f7e73993bbb82.gif){kind=small}
![=> T[P_k] = T[P_k' - K_kHP_k' - P_k'H^TK_k^T+K_k(HP_k'H^T+R)K_k^T]](http://img.e-com-net.com/image/info8/d4fef52530c4481d9f4753d072e1b240.gif){kind=small}
![=> T[P_k] = T[P_k'] - T[K_kHP_k'] - T[P_k'H^TK_k^T]+T[K_k(HP_k'H^T+R)K_k^T]](http://img.e-com-net.com/image/info8/393eb8f7a164498cac15583f5dfad86b.gif){kind=small}
因为
和都是nxn矩阵，同时又因，如果A和B都是正方形矩阵，![T[AB] = T[BA] = T[A^TB]=T[B^TA]](http://img.e-com-net.com/image/info8/f5dcb27836f747dbbb98b523ecb2b834.gif){kind=small}
点击查看维基百科说的，所以![T[(K_kH)P_k'] = T[P_k'(K_kH)^T] =T[P_k'H^TK_k^T]](http://img.e-com-net.com/image/info8/8d978b090f954e11a01389232be2ed5a.gif){kind=small}
。
![=> T[P_k] = T[P_k'] - 2T[K_kHP_k']+T[K_k(HP_k'H^T+R)K_k^T]](http://img.e-com-net.com/image/info8/34dd137c17ea49968b668e8b2c4641e9.gif){kind=small}
... ... (8)
我们在这一章节开头就说到我们推导的目标：把的迹对的偏导等于0，即，来求得什么对应着的迹的最小值。
所以，我们对式子（8）求对的偏导：
![\frac{dT[P_k]}{dK_k} = \frac{dT[P_k]'}{dK_k} - 2\frac{dT[K_kHP_k']}{dK_k} +\frac{dT[K_k(HP_k'H^T+R)K_k^T]}{dK_k}](http://img.e-com-net.com/image/info8/119d03fdb254495aa589d88930182b4b.gif){kind=small}
... ... (9)
根据维基百科我们知道：![\frac{dT[AB]}{dA}=\frac{dT[BA]}{dA}=B^T](http://img.e-com-net.com/image/info8/9f2201fc690d4d11931239b60502e1f0.gif){kind=small}
以及![\frac{dT[ABA^T]}{dA}=2AB](http://img.e-com-net.com/image/info8/f73127d66abb40ef93ebde2f1c68d421.gif){kind=small}
所以 ![\frac{dT[K_kHP_k']}{dK_k} = (HP_k')^T](http://img.e-com-net.com/image/info8/4a963d01345f4e209ac71cc1b72e9f02.gif){kind=small}
以及![\frac{dT[K_k(HP_k'H^T+R)K_k^T]}{dK_k}=2K_k(HP_k'H^T+R)](http://img.e-com-net.com/image/info8/47064562f0bf4c0a8e7ff6d8de0f9906.gif){kind=small}
那么（9）=> ![\frac{dT[P_k]}{dK_k} = -2(HP_k')^T+2K_k(HP_k'H^T+R)=0](http://img.e-com-net.com/image/info8/75aad51f7e354b5bb132728de8233a97.gif){kind=small}
=> 
=> 
=> 
... ... (10)
所以卡曼增益可利用公式（10）求得。
通过公式我们知道卡曼增益与协方差的先验估计，状态观测转换矩阵
以及观测噪声有关。
2.4 用卡曼增益来更新状态估算误差矩阵
我们把公式（10）代入到公式（6），得到状态估算误差矩阵
... ... (11)
2.5 用新的状态估算误差矩阵预测（投影）下一帧的状态估算误差矩阵的先验估计
下一帧的状态的先验估计等于当前状态投影 
那么下一帧误差的先验估计为：
就像章节2.1里面，我们把 E() 设为。这里，下一帧的状态估算误差矩阵的先验估计
= E(
) = E(
)
同时，为从开始到现在累积的误差，而为k时刻与k+1时刻间的过程噪声，它们之间无相关性，E(
) = 0.
那么 = ![E[\Phi e_k (\Phi e_k)^T] + E[w_k w_k^T]+2E[e_k w_k]](http://img.e-com-net.com/image/info8/66f13ca1896e4f6f826f1141bd331ff9.gif){kind=small}
... ... (12)
3. 总结
一个完整的迭代卡曼滤波器包含四个步骤：
a. 计算当前帧卡曼增益： (公式10）
b. 更新当前帧状态估计： =
c. 更新当前状态估算误差矩阵： 
(公式11）
d. 投影出下一帧的状态的先验估计（用于下次步骤b）：
投影出下一帧的估算误差矩阵的先验估计（用于下次步骤a,c）： 
(公式12）