高斯滤波的基本原理_Kalman Filter 卡尔曼滤波 基础原理

本文只是记录一下学习时公式推导过程，限于作者水平，不足之处请见谅。

卡尔曼滤波器解决的问题是：一般地，当可以由上一时刻状态估计当前时刻状态，且可通过传感器测量当前时刻状态，而两种方式都却有误差时，到底信哪一种方式得出的当前状态值更多些？即如何确定最优估计值。

注：上面三张图中部分图片来自参考文献[2].

---

卡尔曼滤波器推导前的变量定义:

首先定义离散系统的状态方程

(1)

其中

为系统矩阵（状态转移矩阵），

代表系统模型； 为输入矩阵（控制矩阵），即输入如何影响状态变量；

为观测矩阵，即系统状态如何影响输出

；

是系统过程的噪声，

是观测噪声；状态变量初值为

、

、

、

、

。

设

，

，其中

为系统过程噪声方差，

为观测噪声的方差。

两次估计的误差含义为：

误差

和

的表达式如(2)和(3)

(2)

对于

，此处直接用了修正公式

(3)

先验估计

的均方误差阵为

（4）

后验估计

的 均方误差阵为

(5)

因

中

，且噪声和其他变量相乘的期望为零，得

(6)

因

中

，噪声和其他变量相乘的期望为零，得

(7)

至此，卡尔曼滤波基础原理中，差分方程、变量、误差、均方误差等变量的定义结束。

---

卡尔曼率滤波的5个基本公式：

公式 ①：状态预测，获得第k次状态的估计值

设激励

均值不为0，正交投影(Orthogonal Projection)引理

，系统的状态矢量

基于前

次观测矢量

的线性最小均方误差估计矢量，是 在

上的正交投影。再根据系统的状态方程和正交投影引理

，得根据第

次状态对第

次状态的估计值

(8)

上式中

表示

在

上的正交投影。

公式②：获得预测值

的均方误差

(9)

公式③：获得修正值

根据正交投影引理3，第

次的观测矢量

的线性最小均方误差估计矢量

可以由第

次观测矢量

的估计矢量

与第

次的观测矢量

运算获得，计算如下式

(10)

根据正交投影引理

，其中

(11)

(12)

(13)

化简得

(14)

公式④：获得卡尔曼增益

计算方法一：利用正交投影定理得

(15)

计算方法二：

为了估计最优，即后验估计

的均方误差最小，进而转化为求

最小，求导并令导数为零

(16)

则，最优估计下卡尔曼增益应，如下式

(17)

公式⑤：获得第

次修正值的均方误差

(18)

带入卡尔曼增益

得公式(18)的最终达式

(19)

自此，卡尔曼滤波的基本原理与公式推导已经详细地给出。为了方便记忆和总结，卡尔曼的公式之间变量的关系及用到的数学技巧总结如图1。使用时只需依次迭代5个公式即可。

图1 卡尔曼滤波的5个基本公式中，变量的关系和公式递推顺序

注：

1、高斯分布的乘积还是高斯分布。

2、根据图1可以看出，卡尔曼增益的大小是由Q、R决定，标准的卡尔曼滤波在线性时不变系统下是可以离线计算出增益的。

卡尔曼增益最后会变成定值吗？​www.zhihu.com

参考文献：

[1] 赵树杰. 信号检测与估计理论[M]. 清华大学出版社有限公司, 2005

[2] Faragher R . Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation [Lecture Notes][J]. IEEE Signal Processing Magazine, 2012, 29(5):128-132.

[3] P.A. Bromiley;Products and Convolutions of Gaussian Probability Density Functions