卡尔曼滤波

卡尔曼滤波器会追踪系统的估计状态，以及估计的变异量或是不确定性。会透过状状态转换模型以及其量测来更新估计值。 是指时间k时的估计，还没有考虑第k次量测yk的资讯，为对应的不确定性
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含杂讯的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。
卡尔曼滤波在技术领域有许多的应用。常见的有飞机及太空船的guidance, navigation, and control。卡尔曼滤波也广为使用在时间序列的分析中，例如信号处理及计量经济学中。卡尔曼滤波也是机器人运动规划及控制的重要主题之一，有时也包括在trajectory optimization。卡尔曼滤波也用在中轴神经系统运动控制的建模中。因为从给与运动命令到收到感觉神经的回授之间有时间差，使用卡尔曼滤波有助于建立符合实际的系统，估计运动系统的目前状态，并且更新命令。
卡尔曼滤波的演算法是二步骤的程序。在估计步骤中，卡尔曼滤波会产生有关目前状态的估计，其中也包括不确定性。只要观察到下一个量测（其中一定含有某种程度的误差，包括随机杂讯）。会通过加权平均来更新估计值，而确定性越高的量测加权比重也越高。演算法是迭代的，可以在实时控制系统中执行，只需要目前的输入量测、以往的计算值以及其不确定性矩阵，不需要其他以往的资讯。
使用卡尔曼滤波不用假设误差是正态分布，不过若所有的误差都是正态分布，卡尔曼滤波可以得到正确的条件机率估计。
也发展了一些扩展或是广义的卡尔曼滤波，例如运作在非线性系统的extended Kalman filter及无迹卡尔曼滤波（unscented Kalman filter）。底层的模型类似隐马尔可夫模型，不过潜在变量的状态空间是连续的，而且所有潜在变量及可观测变数都是正态分布。

**应用实例**
卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用（如雷达、机器视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。
例如，对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。

**命名**
这种滤波方法以它的发明者鲁道夫·卡尔曼（Rudolph Kalman）命名，但是根据文献可知实际上Peter Swerling在更早之前就提出了一种类似的算法。
（Stanley Schmidt）首次实现了卡尔曼滤波器。卡尔曼在NASA艾姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文由Swerling（1958）、Kalman (1960)与Kalman and Bucy（1961）发表。
目前，卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。
----
以下的讨论需要线性代数以及概率论的一般知识。
----

**演算概念**
卡尔曼滤波器使用系统的动态模型（例如，运动的物理定律），该系统的已知控制输入以及多个顺序的测量值（例如来自传感器的测量值）来形成对系统变化量（其状态）更好的估计，其精度比仅使用一种测量获得的估算值高。它是一种常见的感测器融合和Data fusion算法。
感测器数据的杂讯，描述系统演化的方程式的近似值以及未考虑所有因素的外部因素都限制了确定系统状态的能力。卡尔曼滤波器有效地处理了由于感测器数据杂讯引起的不确定性，并在一定程度上处理了随机外部因素。卡尔曼滤波器使用加权平均值生成系统状态的估计值，作为系统预测状态和新测量值的平均值。权重的目的是估计值具有更好（即较小）的不确定性的值会被更多“信任”。权重是根据共变异数来计算的，共变异数是对系统状态预测的估计不确定性的度量。加权平均值的结果是介于预测状态和测量状态之间的新状态估计，并且比任何一个状态都有更好的估计不确定性。在每个时间步重复此过程，新的估计值及其共变异数将通知后续迭代中使用的预测。这意味着卡尔曼滤波器可以递回地工作，并且只需要系统状态的最后“最佳猜测”，而不是整个历史，就可以计算新状态。
测量和当前状态估计的相对确定性是重要的考虑因素，通常根据卡尔曼滤波器的增益来讨论滤波器的反应。卡尔曼增益是赋予测量值和当前状态估计值的相对权重，可以进行“调整”以获得特定的性能。增益高时，滤波器将更多的精力放在最新的测量上，因此反应速度更快。增益较低时，滤波器会更紧密地遵循模型预测。在极端情况下，接近1的高增益将导致估计的轨迹更加跳跃，而接近零的低增益将消除杂讯，但会降低反应速度。
在执行滤波器的实际计算时（如下所述），状态估计值和共变异数被编码到矩阵中，以处理单个计算集中涉及的多个维度。这允许在任何过渡模型或共变异数中表示不同状态变量（例如位置，速度和加速度）之间的线性关系。

**基本动态系统模型**
卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。
为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义矩阵Fk, Hk, Qk, Rk，有时也需要定义Bk，如下。
卡尔曼滤波器的模型。圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差矩阵在右下方标出。
卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：

其中
*Fk是作用在xk−1上的状态变换模型（/矩阵/向量）。- Bk是作用在控制器向量uk上的输入－控制模型。- wk是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。

时刻k，对真实状态xk的一个测量zk满足下式：

其中Hk是观测模型，它把真实状态空间映射成观测空间，vk是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。

初始状态以及每一时刻的噪声{x0, w1, ..., wk,  v1 ... vk}都认为是互相独立的。
实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种，在下边讨论中有描述。

