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

　　　　 $\color{blue}{\bf本文由}$ $\color{red}{\it @David Han}$ $\color{blue}{ \bf出品 ,转载请注明出处}$
　　　　 $\color{green}{\bf文章链接：}$ 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) = F x (t - 1)

$x(t)=Fx(t-1)$
t=0时，小车位置服从红色的正态分布
这里写图片描述

我们通过数学模型，预测t=1时刻的小车位置应该是
这里写图片描述

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

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

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

航迹推演

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

这里写图片描述

假设两轮间距是 $L$
已知 $\Delta s_r$ , $\Delta s_l$ 和 $\Delta t$ ,在很短的距离内，我们利用已直代曲的思想，将 $\Delta s$ 近似看成 $\Delta d$
这里写图片描述
主要的公式：

\begin{array}{l} Δ d = Δ s = \frac{Δ s_{r} + Δ s_{l}}{2} \\ Δ θ = \frac{Δ s_{r} - Δ s_{l}}{L} \\ Δ x = Δ s * c o s (θ + Δ θ / 2) \\ Δ y = Δ s * s i n (θ + Δ θ / 2) \end{array}

$\begin{array}{l} \Delta d =\Delta s =\frac{\Delta s_r +\Delta s_l }{2}\\ \Delta \theta =\frac{\Delta s_r -\Delta s_l }{L} \\ \Delta x= \Delta s*cos(\theta+\Delta \theta/2 )\\ \Delta y = \Delta s*sin(\theta+\Delta \theta/2 )\\ \end{array}$
这里写图片描述

位移量

x, y

$x,y$
补充：

v = \frac{Δ s}{Δ t} = \frac{v_{r} + v_{l}}{2}

$v=\frac{\Delta s}{\Delta t} =\frac{ v_r+v_l}{2}$

w = \frac{Δ s_{r} - Δ s_{l}}{Δ t * L} = \frac{v_{r} - v_{l}}{L}

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

编码器500线，通过16:1的减速器，之后两相输入驱动器，一个周期有4个上升沿，因此电机转一圈，产生32000个脉冲。
上面的公式当中

Δ s_{r}, Δ s_{l}

$\Delta s_r, \Delta s_l$ ，可以通过统计

Δ t

$\Delta t$ 时间内，编码器产生的脉冲数，来计算

Δ s_{r}, Δ s_{l}

$\Delta s_r, \Delta s_l$ 的位移量。
假定

Δ t

$\Delta t$ 时间内，左轮的脉冲个数是

Δ N_{l}

$\Delta N_l$ ,右轮的脉冲个数是

Δ N_{r}

$\Delta N_r$ ,轮子的直径是

D

$D$

Δ s_{l} = \frac{Δ N_{l} * D * π}{32000}

$\Delta s_l=\frac{\Delta N_l*D*\pi}{32000}$

Δ s_{r} = \frac{Δ N_{r} * D * π}{32000}

$\Delta s_r=\frac{\Delta N_r*D*\pi}{32000}$

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

贝叶斯准则

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

p (x | y) = \frac{p (x, y)}{p (y)}

$p(x|y)=\frac {p(x,y)}{p(y)}$
贝叶斯准则:
这一步，可以通过条件概率逆推过去。称为两个恒等式

p (x | y) = \frac{p (y | x) p (x)}{p (y)}

$p(x|y)=\frac {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)=\eta p(y|x)p(x)$
在移动机器人运动的过程中，如果我们想求x点在t时刻的位置。那么我们需要知道x在t-1时刻的位置，t-1时刻的测量值和t 时刻的控制动作。用数学公式表达就是：

p (x_{t} | x_{0 : t - 1}, z_{1 : t - 1}, u_{1 : t})

$p(x_t|x_{0:t-1},z_{1:t-1},u_{1:t})$
这里的0:t-1表示的是，从0时刻到t-1时刻。