3.3確定卡爾曼濾波算法的初始值

　　卡爾曼濾波算法作為一個迭代過程，需要賦予其初值，初值的選擇至關(guān)重要，如果初值選擇不合適，就不能滿足收斂性的要求。在CV模型中，P(0|0)的確定方法已經(jīng)由模型給出，這里只需給出X(0|0)的取值，本文中取X(0|0)=[10，-0.4]'.

　　4仿真實驗與結(jié)果分析

　　在完成上述準備工作后，筆者在Matlab軟件中進行仿真實驗，仿真實驗流程如圖3所示。

　　圖3仿真實驗流程圖

　　根據(jù)仿真實驗流程圖，在Matlab中先對模擬出的目標位置信息進行卡爾曼濾波處理，如圖4和圖5所示，這里的目標指的是所測障礙物。首先，從圖4和圖5可以看出：經(jīng)卡爾曼濾波處理后的目標位置的估計值在前2 s偏離真實值較遠，從第4 s以后，無論觀測值如何波動，估計值曲線均能很好地跟蹤真實值曲線，說明卡爾曼濾波算法起到了良好的濾波效果。

　　將融合處理前后，目標位置的估計值曲線和目標位置估計值的方差曲線分別置于同一幅圖中，如圖6所示，通過對比反映STF融合算法的優(yōu)點。從圖6( b)中可以看出：融合處理后，目標位置估計值的方差變小，說明融合處理后對目標位置的估計更加準確。從圖6中可以發(fā)現(xiàn)，融合曲線介于雙目視覺傳感器的估計值曲線和超聲測距傳感器的估計值曲線之間，且更加靠近準確度高的超聲測距傳感器的估計值曲線。

　　在本文所引文獻中，實驗驗證環(huán)節(jié)均在具體的應用場景下進行，實驗結(jié)果是移動機器人能夠進行無礙行走，文中均未給出具體的測量精度。本文仿真實驗的結(jié)果表明：融合處理后，測量精度可達4.6 cm，滿足了仿生四足機器人對測距的精度要求。

　　圖4雙目視覺傳感器系統(tǒng)的卡爾曼濾波

　　圖5超聲測距傳感器系統(tǒng)的卡爾曼濾波

　　圖6融合前后目標位置估計值曲線和方差曲線的對比