最近稍闲,稍微整理了一下以前的部分代码,虽然写得不够好,但是对于新手也许也有一定的分享价值。具体算法细节我就暂时不讲了,网上太多了。
所以分享这个我用JAVA写的一个模拟器。模拟定位和惯导(或者pdr)利用粒子滤波和卡尔曼滤波的可视化小demo,代码放在GitHub上了,如果觉得有用请点星星:-D.
先上一张运行图,其中图上的各种颜色点点标识不同的算法定位结果,和右边显示的颜色对应。未有说明的红色标识真实位置,后面的定位结果图都将以红色为参照标准:
为了方便对比,先看单独模拟INS(pdr)的效果部分(紫色点),把其他定位结果先隐藏。情况还是比较符合预期,INS刚开始比较准,然后慢慢出现偏差累积。
再来看看单独利用MLE(最大似然估计)的定位结果:其实MLE定位结果(黄色点)统计值是不错的,但是因为其跳动和不连续的特征,导致体验不好也就是很难看出是一条连续路径。
所以我们需要把两种方法的定位结果通过KF或者PF进行融合如下两张图所示,弥补互相的不足。这样连续性和精度上面都有提高。
在这一段模拟上面可以看到平均精度卡尔曼滤波最好,INS还在走向发散。这里的粒子滤波出现了比较严重的粒子退化现象,感兴趣的童鞋可以调整一下参数。
作者:LeslieXong