卡尔曼滤波器在移动机器人定位中的应用（上）

一个人的喜欢就是把自己对偶然间闪过的念想坚持，直到它变成一种习惯。——xiahouzuoxin

说实话，这样的人生确实比较有趣，但这个句子不是我说的。在下面，凭借着人类特有的懦弱与阿Q精神，任何人都可以被迫做到：)

我可以做到把自己持久性的不喜欢坚持，直到它变成一种习惯和喜欢。——肥鼠路易

Overview and Motivation

The study of mobile robots is an intrinsically interdisciplinary research area involving the following:
Mechanical engineering—vehicle design and in particular locomotive mechanisms.
Computer science—representation,sensing,and planning algorithms.
Electrical engineering—system integration,sensors,and communications.
Cognitive psychology,perception,and neuroscience—insights on how biological organisms solve similar problems.

位移传感器

绝对位移传感器：输出是相对于一个绝对（固定）参考点的位置，传感器断电再恢复供电时，无需复位
相对位移传感器：增量式位移传感器（如增量式编码器，增量式光栅尺），断电后需要进行系统复位，才能继续测量。
位移与加速度是可以互相转换的。加速度传感器测得的数据经二次积分处理，得到的就是位移量，同样的位移传感器测得的数据经二次微分处理，得到的数据为加速度值。其实不管使用加速度测量位移或者位移测量加速度在实际应用中都非常少见，主要原因信号转换过程要求采集系统必须具备相应的微分或者积分运算能力，而不管位移传感器或者加速度传感器都是极为常用的产品，还不如直接选用相关的传感器去测量对应的物理量划算。
--------------------------------------------------------------------------- ——作者：米朗位移传感器

思考移动机器人的定位问题：假设我们的移动机器人只在平面上移动，那么要确定的无非是机器人的位置和转向
$$X=[x,y,\theta {]}^T$$
在这里，我们运用机器人定位研究领域中最常见的算法，卡尔曼滤波器。

卡尔曼滤波器

TA的典型实例从一组有限的,包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。
卡尔曼滤波模型假设(k+1)时刻的真实状态$X_{k+1}$是从k时刻的状态$X_k$演化而来。（相关矩阵符号含义见维基百科）
$$X_{k+1}=F_{x+1}{X}_k+B_{k+1}{u}_{k+1}+w_{k+1}$$
$$Z_{k+1}=H_{k+1}{X}_{k+1}+v_{k+1}$$
其中，$F_{x+1}$相当于是状态转移矩阵。$F_k$和$B_k$是