最近稍閑，稍微整理了一下以前的部分代碼，雖然寫得不夠好,但是對於新手也許也有一定的分享價值。具體算法細節我就暫時不講了，網上太多了。
所以分享這個我用JAVA寫的一個模擬器。模擬定位和慣導（或者pdr）利用粒子濾波和卡爾曼濾波的可視化小demo，代碼放在GitHub上了,如果覺得有用請點星星:-D.
先上一張運行圖，其中圖上的各種顏色點點標識不同的算法定位結果，和右邊顯示的顏色對應。未有說明的紅色標識真實位置，后面的定位結果圖都將以紅色為參照標准：
為了方便對比，先看單獨模擬INS(pdr）的效果部分（紫色點），把其他定位結果先隱藏。情況還是比較符合預期，INS剛開始比較准，然后慢慢出現偏差累積。
再來看看單獨利用MLE(最大似然估計）的定位結果：其實MLE定位結果（黃色點）統計值是不錯的，但是因為其跳動和不連續的特征，導致體驗不好也就是很難看出是一條連續路徑。
所以我們需要把兩種方法的定位結果通過KF或者PF進行融合如下兩張圖所示，彌補互相的不足。這樣連續性和精度上面都有提高。
在這一段模擬上面可以看到平均精度卡爾曼濾波最好，INS還在走向發散。這里的粒子濾波出現了比較嚴重的粒子退化現象，感興趣的童鞋可以調整一下參數。