SLAM重要概念，通俗讲解（持续更新中）

　　　　 本文由 本 文 由 @DavidHan @ D a v i d H a n 出品,转载请注明出处 出 品 , 转 载 请 注 明 出 处
　　　　 文章链接： 文 章 链 接 ： http://blog.csdn.net/david_han008/article/details/78573616
　　　　
　　　　

卡尔曼滤波

什么是卡尔曼滤波？在知乎上见到一些有趣的回答。参考原文

假设，你有两个传感器，测量同一个信息，可是他们每次的读数都不太一样，怎么？

取平均

再假设，你知道其中那个贵的传感器应该更加准确一些，便宜的那个应该差一些，那有比取平均更好的办法吗？

加权平均

那么，怎么加权呢？假设两个传感器的误差都符合正态分布，假设你知道两个正态分布的方差，利用这两个方差，你可以得到一个最优的权重。

接下来重点来了，假设你只有一个传感器，但是你还有一个数学模型，模型可以帮助你算出一个值，但是这个值也不是那么准，怎么办？

把模型算出来的值和传感器测量的值，（就像两个传感器一样），取加权平均

OK，最后说明一点：你的数学模型其实只是一个迭代的。也就是说，知道x(k)，才可以求出x(k+1)。问题是x(k)是多少呢？答案是：x(k)就是你上一步卡尔曼滤波得到的值

这就是卡尔曼滤波。

还有第2种，有趣的回答：
假设在轨道上，有一辆小车，在无外力的作用情况下，小车在t时刻的状态量x(t)只与x(t-1)有关

x(t)=Fx(t−1) x ( t ) = F x ( t − 1 )

t=0时，小车位置服从红色的正态分布

我们通过数学模型，预测t=1时刻的小车位置应该是

为什么正态分布变得矮胖了，原因很简单，因为叠加了一层噪声（或者说不确定性）。
为了避免数学模型带来的误差。我们使用激光雷达在t=1时刻，进行了测量，下面图中的蓝色的部分

好了，我们现在得到两个不同的结果，那么，我们现在进行卡尔曼滤波融合，找到相应的权值。就像下面这个图所示。红色和蓝色合并为下面的绿色部分。

并且真正的牛逼的之处，在于加权值之后的绿色部分，仍然符合正态分布，可以作为第二次位置估计的初值，一次迭代下去。

航迹推演

航迹推演涉及的是小车的运动学模型，在这里涉及到的航迹推演算法是，已知左右轮的速度，而来的（还有其他航迹推演的方法，例如已知角速度）

假设两轮间距是 L L
已知 Δsr Δ s r , Δsl Δ s l 和 Δt Δ t ,在很短的距离内，我们利用已直代曲的思想，将 Δs Δ s 近似看成 Δd Δ d

主要的公式：

Δd=Δs=Δsr+Δsl2Δθ=Δsr−ΔslLΔx=Δs∗cos(θ+Δθ/2)Δy=Δs∗sin(θ+Δθ/2) Δ d = Δ s = Δ s r + Δ s l 2 Δ θ = Δ s r − Δ s l L Δ x = Δ s ∗ c o s ( θ + Δ θ / 2 ) Δ y = Δ s ∗ s i n ( θ + Δ θ / 2 )

位移量 x,y x , y
补充：

v=ΔsΔt=vr+vl2 v = Δ s Δ t = v r + v l 2

w=Δsr−ΔslΔt∗L=vr−vlL w = Δ s r − Δ s l Δ t ∗ L = v r − v l L

注：这些公式不难，直接在STM32当中计算出来
有了上面的推导公式，不难写出小车的线速度是多少，角速度是多少，然后在代码里面，只需要对上面的数值进行累加即可。
附 编码器计算里程：
购买的电机： 点击这里
配套的驱动器： 点击这里
重要结论， 电机转一圈 产生32000QC（也就是32000个脉冲）
计算过程如下：

编码器500线，通过16:1的减速器，之后两相输入驱动器，一个周期有4个上升沿，因此电机转一圈，产生32000个脉冲。
上面的公式当中 Δsr,Δsl Δ s r , Δ s l ，可以通过统计 Δt Δ t 时间内，编码器产生的脉冲数，来计算 Δsr,Δsl Δ s r , Δ s l 的位移量。
假定 Δt Δ t 时间内，左轮的脉冲个数是 ΔNl Δ N l ,右轮的脉冲个数是 ΔNr Δ N r ,轮子的直径是 D D

Δsl=ΔNl∗D∗π32000 Δ s l = Δ N l ∗ D ∗ π 32000

Δsr=ΔNr∗D∗π32000 Δ s r = Δ N r ∗ D ∗ π 32000

注：在实际写代码的时候，千万要注意单位
例如： cos(θ) c o s ( θ ) 中，在C++当中 cos(弧度),要格外注意，另外，在ROS当中，yaw的单位也是弧度，长度的单位是米，当角速度为正值的时候，x轴正方向，转向y轴的正方向。

贝叶斯准则

目前，我还么有找到一种很好的方式来解释。不如公式推导：
条件概率的定义：

p(x|y)=p(x,y)p(y) p ( x | y ) = p ( x , y ) p ( y )

贝叶斯准则:
这一步，可以通过条件概率逆推过去。称为两个恒等式

p(x|y)=p(y|x)p(x)p(y) p ( x | y ) = p ( y | x ) p ( x ) p ( y )

在上面的公式当中，我们通过y来求x的估计量。这里y一般是指的data.也就是传感器的测量值。p(x)称为先验概率分布，p(x|y)称为后验概率分布。p(y|x)称为生成模型。由于p(y)是相同的变量，因此1/p(y)可以被写成贝叶斯准则中的统一化变量。，通常用n来表示，因此上面公式表示为：

p(x|y)=ηp(y|x)p(x) p ( x | y ) = η p ( y | x ) p ( x )

在移动机器人运动的过程中，如果我们想求x点在t时刻的位置。那么我们需要知道x在t-1时刻的位置，t-1时刻的测量值和t 时刻的控制动作。用数学公式表达就是：

p(xt|x0:t−1,z1:t−1,u1:t) p ( x t | x 0 : t − 1 , z 1 : t − 1 , u 1 : t )

这里的0:t-1表示的是，从0时刻到t-1时刻。