目標跟蹤的卡爾曼濾波算法分析案例目錄TOC\o"1-3"\h\u9785目標跟蹤的卡爾曼濾波算法仿真分析案例 1139021.1系統模型 173861.2濾波過程 183241.3算法仿真分析 3154981.4擴展卡爾曼濾波算法 6系統模型應用KF的系統噪聲是高斯分布的，且它的狀態方程和觀測方程必須滿足線性關系，這樣的卡爾曼濾波算法才能達到最優的估計效果。為了更清晰地描述系統的模型，將離散時間系統的動態模型的目標狀態方程和觀測到目標的狀態方程用數學模型表達如下 其中，是目標運動的一個狀態向量，表示的是在第個時刻的目標狀態；是階的一個狀態轉移矩陣；表示為階的一個過程噪聲矩陣；是觀測方程，表示在第時刻的其對應的觀測值；表示系統輸出的狀態；是一個階的觀測矩陣；，表示具有均值為零的獨立隨機過程，即高斯白噪聲，且對滿足 式中，為過噪聲方差；為觀測噪聲方差；是狄拉克函數；從式中可看出，是互不相關的。濾波過程卡爾曼濾波的主要思想是：在已知當前時刻的狀態的情況下，去預測下一個時刻的狀態值，再用當前時刻的值和所預測到的值進行對比校正，從而獲得當前時刻的更新值，這個更新的值更接近于真實值。大多按“預測-測量-更新”的方式遞推進行，遞推卡爾曼公式如下狀態一步預測 預測的協方差矩陣 卡爾曼增益 狀態更新 誤差協方差矩陣更新 在上式中，表示最佳一步預測值，為預測協方差矩陣，為最優濾波增益，為最佳濾波值，為平滑估計協方差矩陣，是一個的單位矩陣。上述遞推過程的具體過程可用REF_Ref69753063\h圖2.1表示圖STYLEREF1\s2.SEQ圖\*ARABIC\s11KF濾波算法流程框圖算法仿真分析假設目標在平面上作勻速直線運動，設聲吶采樣時間為,在采樣時刻為時，為目標在時刻的真實位置，為傳感器在時刻的觀測值，則其觀測模型為 式中為觀測噪聲，假設觀測噪聲是均值為零的高斯白噪聲，假設目標的速度為，其加速度為，則有 在忽略目標自身的動力因素的情況下，即隨機加速度，是由于海洋的各個因素造成的加速度，將它假設為均值為零的高斯白噪聲，且獨立于。假設目標在采樣時刻的系統狀態含有觀測目標的位置和速度，即 則目標運動狀態方程為 觀測方程為 對應系統狀態模型式和可得 ，， 若將勻速直線運動的目標系統推廣到四維，則其狀態向量為 式中的狀態向量包含水平和垂直方向的位置和速度，則系統方程表示為 假設目標在二維的坐標系的海面上運動，目標的初始位置為，初始速度為，采樣周期為，采樣頻率為，觀測噪聲為均值為零，方差為；過程噪聲方差取,,三種情況。其估計偏差用均方根誤差（RMSE）表示，定義為 式中，目標真實位置為，目標觀測位置為，為采樣次數。根據上述的條件，對KF濾波跟蹤進行驗證，其仿真結果圖如REF_Ref69896506\h圖2.2所示，REF_Ref69896506\h圖2.2是過程噪聲方差為時目標跟蹤的效果圖，從圖中可看出卡爾曼濾波的軌跡接近真實軌跡，其觀測的點也大多分散在真實軌跡那條直線的周圍，驗證了KF算法的估計值與真實值的正確性。REF_Ref69903608\h圖2.3是過程噪聲方差時，KF濾波前后的位置估計偏差對比圖，從圖中看出經過濾波后的誤差大大降低了,其誤差值也比較集中，波動范圍不大；REF_Ref69903276\h圖2.4REF_Ref70106695\h~2.6分別是，，的目標跟蹤軌跡的效果圖，隨著過程噪聲方差的增大，其跟蹤效果也降低了，真實軌跡漸漸彎曲；REF_Ref70106945\h圖2.7是過程噪聲方差取,,時，其目標跟蹤的誤差對比圖，從圖中可看出過程噪聲方差越小，目標跟蹤誤差也低。圖STYLEREF1\s2.SEQ圖\*ARABIC\s12目標真實、觀測和KF濾波后軌跡對比圖圖STYLEREF1\s2.SEQ圖\*ARABIC\s13濾波前后的誤差對比圖圖STYLEREF1\s2.SEQ圖\*ARABIC\s14目標真實、觀測和KF濾波圖圖STYLEREF1\s2.SEQ圖\*ARABIC\s15目標真實、觀測和KF濾波對比圖圖STYLEREF1\s2.SEQ圖\*ARABIC\s16目標真實、觀測和KF濾波對比圖圖STYLEREF1\s2.SEQ圖\*ARABIC\s17不同的過程噪聲方差對比圖擴展卡爾曼濾波算法上一節對KF的目標跟蹤和影響其濾波的因素，這為 EKF奠定了基礎。線性系統是KF的作用環境，對于非線性系統的濾波問題，后來學者們提出了擴展卡爾曼算法，EKF將非線性系統問題轉化為近似線性的問題。EKF濾波主要圍繞濾波值將和展開成泰勒級數，對于展開的泰勒級數將其二階以上的余項忽略掉，那么剩下的一階的部分就可看似一個線性函數,然后用KF進行濾波。EKF和KF的濾波過程及其相似。EKF濾波主要處理的是過程噪聲為高斯白噪聲的非線性系統，假設離散非線性系統的動態方程表示為 式為狀態方程，為觀測方程，表示目標的第時刻的狀態向量，為離散時間；和均為非線性函數；是均值為零，方差為的過程噪聲；表示第時刻的觀測向量；為噪聲驅動矩陣；是均值為零，方差為的觀測噪聲，這里和相互獨立。將式和中的非線性函數和對進行一階Tayor展開得狀態方程為 觀測方程為 其中，是對非