卡尔曼滤波(Kalman filter) 含详细数学推导
概述
总的来说，卡尔曼滤波器是一个状态估计器，它利用 传感器融合 、 信息融合 来提高系统的精度。通常，我们要观测一个系统的状态，有两种手段。一种是通过系统的 状态转移方程， 并结合上一时刻的状态推得下一时刻的状态。一种是借助 辅助系统（量测系统） 的测量得到系统状态。这两种方式都有各自的不确定性，卡尔曼滤波可以将这两者做到最优结合（加权平均），使得我们估计的状态的不确定性小于其中任何一种。所以权重的选择至关重要，它意味着我们更信任哪一种方式得出的状态（当然是更加信任 不确定性较小 的状态）。
建模
比如，我们观测一辆小车的 速度 和 位置 （所以我们要观察的状态就是速度 v 和位置 p ，即状态 x=(p,v) ,这里的 p,v 都是 向量 ）,当我们有了 k-1 时刻的状态 x_{k-1}=(p_{k-1},v_{k-1}) 后，那么如果速度不变，我们就可以得到下一时刻的状态 x_k=(p_k,v_k) ,其中 p_k=p_{k-1}+(t_k-t_{k-1})v_{k-1} ， v_k=v_{k-1} 。
当然，我们也可以对小车进行控制，比如让小车加速、拐弯等等，这些都可以用一个加速度来表示： a_k ，这样，下一时刻状态 x_k=(p_k,v_k) 的表示变为：
p_k=p_{k-1}+(t_k-t_{k-1})v_{k-1}+\frac{1}{2}a_k(t_k-t_{k-1})^2
v_k=v_{k-1}+(t_k-t_{k-1})a_k
将 (t_k-t_{k-1}) 写成 \Delta t ，化简为：
p_k=p_{k-1}+v_{k-1}\Delta t+\frac{1}{2}a_k(\Delta t)^2
v_k=v_{k-1}+a_k\Delta t
写成状态转移的形式：
x_k=F_kx_{k-1}+G_ka_k （1）
其中：
F_k=\begin{bmatrix} 1 & \Delta t\\ 0&1 \end{bmatrix} 被称为 状态转移模型 （ 状态转移矩阵 ）
G_k=\begin{bmatrix} \frac{1}{2}\Delta t^2\\ \Delta t \end{bmatrix} 被称为 控制输入模型， 他将我们的 控制输入 转变为 状态的改变。
但是，现实的情况并没有那么理想，小车可能会受到外界的各种扰动，比如，轮胎打滑，地面崎岖等等，都会使得状态转移的过程中 混入噪音干扰（过程噪声） ，而且这个噪音往往是不可被测量的，也就是说，没办法通过建模消除噪音项。不过，往往我们可以认为这个噪音是服从零均值的高斯分布的。那么（1）式重写为
x_k=F_kx_{k-1}+G_ku_k+w_k （2）
其中 u_k 是控制输入，在本例中即为 a_k 。 w_k\sim N(0,Q_k) ， Q_k 为其协方差矩阵。
上面的状态转移方程为我们提供了观察系统状态的一种方式，同时，我们可能有一些辅助系统，比如对于这个小车，我们可以用GPS作为得到其状态的另一种方式。
那样的话，GPS每一时刻都可以提供一次当前状态的观测值 z_k ,它与真实状态 x_k 的关系为， z_k=H_kx_k+v_k （3）
其中的 H_k 是观测模型，它把系统 真实状态空间 映射成 观测空间， v_k 是噪声项，称为 观测噪声 ——因为任何的测量系统都是有误差的，所以观测值实际上是真实值与噪声的叠加。我们同样可以认为此噪声是服从0均值的高斯分布的。即
v_k\sim N(0,R_k) , R_k 为其协方差矩阵。
现在我们比较这两种方式得到的系统状态，容易想到，状态转移方程得到的系统状态在演变时会非常平滑，而它的不确定度会随着迭代的进行而逐渐增大，因为误差会在一次次迭代的过程中不断累积（具体反映为估计状态的方差越来越大）。相反，由量测系统得到的状态不存在累积误差，但演变时也会很不平滑。这时我们就需要将两者得到的状态有效结合起来。这就是卡尔曼滤波做的事情了。
卡尔曼滤波
利用我们之前建立的模型（（2）和（3）），假设对于k-1时刻我们对系统有一个估计值 \hat{x}_{k-1|k-1} ,这个估计值的协方差为 \hat{P}_{k-1|k-1} ，则利用状态转移方程我们得到 预测状态
\hat{x}_{k|k-1}=F_k\hat{x}_{k-1|k-1}+B_ku_k （4）
这个预测状态的协方差 P_{k|k-1} 为：
cov(\hat{x}_{k|k-1})=cov(F_k\hat{x}_{k-1|k-1})+cov(B_ku_k)+cov(w_k)
=F_kcov(\hat{x}_{k-1|k-1})F_k^t+0+Q_k
=F_kP_{k-1|k-1}F_k^t+Q_k
然后我们要算出测量残差：
y_k=z_k-H_k\hat{x}_{k|k-1}
直观上理解 y_k ：如果使用状态转移方程计算出预测量 \hat{x}_{k|k-1} ，应该得到的观测值为 H_k\hat{x}_{k|k-1} ，而我们实际的观测值为 z_k ，所以这个 y_k 衡量的是我们的测量值与预测值的偏差， y_k 的协方差 S_k 为
cov(y_k)=cov(z_k)+cov(H_k\hat{x}_{k|k-1})
=cov(H_kx_k+v_k)+H_kcov(\hat{x}_{k|k-1})H_k^t
=cov(H_kx_k)+cov(v_k)+H_kcov(\hat{x}_{k|k-1})H_k^t
第一项为0，因为 x_k 代表k时刻系统状态的真实值，是一个固定值，没有协方差。所以简化为：
R_k+H_kP_{k|k-1}H_k^t
最后，我们得到的后验估计 \hat{x}_{k|k} 为：
\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_ky_k
其中 K_k 为 卡尔曼增益， 我们要选取最优卡尔曼增益使得估计值的均方误差达到最小，均方误差表达为 E({|x_k-\hat{x}_{k|k}|}^2) ,
而 \hat{x}_{k|k} 的协方差 P_{k|k} 为
cov(\hat{x}_{k|k})=cov(x_k-\hat{x}_{k|k})=E((x_k-\hat{x}_{k|k})(x_k-\hat{x}_{k|k})^t) （这一步可能有点突兀，最后会做补充）
所以 E({|x_k-\hat{x}_{k|k}|}^2)= tr(P_{k|k})
P_{k|k}=cov(x_k-\hat{x}_{k|k})
=cov(x_k-(\hat{x}_{k|k-1}+K_ky_k))
=cov(x_k-(\hat{x}_{k|k-1}+K_k(z_k-H_k\hat{x}_{k|k-1})))
=cov(x_k-(\hat{x}_{k|k-1}+K_k(H_kx_k+v_k-H_k\hat{x}_{k|k-1})))
=cov((I-K_kH_k)(x_k-\hat{x}_{k|k-1})-K_kv_k)
=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^t+K_kR_kK_k^t
=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}(K_kH_k)^t+K_kH_kP_{k|k-1}(K_kH_k)^t+K_kR_kK_k^t
=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}H_k^tK_k^t+K_k(H_kP_{k|k-1}H_k^t+R_k)K_k^t
=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}H_k^tK_k^t+K_kS_kK_k^t
因为有： \frac{d(tr(BAC))}{dA}=B^TC^T ，所以
\frac{d(tr(P_{k|k}))}{dK_k}
=-\frac{d(tr(K_kH_kP_{k|k-1}))}{dK_k}-\frac{d(tr(P_{k|k-1}H_k^tK_k^t))}{dK_k}+\frac{d(tr(K_kS_kK_k^t))}{dK_k}
其中，第一项 -\frac{d(tr(K_kH_kP_{k|k-1}))}{dK_k}=-(H_kP_{k|k-1})^T
第二项 -\frac{d(tr(P_{k|k-1}H_k^tK_k^t))}{dK_k} 需要注意 的是，因为 tr(A) 求的是矩阵的对角阵元素之和，所以 tr(A)=tr(A^T) ，所以可以先将第二项括号内的式子转置得到： -\frac{d(tr(K_kH_kP_{k|k-1}^T))}{dK_k} ，而协方差矩阵 P_{k|k-1} 又是对称矩阵，所以可以化为 -\frac{d(tr(K_kH_kP_{k|k-1}))}{dK_k}=-(H_kP_{k|k-1})^T
第三项 \frac{d(tr(K_kS_kK_k^t))}{dK_k}=K_kS_k+K_kS_k^T ， S_k 也为对称矩阵,所以可化为 K_kS_k+K_kS_k=2K_kS_k
综上， \frac{d(tr(P_{k|k}))}{dK_k}=-2(H_kP_{k|k-1})^T+2K_kS_k
令导数为0，即 -2(H_kP_{k|k-1})^T+2K_kS_k=0 ，解得
K_k=P_{k|k-1}^TH_k^TS_k^{-1}
K_k 即为所求卡尔曼增益，用它作为更新(修正)状态的权值(即 \hat{x}_{k|k}=\hat{x}_{k|k-1}+K_ky_k )可以获得最小均方误差。
所以最优估计 \hat{x}_{k|k} 就产生了，这时，再用
P_{k|k}=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^t+K_kR_kK_k^t
更新 最优估计的协方差矩阵 P_{k|k} 即可，这样一次迭代就结束了。然后将 \hat{x}_{k|k} 和 P_{k|k} 作为下一时刻的状态值即可继续迭代。
补充内容：
cov(\hat{x}_{k|k}) 这种写法是一种简写，本来是 cov(\hat{x}_{k|k},\hat{x}_{k|k}) ，就是 \hat{x}_{k|k} 跟自己的协方差。而 x_k 作为真实状态，可认为是一个常量，所以令 \hat{x}_{k|k} 加上一个常量不影响其协方差，所以才能化为 cov(x_k-\hat{x}_{k|k}) 。
那么为什么可以化为 E((x_k-\hat{x}_{k|k})(x_k-\hat{x}_{k|k})^t) 呢？
按照协方差的定义 Cov(a,b)=E(ab)-E(a)E(b) ，本该化为 E((x_k-\hat{x}_{k|k})(x_k-\hat{x}_{k|k})^t)-E(x_k-\hat{x}_{k|k})E((x_k-\hat{x}_{k|k})^t)
但是 \hat{x}_{k|k} 本身相当于 x_k 的基础上加上一个0均值高斯噪声(只要状态转移方程准确，且初值 \hat{x}_{0|0}=x_{0|0} )，所以 E(\hat{x}_{k|k})=E(x_k) ,那么 E(x_k-\hat{x}_{k|k})=E((x_k-\hat{x}_{k|k})^t)=0 ，所以后一项没有了，才变成 E((x_k-\hat{x}_{k|k})(x_k-\hat{x}_{k|k})^t)
最后，有人问到了加减号的问题，对于
y_k=z_k-H_k\hat{x}_{k|k-1}
cov(a+b)=cov(a+b,a+b)
=cov(a,a)+cov(a,b)+cov(b,a)+cov(b,b)
在a和b独立的情况下， cov(b,a)=cov(a,b)=0
即化为 cov(a,a)+cov(b,b) ，所以，无论a或b前面是正号还是正好都是一样的(因为 cov(a,a)=cov(-a,-a) )，而在本例中 z_k 和 H_k\hat{x}_{k|k-1} 也是独立的，所以化为 cov(z_k)+cov(H_k\hat{x}_{k|k-1}) 。
另外，直观上理解：协方差矩阵代表的是数据分布的一种不确定度，这种不确定度本身不具有所谓的正负号，不可能因为加减而有所抵消。