**卡尔曼滤波器**
卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。
卡尔曼滤波器的状态由以下两个变量表示：
*，在时刻k的状态的估计；
*，后验估计误差协方差矩阵，度量估计值的精确程度。
卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

**预测**
（预测状态）
（预测估计协方差矩阵）
可参考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf date=20210126065856 
可参考：http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf date=20200923102207 

**更新**
首先要算出以下三个量：
（测量残差）
（测量残差协方差）
（最优卡尔曼增益）
然后用它们来更新滤波器变量x与P：
（更新的状态估计）
（更新的协方差估计）
使用上述公式计算仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。

**不变量（Invariant）**
如果模型准确，而且与的值准确的反映了最初状态的分布，那幺以下不变量就保持不变：所有估计的误差均值为零
且协方差矩阵准确的反映了估计的协方差：
请注意，其中表示的期望值, 。

**实例**
考虑在无摩擦的、无限长的直轨道上的一辆车。该车最初停在位置0处，但时不时受到随机的冲击。每隔Δt秒即测量车的位置，但是这个测量是非精确的；想建立一个关于其位置以及速度的模型。来看如何推导出这个模型以及如何从这个模型得到卡尔曼滤波器。
因为车上无动力，所以可以忽略掉Bk和uk。由于F、H、R和Q是常数，所以时间下标可以去掉。
车的位置以及速度（或者更加一般的，一个粒子的运动状态）可以被线性状态空间描述如下：

其中是速度，也就是位置对于时间的导数。
假设在（k − 1）时刻与k时刻之间，车受到ak的加速度，其符合均值为0，标准差为σa的正态分布。根据牛顿运动定律，可以推出

其中

且

可以发现
（因为σa是一个标量）。
在每一时刻，对其位置进行测量，测量受到噪声干扰。假设噪声服从正态分布，均值为0，标准差为σz。

其中

且

如果知道足够精确的车最初的位置，那幺可以初始化

并且，若让滤波器知道确切的初始位置，可给出一个协方差矩阵：

如果不确切的知道最初的位置与速度，那幺协方差矩阵可以初始化为一个对角线元素是B的矩阵，B取一个合适的比较大的数。

此时，与使用模型中已有信息相比，滤波器更倾向于使用初次测量值的信息。

**推导**

**推导后验协方差矩阵**
按照上边的定义，从误差协方差开始推导如下：

代入

再代入  

与

整理误差向量，得

因为测量误差vk与其他项是非相关的，因此有

利用协方差矩阵的性质，此式可以写作

使用不变量Pk|k-1以及Rk的定义这一项可以写作
：
这一公式对于任何卡尔曼增益Kk都成立。如果Kk是最优卡尔曼增益，则可以进一步简化，请见下文。

**最优卡尔曼增益的推导**
卡尔曼滤波器是一个最小均方误差估计器，后验状态误差估计（英文：a posteriori state estimate）是

最小化这个矢量幅度平方的期望值，，这等同于最小化后验估计协方差矩阵Pk|k的迹（trace）。将上面方程中的项展开、抵消，得到：
当矩阵导数是0的时候得到Pk|k的迹（trace）的最小值：

此处须用到一个常用的式子，如下：

从这个方程解出卡尔曼增益Kk：


这个增益称为最优卡尔曼增益，在使用时得到最小均方误差。

**后验误差协方差公式的化简**
在卡尔曼增益等于上面导出的最优值时，计算后验协方差的公式可以进行简化。在卡尔曼增益公式两侧的右边都乘以SkKkT得到

根据上面后验误差协方差展开公式，

最后两项可以抵消，得到
.
这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那幺就不能使用这个简化；必须使用上面导出的后验误差协方差公式。

**与递归贝叶斯估计之间的关系**
假设真正的状态是无法观察的马尔可夫过程，测量结果是从隐性马尔可夫模型观察到的状态。
Hidden Markov Model
根据马尔可夫假设，真正的状态仅受最近一个状态影响而与其它以前状态无关。

与此类似，在时刻k测量只与当前状态有关而与其它状态无关。

根据这些假设，隐性马尔可夫模型所有状态的概率分布可以简化为：

然而，当卡尔曼滤波器用来估计状态x时，感兴趣的机率分布，是基于目前为止所有个测量值来得到的当前状态之机率分布


**信息滤波器**
在信息滤波器或逆共变异数滤波器中，估计的共变异数和估计状态分别由信息矩阵和信息向量代替。 这些定义为：

同样，预测的共变异数和状态具有等效的信息形式，定义为：

以及测量共变异数和测量向量，它们定义为：

信息更新现在变得微不足道了。

信息过滤器的主要优点是，只需将其测量信息矩阵和向量相加即可在每个时间步长过滤N个测量值。

为了预测信息过滤器，可以将信息矩阵和向量转换回它们的状态空间等效项，或者可以使用信息空间预测。

如果F和Q是非时变的，则可以将这些值缓存起来，并且F和Q必须是可逆的。

