Robotics: Estimation and Learning 2—卡尔曼滤波 (Kalman Filter)

前言

该笔记主要对Coursera上宾西法尼亚大学的Robotics: Estimation and Learning课程进行总结以及个人的一些理解，与Robotics: Estimation and Learning同系列的还有Robotics: Aerial Robotics、Robotics: Mobility和Robotics: Computational Motion Planning。

1.卡尔曼滤波的动机（Kalman Filter Motivation）

如何利用卡尔曼滤波来进行机器人学的贝叶斯估计？

卡尔曼滤波器是一种广泛使用的针对线性系统的最优跟踪算法，一些跟踪的例子包括自动驾驶汽车追踪行人和车辆，或者追踪装配线（assembly line）中在传送带（conveyor belt）上运动的部件。

我们将讨论卡尔曼滤波器（Kalman filter）的系统模型和测量模型，接着我们将讨论滤波器中的最大后验估计，最后，我们将把线性卡尔曼的思想扩展到非线性模型。

我们将学习时间序列建模，利用卡尔曼滤波器来估计世界的状态。在机器人统计学中的高斯分布 这篇文章中，我们的足球机器人已经学会了检测球的颜色，但是，在一场球赛中，球不会只停在一个地方，因此机器人需要跟踪球的轨迹，甚至预测球未来的运动，许多现实中的机器人都需要做相同的估计。在下图中，小球不断在地面上滚动，但是由于摩擦力的原因，小球越来越慢，除了预测小球的位置，机器人需要辨识出球在环境中的动力学特性。

另一个常见的影响现实世界物体的力——重力。在下图中，小球正在加速朝着地面下降，跟踪球的滤波器应该能建立模型描绘这种行为。

机器人应该对不确定性有些概念，如果机器人不确定球是怎么动的，它就不可能准确的做出相应的行为。当机器人被放到现实世界中的时候，它将从摄像头获得稳定的视频流，应用在机器人统计学中的高斯分布 的原理，机器人应该能够从传感器数据中确定球的位置，但是由于噪声的存在，这个位置可能并不十分准确，因此，机器人将有一堆带有噪声的数据。这些数据可以在任何时候近似地给出小球真实的状态，这一部分的内容重点就是计算这些状态。

我们应该分清测量结果的概念与真实状态的概念，真实世界有其投影，但是机器人只能观察到真实世界的一个投影。例如，球距离机器人的真实距离是11米，但是机器人认为距离是11.32567米。机器人通过摄像头观察到这个位置，而这个从摄像头获得的测量给出了对真实状态的带有噪声的估计。一个可能的噪声来源是代表小球的像素和代表周围区域被错误的识别了。机器人为了做出行动决策，关心的是隐藏状态的高层（high level）概念。

例如，足球机器人只关心球的位置、速度。对一些无人机，姿态就变成了一个特别重要的状态，它可以通过惯性测量单元或GPS进行测量。灭火机器人可能希望追踪火焰，这需要温度的信息来增加对颜色和大小的估计。每种任务都是不同的，所以搞清楚状态由什么构成非常重要。

在这部分我们只需要跟踪球的位置和速度，因此我们的状态由x、y、z坐标和dx、dy、dz组成，当我们测量状态时，我们并不总能获得与状态的形式相一致的信息，例如跟踪小球的时候，我们只对位置进行了观察，没有对速度的直接测量。这些测量与实际状态的差别使追踪变得困难。如下图所示，右图为机器人跟踪红色小球的位置曲线，可以发现数据包含了很多的噪声。

2.卡尔曼滤波模型（Kalman Filter Model）

动力系统和一般滤波器背后的测量模型。

2.1 线性模型（Linear Modeling）

动力系统的数学形式还有概率组成了模型的运动和噪声。为了启发大家，这里使用一个位置跟踪的例子，为了跟踪一个动目标，机器人必须建立其运动的动力系统的模型。动力系统描述了目标实时变化的状态以及机器人如何测量这些状态。

在这个简单的例子中，状态 $x_t$ 表示 $t$ 时刻的状态 $x$, 状态量包括以米为单位的位置$v$ 和 以米每秒为单位的速度 $dv/dt $。由于其动力学特点，状态随着每个时间点而改变。

