使用卡尔曼滤波器的目的
我们假设你建造了一个可以在树林里行走的小机器人,为了精准的进行导航,机器人需要每时每刻都知道它自己的位置
我们用符号
\vec{x_k}
\vec{x_k} = (\vec{p}, \vec{v})
需要注意的是,状态只是一列和你系统有关的变量,它可以是任何的变量,不仅限于位置和速度(例如可以将引擎的温度,操作人员的手指在触摸板上的位置等变量作为系统的状态变量,只要这些变量的状态是可跟踪的).
在制造这个机器人的时候,我们在它上面也安装了一些传感器,其中就包括GPS.机器人上的GPSD定位精度为10m,但是由于树林中存在一些障碍物以及洼地,悬崖我们需求更高精度的定位能力,否则机器人就有可能撞到障碍物或者是跌落.
同时,我们也知道一些与机器人运动相关的信息(变量):知道控制端口发送至机器人驱动轮上的命令,知道机器人的正方向朝向以及前方有无障碍物等.但是很明显,我们无法获取到有关机器人运动的所有信息(变量):机器人有可能收到强风影响,驱动轮可能打滑等.因此仅仅是通过计量驱动轮转动圈数无法准确的计算出机器人走了有多远.
同理,GPS能够非直接的告诉我们一些机器人的状态,但是GPS所提供的信息也是带有误差和不确定行的.因此如果只是依据GPS做出的预测也是不够精确的.
虽然单一的依靠某一种信息无法给我们提供一个具有足够置信度的结果,但是如果我们将所有有用的信息结合在一起,我们能否在不依赖机器人自身以外的传感器得到一个比较的预测结果呢?答案当然是肯定的,这就是卡尔曼滤波器的设计目的.
在看卡尔曼滤波器之前我们先来复习一下几个统计学的概念.
协方差
协方差表示的是两个变量的总体的误差 ，这与只表示一个变量误差的方差不同。协方差反应两个变量的相关程度 。 如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。 如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。
\operatorname {cov} (X,Y)=\operatorname {E} ((X-\mu )(Y-\nu ))
协方差矩阵
因为协方差只能表示两个变量之间的误差,如果有多个变量的时候需要用到协方差矩阵.其 i, j 位置的元素是第 i 个与第 j 个变量之间的协方差。
\Sigma _{ij}=\mathrm {cov} (X_{i},X_{j})=\mathrm {E} {\begin{bmatrix}(X_{i}-\mu _{i})(X_{j}-\mu _{j})\end{bmatrix}}
展开形式为:
\Sigma = \begin{bmatrix}
\mathrm{E}[(X_1 - \mu_1)(X_1 - \mu_1)] & \mathrm{E}[(X_1 - \mu_1)(X_2 - \mu_2)] & \cdots & \mathrm{E}[(X_1 - \mu_1)(X_n - \mu_n)] \\ \\
\mathrm{E}[(X_2 - \mu_2)(X_1 - \mu_1)] & \mathrm{E}[(X_2 - \mu_2)(X_2 - \mu_2)] & \cdots & \mathrm{E}[(X_2 - \mu_2)(X_n - \mu_n)] \\ \\
\vdots & \vdots & \ddots & \vdots \\ \\
\mathrm{E}[(X_n - \mu_n)(X_1 - \mu_1)] & \mathrm{E}[(X_n - \mu_n)(X_2 - \mu_2)] & \cdots & \mathrm{E}[(X_n - \mu_n)(X_n - \mu_n)]
\end{bmatrix}
高斯分布与多维高斯分布
高斯分布代表一个变量随机出现的概率,概率密度函数为:
f(x) = {1 \over \sigma\sqrt{2\pi} }\,e^{- {{(x-\mu )^2 \over 2\sigma^2}}}
其中
\sigma^2
如果我们把
\sigma^2
\Sigma
#卡尔曼滤波器
\vec{x} = \begin{bmatrix}
p\\
v
\end{bmatrix}
卡尔曼滤波器假设所有变量(在此例中为位置和速度)都满足随机的高斯分布.每一个变量都有均值u来代表随机分布的中心点,符号
\sigma^2
在该示意图中,位置和速度是不具有关联性的,也就是说我们无法通过位置推断得到和与速度有关的信息,反之亦然.
当位置和速度变量具有关联性时,其示意图如下所示.如图所示,观察到某一个特定位置的可能性(概率)受当前机器人的速度影响.
上图所述这种情形可能在这种情况下发生.例如,当前我们依据上一时刻的位置去预测机器人下一时刻的位置.如果机器人的运行速度较高,其移动距离也可能变的更大,从而导致计算的位置也发生较大的距离变化.反之亦然.
这种关系是十分重要的,因为它会告诉我们更多的有关信息:一个变量的状态将会反应出其他变量量的一些状态.这也是卡尔曼滤波器的目的,即我们希望尽量的从具有不确定性的信息中获取尽可能多的有用信息.
在之后计算过程中我们用协方差矩阵(covariance matrix)来代表和描述这种关联关系.即Σij描述了第i个状态变量与第j个状态变量之间的关联性.协方差矩阵通常用符号Σ表示,其中的元素表示为Σij.
#用矩阵方式表述问题
估计值
\mathbf{\hat{x}_k}
协方差矩阵Pk;
\begin{equation}
\begin{aligned}
\mathbf{\hat{x}}_k &= \begin{bmatrix}
\text{position}\\
\text{velocity}
\end{bmatrix}\\
\mathbf{P}_k &=
\begin{bmatrix}
\Sigma_{pp} & \Sigma_{pv} \\
\Sigma_{vp} & \Sigma_{vv} \\
\end{bmatrix}
\end{aligned}
\end{equation}
在此处我们仅仅只使用了机器人的位置和速度来表示状态量,但是在实际操作和运算中,状态矩阵的定义可以包括其他任何有用的变量.
现在,我们需要了解一下当前状态(t = k-1)和下一时刻的预测状态(t = k)的情况.我们需要注意的是,虽然我们无法明确指出当前分布中的哪一种情况是当前真实的状态,但是我们仍然可以通过预测方程得到新时刻的分布情况.因为预测方程并非只作用于某一个具体的状态量,而是作用与当前分布中所包含的所有情况.
在下图中我们用矩阵Fk来表示预测步骤:
现在思考一下,我们如何运用一个矩阵来预测下一时刻机器人的位置和速度呢?在此,我们将会运用如下所示的运动学方程进行预测:
\begin{split}
\color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + \Delta t &\color{royalblue}{v_{k-1}} \\
\color{deeppink}{v_k} &= \color{royalblue}{v_{k-1}}
\end{split}
转化为矩阵形式有:
\begin{align}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \begin{bmatrix}
1 & \Delta t \\
0 & 1
\end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}}
= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}}
\end{align}
很明显,这是一个匀速运动模型,新时刻的位置P_new = P_previous + time*v .现在我们已经求得了预测矩阵(或称为状态转移矩阵),但是我们仍然不知道如何去更新协方差矩阵.
为了更新协方差矩阵,我们需要一个新的公式即:
\begin{equation}
\begin{split}
Cov(x) &= \Sigma\\
Cov(\color{firebrick}{\mathbf{A}}x) &= \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T
\end{split}
\end{equation}
该公式表示,将当前分布中所包含的所有情形(点)乘以预测矩阵就能够得到更新或的协方差矩阵了.
以上所属内容,可以总结为以下两个公式:
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\
\color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T
\end{split}
\end{equation}
#外部影响
以火车运行为例,火车的操作员可能会控制运行开关使得火车加速.类似的,在此机器人的案例中,机器人导航系统可能会发出指令使得机器人驱动轮转向或停止.因此,在计算中我们需要考虑到这类变量对系统的影响.通常来说我们将这类变量称作为系统的控制变量,用符号
\color{darkorange}{\vec{\mathbf{u}_k}}
现在我们假设对于机器人这个案例,已知其外部控制量为加速度a(由运动开关或控制命令进行控制),则之前所述的运行方程可更新为:
\begin{split}
\color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + {t} &\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} {t}^2 \\
\color{deeppink}{v_k} &= \color{royalblue}{v_{k-1}} + \color{darkorange}{a} {t}
\end{split}
矩阵形式为:
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix}
\frac{t^2}{2} \\
t
\end{bmatrix} \color{darkorange}{a} \\
&= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}}
\end{split}
\end{equation}
其中Bk为控制矩阵,而
\color{darkorange}{\vec{\mathbf{u}_k}}
现在在让我们思考另外一个问题,如果我们预测并不是一个100%精准的模型,那么将会发生什么?
#外部不确定性
假如我们的变量是通过系统本身的属性以及已知的外部影响而计算得到的,那么状态计算将不会有太大问题.但假如影响系统的量我们无法明确获取到他们的值呢?
仍然以我们的机器人为例,在运动过程中其驱动轮可能打滑,机器人也有可能撞到地面上的隆起物(地面凹凸不平)使速度变慢.我们很难表示或追踪这些因素所产生的影响.一旦这些情况发生,我们的预测结果就很有可能与实际结果产生很大的偏差,因为我们并未在数学模型中考虑到这些因素.
但是不用担心,在数学上对于这种情况是有解决办法的.我们在每一次的预测步骤中加入新的不确定性来表示这些世界中存在但是我们无法明确表示的变量所带来的影响.
原始估计中的每个状态都会通过变换方程运动到一个新的状态范围中.
如图下所示,上一时刻状态
\color{royalblue}{\mathbf{\hat{x}}_{k-1}}
\color{mediumaquamarine}{\mathbf{Q}_k}
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\
\color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k}
\end{split}
\end{equation}
这两个公式代表了卡尔曼滤波器中的预测部分,是卡尔曼滤波器五个基础公式中的前两个.其中估计值
\color{deeppink}{\mathbf{\hat{x}}_k}
\color{deeppink}{\mathbf{P}_k}
好了,现在我们已经得到了系统的预测(估计)值了.但是在真是的系统中,我们往往还能通过传感器得到一些能反映系统状态的测量值.
#测量值(观测值)
通常,我们能够计算出传感器返回值的分布情况(通过传感器出厂说明中的参数等信息):
\begin{equation}
\begin{aligned}
\mathbf{z}_k &= \mathbf{H}_k \color{deeppink}{\mathbf{\hat{x}}_k} \\
\mathbf{R}_k &= \mathbf{H}_k \color{deeppink}{\mathbf{P}_k} \mathbf{H}_k^T
\end{aligned}
\end{equation}
需要注意的是卡尔曼滤波器之所以好用的一个原因就是它可以很好的处理传感器的噪声.也就是说,传感器的观测值(或测量值)是具有一定程度的不可靠性的(由噪声引起),因此传感器的读数是并非是一个精确值,而是一个带有不确定区域的范围.
通过传感器的观测,我们可以猜测我们的机器人当前所处的状态.但是因为不确定性的存在,因此某些状态比其他状态有更大的可能行被视作传感器的读数.
在卡尔曼滤波器中我们用符号
\color{mediumaquamarine}{\mathbf{R}_k}
\color{yellowgreen}{\vec{\mathbf{z}_k}}
因此,现在我们有了两个不同的高斯分布:一个代表预测步骤,一个代表传感器的观测.
那么现在哪一种才是当前最有可能的状态情况呢?对于某一组传感器读数(Z1,Z2),我们现在有两种可能:
当前的传感器测量为错误的,不可以很好的代表机器人的状态(当前机器人的状态应该完全与预测步骤的分布相同);
当前的传感器测量为正确的,可以很好的代表当前机器人的状态(当前机器人的状态应该完全与传感器观测分布相同);
如果现在我们假设这两种可能行都是正确的,我们将其分布相乘,就能得到一个新的如下所示的分布:
相乘后剩下的区域为高量部分,在此区域中两种假设情况都是高量.这就意味这这个新生成的分布区域比之前任一一种假设都要精确(完全相信预测步骤和完全相信传感器观测),因此该区域为对机器人当前状态的最好估计(可以用下图表示).
#结合高斯
\begin{equation} \mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x – \mu)^2 }{ 2\sigma^2 } } \end{equation}
现在我们想要知道,当我们讲两个高斯曲线(分布)相乘时会发生什么.下图中的蓝色部分表示相乘后得到的新的高斯分布的结果.
\begin{equation} \mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) \cdot \mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, \color{royalblue}{\mu’}, \color{mediumblue}{\sigma’}) \end{equation}
现在我们将一阶高斯分布的公式带入到上式中有:
\begin{equation} \color{purple}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} \end{equation}
\begin{equation}
\begin{split}
\color{royalblue}{\mu’} &= \mu_0 + &\color{purple}{\mathbf{k}} (\mu_1 – \mu_0)\\
\color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – &\color{purple}{\mathbf{k}} \sigma_0^2
\end{split}
\end{equation}
现在让我们将上式改写为矩阵形式的表达式,并令Σ为高斯分布的协方差矩阵:
\begin{equation}
\color{purple}{\mathbf{K}} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1}
\end{equation}
\begin{equation}
\begin{split}
\color{royalblue}{\vec{\mu}’} &= \vec{\mu_0} + &\color{purple}{\mathbf{K}} (\vec{\mu_1} – \vec{\mu_0})\\
\color{mediumblue}{\Sigma’} &= \Sigma_0 – &\color{purple}{\mathbf{K}} \Sigma_0
\end{split}
\end{equation}
K被成为卡尔曼增益,我们将会在后面用到它.
#整合
(\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})
(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})
\begin{equation}
\begin{aligned}
\mathbf{H}_k \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} + \color{purple}{\mathbf{K}} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\
\mathbf{H}_k \color{royalblue}{\mathbf{P}_k’} \mathbf{H}_k^T &= \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} – \color{purple}{\mathbf{K}} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T}
\end{aligned}
\end{equation}
因此卡尔曼增益为:
\begin{equation}
\color{purple}{\mathbf{K}} = \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1}
\end{equation}
在以上的推导过程中我们可以发现每一个变量都乘上了变量,因此可以约掉,又发现在计算协方差矩阵时可以约掉.因此简化后的公式为:
\begin{equation}
\begin{split}
\color{purple}{\mathbf{K}’} &= \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \\
\color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} + \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\
\color{royalblue}{\mathbf{P}_k’} &= (\color{deeppink}{\mathbf{I}} – \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k})\color{deeppink}{\mathbf{P}_k}
\end{split}
\end{equation}
在卡尔曼滤波器中我们称这三个公式为更新步骤,这也是卡尔曼滤波器五个基本公式中的最后三个.
此处的估计值
\color{royalblue}{\mathbf{\hat{x}}_k’}
\color{royalblue}{\mathbf{P}_k’}
卡尔曼滤波
预测
\begin{equation}
\begin{split}
\color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\
\color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k}
\end{split}
\end{equation}
根据测量值修正预测:
\begin{equation}
\begin{split}
\color{purple}{\mathbf{K}’} &= \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \\
\color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} + \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\
\color{royalblue}{\mathbf{P}_k’} &= (\color{deeppink}{\mathbf{I}} – \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k})\color{deeppink}{\mathbf{P}_k}
\end{split}
\end{equation}
扩展卡尔曼
z = Hx
在EKF中,找不到这样的矩阵,不过我们可以根据集合关系建立联系:
h(x') = \begin{pmatrix} \rho\\ \phi\\ \dot{\rho} \end{pmatrix} = \begin{pmatrix} \sqrt{ p{'}_x^2 + p{'}_y^2 }\\
\arctan(p_y' / p_x')\\ \frac{p_x' v_x' + p_y' v_y'}{\sqrt{p{'}_x^2 + p{'}_{y}^2}} \end{pmatrix}
但是这里有个问题,在卡尔曼中测量数据与状态向量的变化时线性相关的,而radar中的角度等与状态向量呈现非线性关系,也就是说h(x')只能代表x'的时刻是正确的,下一时刻就不满足上述关系了.EKF使用泰勒公式解决这个问题.
泰勒公式
设 n 是一个正整数。如果定义在一个包含 a 的区间上的函数 f 在 a 点处 n+1 次可导，那么对于这个区间上的任意 x，都有：
{\displaystyle f(x)=f(a)+{\frac {f'(a)}{1!}}(x-a)+{\frac {f^{(2)}(a)}{2!}}(x-a)^{2}+\cdots +{\frac {f^{(n)}(a)}{n!}}(x-a)^{n}+R_{n}(x).}
其中的多项式称为函数在a 处的泰勒展开式，剩余的
R_{n}(x)
(x-a)^{n}
多元泰勒公式
对于多元函数，也有类似的泰勒公式。
{\displaystyle f(a+h)=f(a)+Jf(a)h+
{\frac {1}{2}}(h_{1},...h_{n})Hf(a){\begin{pmatrix}h_{1}\\...\\h_{n}\\\end{pmatrix}}+...}
其中
{\displaystyle Jf(a)}
雅可比矩阵 ，
{\displaystyle Hf(a)}
海森矩阵 .
取近似值,只取第一项,即:
h(x) \approx h(u) + \frac {\partial h(u)}{\partial x}(x-u)
泰勒在EKF中的应用
在非线性卡尔曼中,我们需要使用一个非线性函数,使得
z = h(x)
结合多元泰勒公式,得到:
z \approx h(x') + H_j(x-x')
其中
H_j
这说明我们可以使用
H_j