文章目录
- 卡尔曼滤波简介
- 基础知识
- 正态分布
- 协方差
- 卡尔曼滤波公式
- 卡尔曼滤波公式的推导过程
卡尔曼滤波简介
论文:A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法），Rudolf Emil Kalman，1960。
论文地址:http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf 。
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。比如我们可以使用第k个时刻的状态信息输入卡尔曼滤波器来预测第k+1个时刻的状态信息，同时，第k+1个时刻的实际状态信息还可以用来更新卡尔曼滤波器的参数，使其在之后时刻的预测中变得更加准确。
基础知识
正态分布
又叫高斯分布。
若随机变量X服从一个位置参数为μ、尺度参数为σ的概率分布，且其概率密度函数为
则这个随机变量X就称为正态随机变量，正态随机变量服从的分布就称为正态分布，记作
称随机变量X服从正态分布。
当μ=0，σ=1时的正态分布是标准正态分布。一般正态分布可以通过下式转化成标准正态分布:
协方差
协方差用来描述两个变量在变化时的线性相关关系，如果一个变量在变大时另一个变量也在变大，即两个变量是同向变化的，此时协方差是正的。如果一个变量在变大时另一个变量在变小，则两个变量是反向变化的，此时协方差是负的。
协方差公式:
卡尔曼滤波公式
卡尔曼滤波模型假设k时刻状态的真实向量xk可以用下面的公式表示：
其中xk是系统在k时刻的真实状态向量，大小为nx1。A是状态转移矩阵（与xk-1即系统在k-1时刻的真实状态向量相乘），大小为nxn，矩阵A的值都是不变的常数。uk为k时刻的系统输入，大小为kx1。B是将输入转换为状态的矩阵，大小为nxk，矩阵B的值都是不变的常数。在大多数情况下，上式没有Buk项。
随机变量wk-1为过程噪声，假定wk-1符合均值为零，协方差矩阵为Q的多元正态分布，注意协方差矩阵Q的值也都是不变的常数。即:
上式表达的含义是，每时刻的真实向量xk都可以通过一个线性随机方程估计出来。
在k时刻，对真实状态向量xk的观测值向量zk满足下式：
其中zk是k时刻的测量值向量，大小为mx1（注意不是nx1），H是状态向量到测量向量的转换矩阵，大小为mxn，矩阵H的值都是不变的常数。xk是k时刻的真实状态向量，大小为nx1。
随机变量vk是观测噪声，假定vk符合均值为零，协方差矩阵为R的多元正态分布，注意协方差矩阵R的值也都是不变的常数。即:
上式表达的含义是，任何测量值zk都是信号值与测量噪声的线性组合。
总结一下，A、B、H、Q、R都是值不变的矩阵或向量，一旦模型建立好后，它们在之后的迭代过程中是不会变化的。
假设初始状态以及每一时刻的噪声{x0, w1, …, wk, v1 … vk}都认为是互相独立的。
大多数真实世界中的动态系统并不是一个严格的线性模型，系统中存在一定的不确定性，这会导致我们估计的系统状态矩阵xk存在偏差，这个真实值和估计值之间的偏差值我们用过程噪声wk-1来表示，且近似wk-1为高斯分布。另外，观测值往往也包含噪声和干扰，或者观测矩阵H自身存在观测偏差，这个真实值和观测值之间的偏差值用测量噪声vk表示，且近似vk为高斯分布。
卡尔曼滤波五个方程：时间更新方程组（两个，用于预测）以及测量更新方程组（三个，用于修正）。这两个方程组在滤波器运行的每一个时刻k都会执行。在预测阶段，卡尔曼滤波器使用上一时刻k-1的状态估计值，做出对当前时刻k的状态估计值。在更新阶段，滤波器利用对时刻k的状态观测值优化在预测阶段获得k时刻的预测值，以获得一个更精确的k时刻新估计值。
预测（两个时间更新方程组）:
上式左边xk是在时刻k的状态预估计值。xk-1表示k-1时刻的状态估计值。uk为k时刻的系统输入，大小为kx1。B是将输入转换为状态的矩阵，大小为nxk。在大多数情况下，上式没有Buk项。
注意上面xk-和xk-1的含义区别：一个是k时刻的状态预估计值，一个是k-1时刻的状态估计值。
上式左边Pk是k时刻的预估计误差协方差（度量状态估计值和真实值之间的误差），Pk-1表示的是k-1时刻的估计误差协方差。wk符合均值为零，协方差矩阵为Q的多元正态分布。
注意上面Pk-和Pk-1的含义区别：一个是k时刻的预估计误差协方差，一个是k-1时刻的估计误差协方差。
更新（三个测量更新方程组）:
上面三个方程计算的分别是k时刻的卡尔曼增益Kk，k时刻的状态估计值xk，k时刻的估计误差协方差Pk。zk即k时刻的测量值。H是状态向量到测量向量的转换矩阵。vk符合均值为零，协方差矩阵为R的多元正态分布。I为单位矩阵。
迭代过程:
在时刻k时我们先计算上面预测的两个方程的值（分别称为k时刻的预估计值和k时刻的预估计误差协方差），有了这两个值就能计算k时刻的卡尔曼增益Kk，再然后得到k时刻的估计值和k时刻的估计误差协方差。这两个值在下一个时刻k+1时计算k+1时刻的预估计值和k+1时刻的预估计误差协方差时会继续用到。
卡尔曼滤波公式的推导过程
假设k时刻的真实状态向量xk为:
其中xk是系统在k时刻的真实状态向量，大小为nx1。A是状态转移矩阵（与xk-1即系统在k-1时刻的真实状态向量相乘），大小为nxn，矩阵A的值都是不变的常数。uk为k时刻的系统输入，大小为kx1。B是将输入转换为状态的矩阵，大小为nxk，矩阵B的值都是不变的常数。在大多数情况下，上式没有Buk项。
随机变量wk-1为过程噪声，假定wk-1符合均值为零，协方差矩阵为Q的多元正态分布，注意协方差矩阵Q的值也都是不变的常数。即:
上式表达的含义是，每时刻的真实向量xk都可以通过一个线性随机方程估计出来。
在k时刻，对真实状态向量xk的观测值向量zk满足下式：
其中zk是k时刻的测量值向量，大小为mx1（注意不是nx1），H是状态向量到测量向量的转换矩阵，大小为mxn，矩阵H的值都是不变的常数。xk是k时刻的真实状态向量，大小为nx1。
随机变量vk是观测噪声，假定vk符合均值为零，协方差矩阵为R的多元正态分布，注意协方差矩阵R的值也都是不变的常数。即:
上式表达的含义是，任何测量值zk都是信号值与测量噪声的线性组合。
总结一下，A、B、H、Q、R都是值不变的矩阵或向量，一旦模型建立好后，它们在之后的迭代过程中是不会变化的。
假设初始状态以及每一时刻的噪声{x0, w1, …, wk, v1 … vk}都认为是互相独立的。
下面三个变量的含义如下:
上面三个变量分别表示k时刻的状态真实值，k时刻的状态预估计值（先验估计值），k时刻的状态估计值（预估计值再经过调整后的估计值，又叫后验估计值）。
又已知:
上式即卡尔曼滤波五个公式中的预测方程组中的第一个，求k时刻的状态预估计值（先验估计值）。
上式即卡尔曼滤波五个公式中的更新方程组中的第二个，求得k时刻的状态估计值（预估计值再经过调整后的估计值，又叫后验估计值）。
卡尔曼增益K表示过程误差与测量误差的比重，k的取值在[0, 1] 。即：
当K=0时，即预测误差为0，系统的状态最优估计值完全取决与预测值；当K=1时，即测量误差为0，系统的状态值完全取决于测量值。K会随着迭代过程而逐步更新。
现在我们定义四个量:
其中ek-称为先验状态误差（真实值与预估计值之间的误差），ek称为后验状态误差（真实值与估计值之间的误差）。Pk-为真实值与预估计值之间的协方差，Pk为真实值与估计值之间的协方差。
由下面两式:
我们可推导出:
继续化简，得:
将ek-和ek的表达式代入上式，得:
Pk可化为:
其中I代表单位矩阵，Rk即前面的测量噪声vk在k时刻的协方差矩阵。上面应用了转置矩阵的一个性质:
Pk可继续化为:
卡尔曼滤波的估计原则是使真实值与估计值之间的协方差Pk最小，使估计值越来越接近真实值。因此目标函数为:
上面的Pk表达式对卡尔曼增益矩阵K求偏导，并令偏导数为0:
则当Pk取极小值时卡尔曼增益矩阵K的计算公式如下:
又已知:
推导出真实值与估计值之间的协方差Pk的计算公式如下:
再由ek-的表达式:
则ek+1-可化为:
又已知:
我们来求k时刻的真实值与预测值之间的协方差PK-，上式可化为:
则k+1时刻的真实值与预测值之间的协方差Pk+1-为:
推导完毕。
