【筆記】卡爾曼濾波器C程序

一.濾波器

1.卡爾曼濾波器

• 卡爾曼濾波器——“最佳線性濾波器”
• 卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。

float kalmanFilter(float *Original_Data) 
{
    static float prevData=0; 
 	static float p=10, q=0.0001, r=0.005, kGain=0;
	p = p+q; 
	kGain = p/(p+r);
	
	*Original_Data = prevData+(kGain*(*Original_Data-prevData)); 
	p = (1-kGain)*p;
	
	prevData = *Original_Data;
	
	return *Original_Data; 
}

測試效果：

• 綠線爲採集到的CPU溫度值
• 紅線爲濾波後CPU的溫度值