各种机器人在实际应用中往往会面对一些共同的问题——状态估计和控制。

机器人的状态，是指一组完整描述它随时间运动的物理量，比如位置，角度和速度。本书的重点关注机器人的状态估计。

本书中，我们先介绍受高斯噪声影响下的线性系统状态估计的经典理论，然后我们介绍如何将它们扩展到非高斯的非线性系统中，我们还会详细介绍三维空间机器人的状态估计，并对旋转采用更加具有针对性的方法。

本章中，我们先简单介绍一些估计理论的历史，讨论不同类型的传感器与测量手段，最后引出什么是状态估计问题，末尾会推荐一些相关的阅读资料。

1.1 状态估计简史

早在4000年前的航海家就面临着一个状态估计问题——如何判断船只在大海中的位置？图表观测太阳—》指南针—》直角器、星盘、四分仪、六分仪、经纬仪等—》有准确的时间才可以准确地估计经度，1764年可携带的精确计时工具问世。

状态估计的起源，最早可以追溯到早期的天文学，高斯提出最小二乘法并用最小化观测误差对行星轨迹进行预测，误差仅在0.5度以内。随后在1809年他证明了在正态分布误差假设下，最小二乘即最优估计，这是太部分沿用至今的经典估计算法的源头。

20世纪中期，状态估计理论才真正起步，这和我们刚进入计算机时代密切相关。1960年，卡尔曼发表了两篇里程碑式的文章，指明了随后状态估计研究的大部分内容。首先，它引入了能观性的概念，也就是说动态系统的状态何时能够从该系统的一组观测值中推断出来；其次对于受到观测噪声影响的系统，他提出了一个估计系统状态的优化框架——卡尔曼滤波，它在航天等领域应用广泛。

最近15年，新的传感器如数字相机、激光成像、GPS等的出现，为这个古老的领域带来了新的挑战。

1.2 传感器，测量和问题定义

状态估计的过程，就是理解传感器本质的的过程。任何传感器的精度都是有限的，所以每个真实传感器的测量值都有不确定性。当我们想结合多种传感器的测量值来估计状态时，最重要的解释了解所有的不确定量，从而推断我们对状态估计的置信度。

我们把传感器分成内感受型和外感受型，内感受型是由自身刺激产生的反应，比如加速计，陀螺仪；外感受器是外部机体接受刺激产生的反应，如相机，激光，GPS。大致来说，我们可以将外感受器理解成测量运动主体的位置和朝向的装置，而内感受器则测量运动主体的速度或者加速度。

状态估计，是根据系统的先验模型和测量序列，对系统内在状态进行重构的问题。

1.3 本书的组织结构

• 1、状态估计机理，介绍经典和最流行的估计工具，它涵盖了递归状态估计方法和批量方法。本书也介绍贝叶斯方法应用于估计问题中，我们会对比全贝叶斯方法和最大后验估计方法(MAP)。最后还会涉及一些实践中的问题，比如鲁棒性估计和偏差。
• 2、三维空间运动机理，讲解三维空间几何基础，矩阵李群，三维物体的平移和旋转，还会讨论几何学，运动学，以及这些量（旋转和位姿）相关的概率论与统计方法。
• 3、应用，将分析大量的经典三维估计问题，包括如何估计三维空间中物体的平移和旋转。

1.4 与其他教程的关系

《概率机器人》

《Computational principles of mobile robotics》

《Mobile Robotics: Mathematics, Models, and Methods》

《Robotics, vision and control》

《Bayesian Filtering and Smoothing》

《Stochastic Models, Information Theory, and Lie Groups: Classical Results and Geometric Methods》