B是初始位置然后走了一步到X位置P(B)(在初始位置的可能性)*P(X|B)(在B初始位置可能性的基础上X的可能性)=P(X)(在X位置的可能性)
在预测的同时得到了观测Z,B到X传感器得到观测数据Z,然后根据观测(P(Z|X,B))和预测(P(X|B))更新p(X|Z,B)(贝叶斯)
预测的分布
观测的分布
举个日常生活的例子:
假设你有两个传感器,测的是同一个信号。可是它们每次的读数都不太一样,怎么办?取平均。
再假设你知道其中贵的那个传感器应该准一些,便宜的那个应该差一些。那有比取平均更好的办法吗?加权平均。
怎么加权?假设两个传感器的误差都符合正态分布,假设你知道这两个正态分布的方差,用这两个方差值,(此处省略若干数学公式),你可以得到一个“最优”的权重。
接下来,重点来了:
假设你只有一个传感器,但是你还有一个数学模型。模型可以帮你算出一个值,但也不是那么准。怎么办?把模型算出来的值,和传感器测出的值,(就像两个传感器那样),取加权平均。OK,最后一点说明:你的模型其实只是一个步长的,也就是说,知道x(k),我可以求x(k+1)。问题是x(k)是多少呢?答案:x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。
俩个直接相乘就得到融合的分布
融合之后的期望等于Xp的期望加上修正。δp表示预测的方差(不确定性),δm表示观测的方差(不确定性)δp^2/(δp^2+δm^2)权重,决定到底该相信谁,δp大(不确定性大),权重大,观测占主要,δp小(不确定性小),权重小,预测占主要
Predict(前两个式子) 为单纯的预测值,然后用测量值来修正预测值
红框和蓝框分别对应。上下式子的对应关系,P为δp^2 H就是H R为δz^2,-1次方为除,Kk和Sk组成卡尔曼增益K,Update state estimate为Xf,Update estimate covariance为δf^2.
卡尔曼在Slam中的H矩阵就是把像素坐标点变成三维坐标点的变换矩阵
计算代价大,路标越多计算代价越大,所以需要粒子滤波器
运动模型(预测)和传感器观测融合
猜自己的起始位置然后开始运动,每个起始位置都是一个粒子,每个粒子的运动是一样的,有航向,得到基于航向的观测,观测与传感器得到的信息比较(传感器信息已知)根据准确赋予权重,让观测收敛,一些粒子看到的是对的,一些是错的丢弃,基于对的再散点采样,不停的迭代,直到收敛。