**频率加权卡尔曼滤波器**
在1930年代，Fletcher和Munson进行了有关不同频率的声音感知的开创性研究。他们的工作导致了在工业杂讯和听力损失调查中加权测得的声音水平的标准方法。此后，已在滤波器和控制器设计中使用了频率
加权，以管理目标频段内的性能。
通常，频率整形函数用于加权指定频段中误差频谱密度的平均功率。 令  表示由 传统的卡尔曼滤波器。 同样，让  表示因果频率加权传递函数。 最小化  是通过简单地构建  而产生的。
的设计仍然是一个悬而未决的问题。 一种进行方式是识别产生估计误差的系统，并将  设置为等于该系统的倒数。 可以重复执行此过程，以提高均方误差为代价，增加滤波器阶数。可以将相同的技术应用于平滑器。

**非线性滤波器**

基本卡尔曼滤波器（The basic Kalman filter）是限制在线性的假设之下。然而，大部份非平凡的（non-trivial）的系统都是非线性系统。其中的「非线性性质」（non-linearity）可能是伴随存在过程模型（process model）中或观测模型（observation model）中，或者两者兼有之。

**扩展卡尔曼滤波器**
在扩展卡尔曼滤波器（Extended Kalman Filter，简称EKF）中状态转换和观测模型不需要是状态的线性函数，可替换为（可微的）函数。


函数f可以用来从过去的估计值中计算预测的状态，相似的，函数h可以用来以预测的状态计算预测的测量值。然而f和h不能直接的应用在协方差中，取而代之的是计算偏导矩阵（Jacobian）。
在每一步中使用当前的估计状态计算Jacobian矩阵，这几个矩阵可以用在卡尔曼滤波器的方程中。这个过程，实质上将非线性的函数在当前估计值处线性化了。
这样一来，卡尔曼滤波器的等式为：
预测


使用Jacobians矩阵更新模型


更新

 



预测
如同扩展卡尔曼滤波器（EKF）一样, UKF的预测过程可以独立于UKF的更新过程之外，与一个线性的（或者确实是扩展卡尔曼滤波器的）更新过程合并来使用；或者，UKF的预测过程与更新过程在上述中地位互换亦可。

**卡尔曼-布西滤波器**
卡尔曼-布西滤波器（Kalman-Bucy filter，以Richard Snowden Bucy命名）是Kalman滤波器的连续时间版本。
它基于状态空间模型

其中  和  分别代表两个白噪声项  和  的强度（或更准确地说：功率谱密度-PSD-矩阵）。
该滤波器由两个微分方程组成，一个用于状态估计，一个用于共变异数：

卡尔曼增益由

注意，在此表达式中，对于  ，观察杂讯  的共变异数同时表示预测误差（或创新） 的共变异数； 这些共变异数仅在连续时间的情况下才相等。
离散时间卡尔曼滤波的预测步骤和更新步骤之间的区别在连续时间内不存在。
用于共变异数的第二个微分方程是Riccati方程的一个示例。

**混合型卡尔曼滤波器**
大多数物理系统表示为连续时间模型，而离散时间测量则经常通过数字处理器进行状态估计。 因此，系统模型和测量模型由下式给出：

当
.

**初始值**


**预测**

预测方程式是从连续时间卡尔曼滤波器的方程式推导而得，而无需进行测量更新，例如:  。预测状态和共变异数分别通过求解一组初始值等于上一步估计值的微分方程组来计算。
在线性非时变系统的情况下，可以使用矩阵指数将连续时间动态精确地离散化为离散时间系统。

**更新**

更新方程与离散时间卡尔曼滤波器的更新方程相同。

**应用**
自动驾驶仪
*Dynamic positioning
*经济学，特别是宏观经济学，时间序列模型，以及计量经济学
*惯性导航系统
*Radar tracker
*卫星导航系统

**参见**
 即时定位与地图构建- Fast Kalman filter- 比较：维纳滤波及粒子滤波器- 施密特–卡尔曼滤波器- 滤波问题- 滚动时域估计- 无迹变换

**参考文献**
 Gelb A., editor. Applied optimal estimation. MIT Press, 1974.- Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME - Journal of Basic Engineering Vol. 82: pp. 35-45 (1960)- Kalman, R. E., Bucy R. S., New Results in Linear Filtering and Prediction Theory, Transactions of the ASME - Journal of Basic Engineering Vol. 83: pp. 95-107 (1961)- JU97 Julier, Simon J. and Jeffery K. Uhlmann. A New Extension of the Kalman Filter to nonlinear Systems. In The Proceedings of AeroSense: The 11th International Symposium on Aerospace/Defense Sensing,Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997.- Harvey, A.C. Forecasting, Structural Time Series Models and the Kalman Filter. Cambridge University Press, Cambridge, 1989.

**外部链接**
  An Introduction to the Kalman Filter date=20210224114008 , SIGGRAPH 2001 Course, Greg Welch and Gary Bishop-  Kalman filtering chapter from Stochastic Models, Estimation, by Peter Maybeck-  Kalman Filterdate=20070408165701  webpage, with lots of links-  Kalman Filtering-  The unscented Kalman filter for nonlinear estimation date=20210225013227 -  Kalman Filters, thorough introduction to several types, together with applications to Robot Localization date=20181010160514