Kalman Filter

Kalman Filter是訊號處理裡很常見的濾波器,特別針對是會有雜訊干擾或偏移誤差的感測器做濾波。
主要有兩個步驟:

未命名

以下是簡單的測試程式: 

double P = 10; // estimate covariance
double R = 1000; // covariance of observation noise
double Q = 1; // covariance of process noise
double kg; // Kalman gain
double estimate = Input[0], measure;
Output[0] = Input[0];
for (int i = 1 ; i < 80 ; i ++)
{
// predict
measure = Input[i];
P = P + Q;
// update
kg = P / (P + R);
estimate = estimate + kg * (measure - estimate);
P = (1 - kg) * P;
Output[i] = estimate;
}

當我改變coveriance noise(Q)值時,可以決定我要濾波的程度,Q越大濾波程度越強

map2[Q=2]

map[Q=15]

map2[Q=1000]

Kalman Filter是個簡單好用的濾波器,但想要依照所需,動態調整濾波程度,這又是另一回事了,因為要分辨是signal還是noise這就是個永遠也解不開的難題。

 

Leave a Reply

Your email address will not be published. Required fields are marked *