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