卡尔曼滤波 概率

目录：                                                               2017.11.13

 

l  概率基础

l  线性卡尔马滤波

l  简单实例

l  公式推导

l  高维高斯分布

l  参考

 

概率基础              

先验概率（可理解为统计概率）,后验概率（可理解为条件概率）：参考附录八

似然估计（似然函数）：参考附录三P152

贝叶斯公式（相关条件概率）：P18

 

高斯分布

一维形式P46：期望（均值）、概率分布情况（方差）P91

高维形式P110：协方差、协方差矩阵P107

高斯分布的运算线性运算、乘积、复合运算（用于KF预测方程）：参考《视觉SLAM十四讲从理论到实践》附录A

 

线性卡尔曼滤波（KF）

 

快速理解

卡尔曼滤波事实上是基于前一时刻状态与后一时刻观测数据的对后一时刻的状态估计。这实际是一种循环的状态概率估计，使用前一时刻的输出状态作为后一时刻的输入状态。

 

下面直接从最本质的一步简单说明：

上式为对均值的更新方程。它由两项组成，前一项为观测状态（测量值）的加权，后一项为前一时刻估计状态的加权。前一项的平均系数就是卡尔曼增益。这个很好理解，若=1/2，就可以当作前一时刻状态和当前观测状态的简单平均。

卡尔曼滤波可以找到对于每个后一状态的最优平均系数（卡尔曼增益），同时也会受到到前一时刻的状态的影响。

 

下面分为三步具体说明：（建立模型、计算、迭代更新）

一、            建立系统模型：

这一步是要依据具体问题建立两个方程：

 方程可以为一维也可以为高维。A、B（H？）为线性定常矩阵（常为1或单位矩阵），u为偏移量（过程控制输入，常为0）

第一个公式表示：x_k可以由一个线性随机差分方程与状态建立联系，过程中可能会引入控制偏移量u，以及过程噪声w~(0，Q)。

第二个公式：测量方程（观测方程）。与上式相似，不过没有过程输入控制。观测引入的环境噪声V~(0，R)。

Q,R两个参数是十分重要的，可以简单理解为原状态和观测方程在矫正观测量的过程中所占的比重。Q越小，观测值所占比重越大；R越小，原状态所占比重越大。这两个参数直接决定了滤波效果。环境观测噪声较容易确定，但是过程噪声就很难设定了。

 

二、            开始计算参数及状态

首先要设置必要参数和初始值

下面有两个独立的方程组：时间更新（预测）、测量更新&#x