大家好，又见面了，我是你们的朋友全栈君。

更新日志：

2020.03.20：修改了部分内容的表述方式，重做了实验并给出相应结果，补充了矩阵迹的求导公式；

2020.09.24：修改了推导中第四个公式的符号错误，第k时刻预测值应由上一时刻的估计结果推出，而非真实值；

1 简介

卡尔曼滤波（Kalman Filter）是一种利用线性系统状态方程，利用对系统的观测数据，对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响，所以系统状态的估计过程也可看作是滤波过程。应用场景之一有利用传感器跟踪感兴趣目标的位置，传感器获取的目标距离、速度、方位角等观测值往往含有噪声。卡尔曼滤波利用目标的动态信息与观测结果相结合，抑制噪声的影响，从而获得一个关于目标位置更准确的估计，这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

2 算法介绍

为了便于读者理解卡尔曼滤波的运作过程，这里先举个简单易懂的例子。

设想现在咱养了一头猪，一周前，这只猪的体重是46±0.5kg，这里所用的±0.5，表示其实对这只猪一周前的重量并不是那么确定的，也就是说，46kg这个重量有0.5kg的误差。现在，一周过去了，咱想要知道它此刻有多重，又大概有多少的误差？

为了确定此刻猪头的重量，一般咱采用的方法是拿个大体重秤去称量，假设现在体重秤给出的结果是49kg。可是这个时候旁边一个人告诉我们，这个体重秤不准，有±1kg的误差。按照人的第一反应，应该是会根据经验思考一下这个49kg到底准不准，按照经验，一周时间这猪头应该差不多长了2kg，所以根据经验判断应该是48kg，有±2kg的误差。怎么办？不管是经验的感性判断还是体重秤的理性判断，都有误差，这个时候想同时把这两种判断考虑到一起，给出一个比较可靠的估计值。之后，每周咱都得用这个不准的体重秤和经验去估计这头猪的重量，这就是卡尔曼滤波的过程。

标准的卡尔曼滤波系统方程如下：

