Kalman濾波算法測試
一、Kalman濾波算法簡介
卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生于匈牙利首都布達佩斯。
1953，1954年于麻省理工學院分别獲得電機工程學 士及碩士學位。
1957年于哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源于他的博士論文和1960年發表的論文
《ANewApproachtoLinearFilteringandPredictionProblems》（線性濾波與預測問題的新方法）。
卡爾曼濾波 Kalman filtering
一種利用線性系統狀态方程，通過系統輸入輸出觀測資料，對系統狀态進行最優估計的算法。由于觀測資料中包括系統中的噪聲和幹擾的影響，是以最優估計也可看作是濾波過程。
表達式 X(k)=A X(k-1)+B U(k)+W(k)
Q:過程噪聲，Q增大，動态響應變快，收斂穩定性變壞
R:測量噪聲，R增大，動态響應變慢，收斂穩定性變好
二、C語言實作
基于LPC1768最小系統硬體平台，内部模拟産生正弦輸入信号，通過配置不同的Kalman系數進行濾波測試。核心濾波算法C語言部分如下：
#if TKIT_KALMAN_EN
TYPE_KM fAlg_KalmanFilter ( const TYPE_KM SrcData,
const TYPE_KM ProcessNiose_Q,
const TYPE_KM MeasureNoise_R,
TYPE_KM * x_p_last,
TYPE_KM * p_p_last)
{
TYPE_KM x_last=*x_p_last;
TYPE_KM p_last=*p_p_last;
TYPE_KM x_mid, x_now;
TYPE_KM p_mid, p_now;
TYPE_KM kg;
x_mid = x_last;
p_mid = p_last+ProcessNiose_Q;
kg = p_mid/(p_mid+MeasureNoise_R);
x_now = x_mid+kg*(SrcData-x_mid);
p_now = (1-kg)*p_mid;
*p_p_last = p_now;
*x_p_last = x_now;
return x_now;
}
#endif //TKIT_KALMAN_EN
三、模拟輸入和測試
模拟方法是通過配置不同的輸入參數ProcessNiose_Q和MeasureNoise_R，以及内部模拟産生原始正弦信号。繪制原始資料以及經過濾波處理之後的資料進行對比，如下。
其中fx是輸入的原始資料，fy是處理過後的資料。
posted on 2019-06-11 10:47 LewisMountain 閱讀(...) 評論(...) 編輯 收藏