【控制理论】卡尔曼滤波基本原理及公式推导

一、卡尔曼滤波基本原理

既然是滤波，那肯定就是一种提纯数据的东西。怎么理解呢，如果现在有一个任务，需要知道家里橘子树今年长了多少个橘子。你想到去年、前年、大前年这三年你把橘子吃到过年，按每天吃3个来算，大概知道每年橘子树产了多少橘子，今年的情况应该也差不多。这叫数学模型预测法；不过你懒得去想去年吃了多少个，就打算给邻居家的小孩一把糖果让他去数，小孩三下五除二就数完了，但是他数的仔细不仔细和有没有摘几个回家吃就不知道了，这叫传感器直接测量法。很明显这两种方法都不那么靠谱，卡尔曼滤波就是将这两种方法测得的数据融合起来，得到一个靠谱的数据。

二、公式推导之前回顾一些知识

1、正态分布

曲线两头低，中间高，左右对称。

若随机变量X服从一个数学期望为μ、方差为σ^2的正态分布，记为N(μ，σ^2)。期望值μ决定了其位置，其标准差σ决定了分布的幅度。σ越大曲线越矮胖，σ越小则越瘦长。————来自百度百科

2、协方差

协方差用来描述两个变量在变化时的联系情况，是同向变化，还是反向变化？你变大，同时我也变大，说明两个变量是同向变化的，这时协方差就是正的。你变大，同时我变小，说明两个变量是反向变化的，这时协方差就是负的。

三、公式推导

这里有一篇很不错的文章讲解推导的先贴在这里

如何通俗并尽可能详细解释卡尔曼滤波？ - 米开朗基罗赵的回答 - 知乎 https://www.zhihu.com/question/23971601/answer/375355599

数学模型估计数据

1、假设我们现在要获取一个系统中某些状态稍微精确的数据，比如是智能小车的速度、位置， $x_{k}=\left [ p,v \right ]^{T}$ ，p是position小车位置。

那么由数学模型，k时刻状态估计值可以由k-1时刻大致推导出来

$\check{x_{k}^{-}}=F\cdot \check{x_{k-1}}$

F：状态转移矩阵，通常由机理关系得到，比如 $x_{t}=x_{t-1}+\Delta t\cdot \dot{x_{t-1}}$

$\check{x_{k}^{-}}$ ：注意右边带有小斜杠，这是先验估计

$\check{x_{k-1}^{}}$ ：这是上一时刻的后验估计

先验估计是还没有结合传感器数据的纯模型估计值，后验估计是模型数据和传感器数据融合之后的也就是我们需要的精确数据。

当然在这些数据转移的时候，变量之间的关系（协方差）也会随着改变。 $\check{P_{k}^{}}$ 为协方差

$\check{P_{k}^{-}}=F\cdot \check{P_{k-1}}\cdot F^{T}$ 来源于： $if :cov(x)=\sum ,then:cov(Ax)=A\sum A^{T}$

2、当然状态不可能只受上一时刻状态的影响，比如智能小车有来自电机驱动的动能，小车有时会获得一个加速度。那么状态还要加上这一外部作用力的影响。

$\check{x_{k}^{-}}=F\cdot \check{x_{k-1}}+B\check{u_{k-1}}$

3、出来已知的外部作用力，还有一些不确定性因素会影响系统的状态变化，比如地上坑坑洼洼，智能小车抖那么一下，状态都会发生改变。这就是白噪声的影响，假设白噪声服从方差 $\sum =Q_{k}$ 正态分布。这一部分的影响要加到协方差中去。

$\check{P_{k}^{-}}=F\cdot \check{P_{k-1}}\cdot F^{T}+Q_{k}$

4、以上模型所有要考虑的因素基本都囊括进去了，但是的模型数据要和传感器数据融合起来，量纲肯定要统一，假设通过矩阵 $H_{k}$ 变化。还是据智能小车的例子，假如我们要利用编码器获取小车的速度，我们设计的数学模型关于速度的单位肯定是米每秒，但是编码器采集的数据有可能是一个脉冲数，它还要通过车轮转一米固定多少个脉冲这样一个间接方法才能获得小车的速度。

所以有：（数学模型估计正态分布的平均值和方差）

（敲公式太累了，偷懒！）

传感器数据

5、传感器的数据也符合正态分布，假设平均值是 $\acute{z_{k}}$ ，方差是 $R_{k}$

开始融合

现有两个高斯分布数据，卡尔曼老先生将这两个高斯斑直接相乘，得到了一个新的高斯斑，就是我们要的更加精确的数据。直接通过相乘就得到了满意的结果，果然是大道至简，牛皮。

暂停一下，吃饭去了，明天再改改。。。