粒子滤波的可视化

粒子滤波的可视化

前段在学习粒子滤波，发现这是一个实现起来相当优雅的算法，但是在内外网看了一圈少有看到将其进行可视化的，于是做了一定的可视化的工作。实现了一个基本的激光粒子滤波仿真。在此做下记录。
环境：windows + python3.9
绘图库：matplotlib（u1s1，matplotlib画图是真的慢）
思路：建立矩形世界模型，理论上可以拓展出任意复杂的环境模型
先上效果图。
第一步进行全局撒点，这一步我设置的撒点数较低。

各个不同颜色的点所表示的什么图中已标注完全，注意黄色的线代表激光（实际上的激光线很稠密，但是可视化时只画了一部分）黑色框代表标志物（可以看做可以穿透的墙），机器人所在环境是循环的（便于编程）
经过一定步骤后可以发现位置估计效果良好。

当然，在这样一个高度相似的环境中其实更多时候很容易估计错误，后续有兴趣我会加上一些其他的环境特征，比如像下面这样。

显然，这是一个很容易发生的状况，对此有其他的解决办法，比如UWB，下面是在某一次测试下不同策略下的估计误差。

【参考】

1. Matlab的仿真，参考了它的地图并由此得到了一点可视化的思路（没错，只是参考了地图的样子与粒子的效果，matlab封装太多了不好改）。
   https://www.youtube.com/watch?v=NrzmH_yerBU
   https://ww2.mathworks.cn/help/nav/ug/localize-turtlebot-using-monte-carlo-localization.html
2. Udacity的这门课程，推荐