卡尔曼SLAM

B是初始位置然后走了一步到X位置P（B）（在初始位置的可能性）*P（X|B）（在B初始位置可能性的基础上X的可能性）=P（X）（在X位置的可能性）

在预测的同时得到了观测Ｚ，B到X传感器得到观测数据Z，然后根据观测（P（Z｜X，B））和预测（P（X｜B））更新ｐ（Ｘ｜Ｚ，Ｂ）（贝叶斯）

预测的分布

观测的分布

举个日常生活的例子：

假设你有两个传感器，测的是同一个信号。可是它们每次的读数都不太一样，怎么办？取平均。

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

怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。

接下来，重点来了：

假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。

俩个直接相乘就得到融合的分布

融合之后的期望等于Xｐ的期望加上修正。δｐ表示预测的方差（不确定性），δｍ表示观测的方差（不确定性）δｐ＾２／（δｐ＾２＋δｍ＾２）权重，决定到底该相信谁，δｐ大（不确定性大），权重大，观测占主要，δｐ小（不确定性小），权重小，预测占主要

Pｒｅｄｉｃｔ（前两个式子）　为单纯的预测值，然后用测量值来修正预测值

红框和蓝框分别对应。上下式子的对应关系，P为δｐ＾２　H就是H　Ｒ为δｚ＾２，－１次方为除，Kｋ和Sｋ组成卡尔曼增益K，Update　state　estimate为Xｆ，Update　estimate　covariance为δｆ＾２．

卡尔曼在Slam中的H矩阵就是把像素坐标点变成三维坐标点的变换矩阵

计算代价大，路标越多计算代价越大，所以需要粒子滤波器

运动模型（预测）和传感器观测融合

猜自己的起始位置然后开始运动，每个起始位置都是一个粒子，每个粒子的运动是一样的，有航向，得到基于航向的观测，观测与传感器得到的信息比较（传感器信息已知）根据准确赋予权重，让观测收敛，一些粒子看到的是对的，一些是错的丢弃，基于对的再散点采样，不停的迭代，直到收敛。