slam基本原理

这儿我用一个不算非常恰当但非常简单的例子帮助初学者对同时定位制图的计算原理有一个直观的了解。请参考上面basic_idea.png的图。
假设黑圆圈是一辆小车，方块A是环境中一个固定的点。小车从位置B运动到了位置C。小车在位置B时通过某种传感器测得A的位置是距离米，同时通过传感器和一些小的计算能知道AB和B运动方向之间的夹角即。运动到位置C后测得A的位置距离它是米，同样可以知道。这时候如果B点的坐标是，那三角形知道两边两角你能不能算第三边呢？
其实不仅我们可以通过测A和小车之间的距离最终了解的长度，我们当然还有传感器能测小车的速度之类的，假设小车从B运动到C只经历了很短的时间，我们测得小车在B点的速度延BC方向是，这很短的时间内我们可以假设小车的速度没有发生改变。那你能不能算边的长度呢？
可以说这两个问题都太简单了，但是如果你能完成你就已经完成SLAM即同时定位与制图了。A属于环境中的点，你能根据传感器确定它的位置，这就是制图；你根据小车在B的位置和一系列传感器数据以及小车的运动模型(它沿BC在极短的时间内坐匀速直线运动)推断出小车在C的位置，这就是定位。合起来就是Simultaneous Localization and Mapping。
说地这么简单，为什么实际想实现一个SLAM系统却没那么容易呢？我们来看看如果面临现实情况会有哪些问题。
Question1:我们说通过测量A相对于B，C的位置构成一个三角形能辅助我们算出，通过测得小车在B的速度以及假设它匀速直线运动我们也能算出。如果传感器完美测量没有任何误差，运动模型也完美，这两种方式算出的应该一样。但当然不会完美。假设测得的长度和真实值差0.01m,测得的长度和真实值差0.01m，那算出的长度就会和真实值有差距，我们把这个差距记为。如果小车在B测得的速度和真实的速度有0.001m/s的差距，小车从B到C也并不是完美的匀速直线运动，那算出的长度也会和真实值有差距，记为。传感器不同，肯定会不一样。那么我们该使用哪个值？
Question2:我们非常完美地假设能直接测得的长度，视觉SLAM中我们有相机作为传感器，普通的相机拍出的是一张图片，即二维平面，A位于这个平面中，我们怎么能确定这个平面中一个点到自己的具体距离？
Question3:假设我们已经能在小车在B点时算出A的位置，小车运动到C时算出A距离C的位置，但我怎么知道这两个A是一个A??有点绕口，这么说，小车在B点拍下了当前环境的照片，有一棵树，在C点拍下了照片，有同样一颗树，这两张照片储存在电脑中，但是电脑不会知道这是同一棵树。图片作为数字矩阵储存在电脑中，这两棵树在电脑里面都只是一堆不相关数字而已。
Question4:环境中不仅有点A，还有点M,N,L....，假设每一个点我们都能测出到小车的距离，但是由于传感器的测量有误差，并且测出的误差各不相同，有的差0.01m有的差0.0095米，有的差0.011米，环境中的每一个点都和B,C形成了一个三角形，每一个三角形都能算出的长度且由于误差算出的各不相同，那么和Question1类似的问题又来了，我们使用哪一个 ？
Question5:由于我们前面的说的测量的误差，无论我们选哪个，或者用某种方式把这些整合起来算一个新的（最简单的，加起来求个平均什么的），都不会是真实的值，仍然会和真实的值有一定误差。当我们计算出的C的位置和实际位置差0.01米，之后小车从C运动到D又算差0.011米，之后小车从D运动到E又差0.0095米...每一个是0.1s的话，如此累积，只需要10s小车的位置误差就能达到米级！误差的累积是不可避免的。面对这种情况，我们又该怎么办？