从现在的时间点$t$到下一点时间点$t+1$，这个变化由状态转移矩阵$A$来表示，状态转移矩阵结合了状态信息，来描述下一个时间点的状态。这种转换简化了现在的状态，使其仅仅依赖之前的状态，这样使得数学问题更加简单。

通过位置和速度的状态，我们知道位置必须基于速度才能实时变化，状态转移矩阵通过给定的公式表现这种关系。

机器人不是直接测量$x$，而是通过它的传感器观测$x$的部分,这个部分称为$z$, 这里状态和测量之间的关系由混合矩阵$C$来表示, 即$z_t=C{x}_t$。 为了完整性，$u$也考虑在内，$u$ 代表不依赖于状态$x$的外部输入, 我们在这一模块中不考虑 $u$，所以将其设为0, 即 $x_{t+1}=A{x}_t+B{u}_t$

在这个模型中X和Z均包含了噪声，状态X是有噪声的，因为线性模型没有捕捉到所有的物理学的相互作用。观察量Z有噪声，因为传感器在 测量时会收到噪声干扰。

2.2 卡尔曼滤波：贝叶斯滤波（KF：Bayesian filtering）

基于动力学模型，我们可以建立一个信息表。在任意给定的时间，我们知道俩部分信息，前一个状态$t-1$时刻的状态$x_{t-1}$和现状态的测量量$t$时刻的观测量$z_t$。有了这俩个信息后，我们想计算出现在$t$ 时刻的状态 $x_t$。

**1).**值得注意的是，我们不相信测量结果中的任意单一的量。为了获得包含这种不确定的估计，我们将这个动力学模型转换成一系列概率表达。给定了系统动力学量，基于前一状态概率条件之上的现在状态的概率，即 $p(x_t|x_{t-1})$，这意味这我们当前的状态不能离之前的状态太远。
还有一件值得注意的是，要把传感器得到的 $Z$ 和真实状态 $x$ 联系起来。我们的传感器仅仅给我们单一测量值，但是我们想要一个概率分布，为了完成这个任务，我们估计了基于t时刻状态条件下获得当前测量值的条件概率 $p(z_t|x_t)$。例如，我们正前方的一个球提供了一个方差很小的确切估计（certain estimation），然而，有一定距离的观察会有一个比较大的方差，因为我们不知道准确距离是多少。

我们将继续使用高斯分布作为数学模型，对于一般滤波器，这意味着选择一个猜测性的机制，建立含有均值和用n表示的方差这样状态的模型 $p(x_t)=N(x_t,P_t)$。

==========================================================================
Prediction using state dynamic model
$p(x_t|x_{t-1})$

Inference from noisy measurements
$p(z_t|x_t)$

Model $x_t$ with a Gaussian (mean and covariance)
$p(x_t)=N(x_t,P_t)$

==========================================================================

2). 现在我们有俩个动力系统模型和概率模型，我们可以结合俩个想法。
首先，线性动力系统模型表示我们可以用状态转移矩阵$A$来建立基于现在状态的下个状态的条件概率, 即$p(x_t|x_{t-1})=Ap(x_{t-1})$。 此外，t时刻的测量值z的概率可以用动力系统模型的矩阵$C$来建立关系，即$p(z_t|x_t)=Cp(x_t)$。

我们通过新的m和o将噪声添加到测量(measurements)和观测(observation)系统中。使用高斯概率分布模型，状态和噪声可以用均值和方差来表示，即$p(x_t|x_{t-1})=AN(x_{t-1},P_{t-1})+N(0,{\Sigma }_m)$, $p(z_t|x_t)=CN(x_t,P_t)+N(0,{\Sigma }_o)$

==========================================================================
Apply linear dynamics
$p(x_t|x_{t-1})=Ap(x_{t-1})$
$p(z_t|x_t)=Cp(x_t)$

Add noise for motion and observations
$p(x_t|x_{t-1})=Ap(x_{t-1})+v_m$
$p(z_t|x_t)=Cp(x_t)+v_o$

Introduce Guassian model of x_{t-1} and x_{t}
$p(x_t|x_{t-1})=AN(x_{t-1},P_{t-1})+$