${{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}$

${{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}$

上面的两个式子分别叫作状态转移方程和观测方程。其中 $\mathbf{A}$ 叫作状态转移矩阵，对应到例子中就是现在与一周前体重的转移系数，取值为1； $\mathbf{B}$ 和 ${{\mathbf{u}}_{k}}$ 是系统模型的参数，在例子中可以理解成一周内猪的固定增长量， $\mathbf{B}$ 可以取值为1， ${{\mathbf{u}}_{k}}$ 取恒值为2，这样一周就是增长2kg； $\mathbf{C}$ 叫作观测矩阵，对应到例子中的体重秤就是1； ${{\mathbf{s}}_{k}}$ 是状态转移噪声向量或者叫过程噪声，对应于例子中的±2kg的经验判断误差； ${{\mathbf{v}}_{k}}$ 是观测噪声向量，对应于例子中的±1kg的体重秤测量误差； ${{\mathbf{\theta }}_{k}}$ 是时刻的真实状态向量； $\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}$ 是时刻的预测值，对应于例子中48kg的经验判断； ${{\mathbf{z}}_{k}}$ 是时刻的观测向量，对应于例子中体重秤称得的重量49kg。

实际应用中， ${{\mathbf{\theta }}_{k}}$ 、 ${{\mathbf{z}}_{k}}$ 、 ${{\mathbf{s}}_{k}}$ 、 ${{\mathbf{v}}_{k}}$ 往往是列向量的形式，可以理解为同时对多个属性做滤波估计操作，例如同时对猪头此刻的体重、长度和高度做出估计（滤波）。状态转移噪声向量 ${{\mathbf{s}}_{k}}$ 一般是服从多维高斯分布的，其均值为 $\mathbf{0}$ 向量，协方差矩阵为 ${\mathbf{Q}}$ ；观测噪声向量 ${{\mathbf{v}}_{k}}$ 一般也是服从多维高斯分布的，其均值为0向量，协方差矩阵是 ${\mathbf{R}}$ 。最终滤波后得出的估计向量值用 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 表示。

那么卡尔曼滤波是怎么实现的？它分为预测（Predict，或者叫传播Propagation）和更新（Update）两个步骤：

Predict：

$\mathbf{\theta }_{k}^{'}=\mathbf{A}\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle+\mathbf{B}\mathbf{u}_{k-1}$

$\mathbf{\Sigma }_{k}^{'}=\mathbf{A}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{A}}^{T}}+\mathbf{Q}$

Update：

$\mathbf{S}_{k}^{'}={{\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)}^{-1}}$

$\mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}\mathbf{S}_{k}^{'}$

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-\mathbf{C\theta }_{k}^{'} \right)$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{C} \right)\mathbf{\Sigma }_{k}^{'}$

为了说明更简洁，咱先把上面的六个公式标为（1）~（6），理一下这个算法的思路：

首先矩阵 $\mathbf{A}$ 、 $\mathbf{C}$ 、 $\mathbf{B}$ 、 ${{\mathbf{u}}_{k}}$ 、 ${\mathbf{Q}}$ 、 ${\mathbf{R}}$ 是已知的，假设现在我们已经有一组数据了，例如从第1周到第50周猪头的观测值 ${{\mathbf{z}}_{k}}$ （体重秤给出， $k=1\tilde{\ }50$ ）和估计值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ （算法算出来的， $k=1\tilde{\ }50$ ），同时根据算法也会有估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ （算法算出来的， $k=1\tilde{\ }50$ ）。接下来是51周（ k=51 ），已经得到了此时猪头的重量观测值 ${{\mathbf{z}}_{51}}$ ，有了这些数据就可以走算法了：

第一步：根据公式（1）和公式（2）计算预测值 $\mathbf{\theta }_{k}^{'}$ 以及预测值与真实值之间的协方差矩阵 $\mathbf{\Sigma }_{k}^{'}$ ；（数据准备）
第二步：根据公式（3）和公式（4）计算卡尔曼增益 $\mathbf{K}_{k}^{'}$ ，然后根据公式（5）估计此时猪头的重量 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ ；（滤波估计）
第三步：根据公式（6）计算估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ ，用于下一次递推；（参数更新）

以上三步算法中 k=51 ，为了更清楚地对应到公式，这里没有更改。以上内容共出现了四个值不知道读者有没有看懵了，如果有，这一段要好好体会一下，它们分别是真实值、估计值、观测值、预测值。

真实值就是目标属性的真实状态值，用 ${{\mathbf{\theta }}_{k}}$ 表示，例如猪头此时此刻的重量，这个值咱从头到尾都不知道，所以才要用卡尔曼滤波方法去估计这一个值；
估计值就是用来估计真实值的数值，用 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 表示，例如算法每递推一次，就会估计出一个猪头重量值；
观测值就很明白了，就是状态值的一个映射，用 ${{\mathbf{z}}_{k}}$ 表示，例如例子中体重秤称得的猪头重量；
预测值就是根据上一次的估计值，根据线性系统的模型参数算出来下一步应该是什么样的状态，用 $\mathbf{\theta }_{k}^{'}$ 表示，例如根据上周猪头的重量，对本周重量的一个经验判断。

有了以上这些，读者就可以实现这一整个算法，主要就是上面的这六个公式，每进行一次递推，就要算一遍这些公式。那么最后的小问题就是初始值的设定了，因为有了这一步与上一步的关系，只要整个算法第一次递推之前的初始值确定，接下来整个程序就能正常的运行起来了。这里初始值就两个，一个是初始估计值 $\left\langle {{\mathbf{\theta }}_{1}} \right\rangle$ ，可以直接取第一次的观测值 ${{\mathbf{z}}_{1}}$ ；另一个是初始的估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{1}}$ ，可以取主对角线上的值均为一个较小值（例如0.1这样的，根据实际数据而定）的对角阵，如果只对一维的数据做滤波，那就是一个较小值（例如0.1）。

好的，大功告成，根据以上的内容，读者就可以自己写出完整的卡尔曼滤波算法了，希望我有把卡尔曼滤波的原理讲清楚了。接下来我想给出递推公式的推导过程，有兴趣的读者可以看一下，自己推看看。

3 公式推导

首先列出我们所有的符号为推导做准备：

真实值 ${{\mathbf{\theta }}_{k}}$ 、估计值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 、观测值 ${{\mathbf{z}}_{k}}$ 、预测值 $\mathbf{\theta }_{k}^{'}$ 、估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ ，求期望的符号 $\left\langle \cdot \right\rangle$

线性系统的状态差分方程： ${{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}$

观测方程： ${{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}$

引入卡尔曼增益以修正观测结果： $\left\langle {{{\bf{\theta }}_k}} \right\rangle = {\bf{\theta }}_k^{'} + {{\bf{K}}_k}\left( {\bf{z}}_k - {\bf{z}}_k^{'} \right) = {\bf{\theta }}_k^{' }+ {{\bf{K}}_k}\left( {\bf{z}}_k - {\bf{C\theta }}_k^{'} \right)$ $\left\langle {{{\bf{\theta }}_k}} \right\rangle = {\bf{\theta }}_k^{'} + {{\bf{K}}_k}\left( {\bf{z}}_k - {\bf{z}}_k^{'} \right) = {\bf{\theta }}_k^{' }+ {{\bf{K}}_k}\left( {\bf{z}}_k - {\bf{C\theta }}_k \right)$

预测值 $\mathbf{\theta }_{k}^{'}=\mathbf{A}{\langle{\mathbf{\theta }}_{k-1}\rangle}+\mathbf{B}{{\mathbf{u}}_{k-1}}$

以上四个公式是给出的前提条件，分别用公式（1）~（4）表示。

OK，可以开始推导了：

计算估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$

${{\mathbf{\Sigma }}_{k}}=\left\langle {{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right){{\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)}^{T}} \right\rangle$

然后把公式（2）代入到公式（3）中去掉 ${{\mathbf{z}}_{k}}$ 再代入到上面的等式得到

${{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)-{{\mathbf{K}}_{k}}{{\mathbf{v}}_{k}} \right]{{\left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)-{{\mathbf{K}}_{k}}{{\mathbf{v}}_{k}} \right]}^{T}} \right\rangle$

然后把里面的因式相乘，再分别求期望，又有 ${{\mathbf{\theta }}_{k}}$ 、 $\mathbf{\theta }_{k}^{'}$ 与观测噪声 ${{\mathbf{v}}_{k}}$ 是独立的，求期望等于零，于是得到

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {{\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)}^{T}}+{{\mathbf{K}}_{k}}\left\langle {{\mathbf{v}}_{k}}{{\mathbf{v}}_{k}}^{T} \right\rangle \mathbf{K}_{k}^{T}$

上面的等式中 $\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle$ 表示真实值与预测值的协方差矩阵，用 $\mathbf{\Sigma }_{k}^{'}$ 表示； $\left\langle {{\mathbf{v}}_{k}}{{\mathbf{v}}_{k}}^{T} \right\rangle$ 表示观测噪声的协方差矩阵，用 ${\mathbf{R}}$ 表示。于是得到

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\mathbf{\Sigma }_{k}^{'}{{\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)}^{T}}+{{\mathbf{K}}_{k}}\mathbf{RK}_{k}^{T} \\ =\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}\mathbf{C\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}\mathbf{K}_{k}^{T}+{{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}$

因为 ${{\mathbf{\Sigma }}_{k}}$ 的对角元即为真实值与估计值的误差的平方，矩阵的迹（用 T[] 表示）即为总误差的平方和，即

$T\left[ {{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {{\mathbf{K}}_{k}}\mathbf{C\Sigma }_{k}^{'} \right]$

利用以下矩阵迹的求导公式（其中 $\mathbf{A}$ 和 $\mathbf{B}$ 表示矩阵， $\mathbf{a}$ 表示列向量）：

$Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})$

$Tr(\mathbf{AB})=Tr(\mathbf{BA})$

$\mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}$

要让估计值更接近于真实值，就要使上面的迹尽可能的小，因此要取得合适的卡尔曼增益 ${{\mathbf{K}}_{k}}$ ，使得迹得到最小，言外之意就是使得迹对 ${{\mathbf{K}}_{k}}$ 的偏导为0，即

$\frac{\partial T\left[ {{\mathbf{\Sigma }}_{k}} \right]}{\partial {{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{C\Sigma }_{k}^{'} \right)}^{T}}=0$ $\frac{\partial T\left[ {{\mathbf{\Sigma }}_{k}} \right]}{\partial {{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{C\Sigma }_{k}^{'} \right)}^{T}}=0$

这样就能算出合适的卡尔曼增益了，即

${{\mathbf{K}}_{k}}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}{{\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)}^{-1}}$

再把这个式子代回 ${{\mathbf{\Sigma }}_{k}}$ 的求值公式就可以得到

${{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}{{\left( \mathbf{C\Sigma }_{k}^{'}{{\mathbf{C}}^{T}}+\mathbf{R} \right)}^{-1}}\mathbf{C\Sigma }_{k}^{'}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{C} \right)\mathbf{\Sigma }_{k}^{'}$

接下来就差真实值与预测值之间的协方差矩阵 $\mathbf{\Sigma }_{k}^{'}$ 的求值公式了

$\mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle$

然后把公式（1）和公式（4）代入得到

$\mathbf{\Sigma }_{_{k+1}}^{'}=\left\langle \left[ \mathbf{A}\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)+{{\mathbf{s}}_{k}} \right]{{\left[ \mathbf{A}\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)+{{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle$

有 ${{\mathbf{\theta }}_{k}}$ 、 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 与观测噪声 ${{\mathbf{s}}_{k}}$ 是独立的，求期望等于零； $\left\langle {{\mathbf{s}}_{k}}{{\mathbf{s}}_{k}}^{T} \right\rangle$ 表示观测噪声的协方差矩阵，用 ${\mathbf{Q}}$ 表示。于是得到

$\mathbf{\Sigma }_{_{k+1}}^{'}=\mathbf{A}\left\langle \left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right){{\left( {{\theta }_{k}}-\left\langle {{\theta }_{k}} \right\rangle \right)}^{T}} \right\rangle {{\mathbf{A}}^{T}}+\left\langle {{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle =\mathbf{A}{{\mathbf{\Sigma }}_{k}}{{\mathbf{A}}^{T}}+\mathbf{Q}$

其中的协方差矩阵的转置矩阵就是它本身。这样就完成了全部公式的推导了。

4 实验结果

为了更形象地给出卡尔曼滤波算法的效果，这里另外构造一个实际应用的例子：一辆车子在空旷场地上行驶，通过GPS测量车子实时相对于一个参考点的横纵坐标，之后通过该观测结果，估计该车辆的实时位置。

状态向量 $\left [x,y,v_{x},v_{y} \right ]^{T}$ ，观测向量 $\left [{z_x},{z_y} \right ]^{T}$ 。

状态转移方程和观测方程为：

$\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{x_{k}}} \\ {{v}_{y_{k}}} \\ \end{matrix} \right]=A\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}$

$\left[ \begin{matrix} {{z_x}_{k}} \\ {{z_y}_{k}} \end{matrix} \right]=C\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}=\begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0& 0 \end{bmatrix}\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}$

这里给定GPS横坐标测量的噪声均值为卡尔曼滤波系列——（一）标准卡尔曼滤波[通俗易懂]0 ，方差为；纵坐标测量的噪声均值为0，方差为（单位为m）；

采样时间点为 $\small 500$ 个，采样的时间间隔 $\small \Delta t=0.01$ 秒

车辆的初始状态为 (-20;20;10;10) ，四个状态量的噪声的方差分别为 (0.01;0.01;0.05;0.05) 。仿真结果如下：

卡尔曼滤波系列——（一）标准卡尔曼滤波[通俗易懂]

可以看到，相对于直接取用GPS的观测结果作为目标状态的估计而言，通过卡尔曼滤波后获得的状态估计要更加准确，状态波动小，较为稳定，更符合实际中车辆的行驶过程。本次实验中，通过卡尔曼滤波技术确定的定位均方误差约为0.5m，而直接用观测结果的定位均方误差为1.7m，可以看出卡尔曼滤波在定位精度上可以带来极大地提升。

这里需要说明一点，为了使得读者能更好的把程序和算法的公式对应起来，代码的变量并没有设置成最节省内存的形式，实际应用中，滤波数据可能非常庞大，每一次递推过程都应该覆盖前一次的计算结果，这样才不会过多占用内存。

5 总结

数据滤波是去除噪声还原真实数据的一种数据技术，卡尔曼滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

卡尔曼滤波算法的主要思想在于同时利用了目标的实时状态信息和观测结果，相比于直接利用单一观测估计系统状态而言，卡尔曼滤波算法利用了更多的先验信息，每一个时刻都对系统状态做估计，并预测了下一个时刻的系统大致状态，之后根据观测结果，利用新息与卡尔曼增益相乘，修正预测量，从而完成对系统状态更准确的估计。其整个过程是递推进行的，每完成一次系统的状态估计，就计算一次估计值与真实值的误差协方差矩阵，用于下一次估计时的变量计算。

矩阵 ${\mathbf{Q}}$ 和矩阵 ${\mathbf{R}}$ 表征的是状态噪声和观测噪声的协方差，而卡尔曼滤波算法的最终状态估计，说到底就是根据矩阵 ${\mathbf{Q}}$ 和矩阵 ${\mathbf{R}}$ 的值去权衡系统的估计偏向预测和偏向观测的程度。举个例子，如果矩阵 ${\mathbf{Q}}$ 远大于矩阵 ${\mathbf{R}}$ ，也就是说状态噪声十分剧烈，相对的观测噪声很小，那么系统的估计就非常接近观测的结果；反之矩阵 ${\mathbf{Q}}$ 远小于矩阵 ${\mathbf{R}}$ ，也就是说观测噪声相当大，相对的状态噪声很小，那么系统的状态估计就更多地偏向于根据上一个时刻的状态估计所预测的当前时刻系统状态。

标准卡尔曼滤波方法是在线性模型下，噪声满足高斯分布时的一种最优估计手段，这同时带来了算法的短处：矩阵 ${\mathbf{Q}}$ 和矩阵 ${\mathbf{R}}$ 需要提前给定无法自适应，非线性模型无法求解，非高斯噪声算法不匹配等等，所以各国学者针对不同的应用背景，对标准尔曼滤波做了诸多扩展，笔者将会接下来的博文中讲解部分扩展形式，希望大家关注。