卡爾曼濾波器由一系列遞歸數(shù)學公式描述。它們提供了一種高效可計算的方法來估計過程的狀態(tài)，并使估計均方差最小?？柭鼮V波器應用廣泛且功能強大：它可以估計信號的過去和當前狀態(tài)，甚至能估計將來的狀態(tài)，即使并不知道模型的確切性質(zhì)。
從本質(zhì)上來講，濾波就是一個信號處理與變換(去除或減弱不想要的成分，增強所需成分)的過程，這個過程既可以通過硬件來實現(xiàn)，也可以通過軟件來實現(xiàn)?？柭鼮V波屬于一種軟件濾波方法，其基本思想是：以最小均方差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和當前時刻的觀測值來更新對狀態(tài)變量的估計，求出當前時刻的估計值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方差的估計。
舉一個簡單的例子，我們通過溫度計測得房間的溫度，根據(jù)經(jīng)驗，室內(nèi)的溫度是恒定的，即上一分鐘的溫度等于現(xiàn)在的溫度(假設我們用一分鐘來做時間單位)，但是畢竟經(jīng)驗不是很準，上下會有誤差，我們把這個誤差看成高斯白噪聲。另外我們在房間放的溫度計也不是非常準確，測量值會比實際值偏差，我們也把這些偏差看成是高斯白噪聲?，F(xiàn)在我們有了兩個關于該房間的溫度值：根據(jù)經(jīng)驗的預測值和溫度計的值(測量值)，下面根據(jù)這兩個值并結合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算k時刻的是實際溫度值，首先要根據(jù)k-1時刻的溫度值，來預測k時刻的溫度。假設是k-1時刻是23度，同時該值的高斯噪聲的偏差是5度(5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預測的不確定 度是4度，他們平方相加再開方，就是5)。然后k時刻從溫度計讀出的值是25度。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢?相信自己還是相信溫度計呢?究竟相信誰多一點，我們可以用他們的covariance來判斷。因為Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值 是：23+0.78*(25-23)=24.56度?？梢钥闯?，因為溫度計的covariance比較小(比較相信溫度計)，所以估算出的最優(yōu)溫度值偏向溫度計的值。
現(xiàn)在我們已經(jīng)得到k時刻的最優(yōu)溫度值了，下一步就是要進入k+1時刻，進行新的最優(yōu)估算。在進入k+1時刻之前，我們還要算出k時刻那個最優(yōu)值(24.56度)的偏差。算法如下：((1- Kg)*5^2)^0.5=2.35。這里的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差(對應于上面的3)。就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益(KalmanGain)。
卡爾曼濾波有5大黃金公式，如下所述：
1、預測方程 (狀態(tài)預測方程)：X(k|k-1) = AX(k-1|k-1) + BU(k)
X(k|k-1) -------- k-1時刻估計k時刻的狀態(tài)值
A --------- 系統(tǒng)參數(shù)，對于多模型系統(tǒng)，它為矩陣
X(k-1|k-1) --------- k-1時刻的狀態(tài)值
U(k) --- k時刻的狀態(tài)控制量 (若無控制量的話，需要設定為0)
自己理解的方程：X(k) = a * X(k-1) ,在MPU6050中X(k)的值為陀螺儀測量的角速度
2、計算誤差協(xié)方差 ：P(k|k-1) = AP(k-1|k-1)A' + Q
P(k|k-1) --------- k-1時刻估計k時刻的協(xié)方差
P(k-1|k-1) ------- k-1時刻的協(xié)方差
A' -------------- A的轉(zhuǎn)置矩陣
Q ---------- 系統(tǒng)過程的過程噪聲協(xié)方差
自己理解的方程：P(k) = A P(k-1) * A '
3、計算卡爾曼增益: Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
Kg ------------ 卡爾曼增益
R ------------ 傳感器的噪聲平均值
自己理解的方程：Kg = P(k-1)/(P(k-1) + R)
4、修正估計 : X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
Z(k) ----------------- MPU6050測出的加速度計的值
自己理解的方程： X(k) = X(k-1) + Kg * (Z(k) - X(k-1))
X(k)即為陀螺儀測量的角速度
當Kg = 0時，狀態(tài)值即為估計值
當Kg = 1時，狀態(tài)值即為觀測值
5、更新誤差協(xié)方差：P(k|k) = (1 - Kg(k)H) P(k|k-1)
1 --- 矩陣，表示單模型單測量
自己理解的方程：P(k) = (1 - Kg ) * P(k - 1)
代碼如下：
float angle_Y = 0; //初始狀態(tài)值
float angle_X = 0;
float angle_Z = 0;
void Kalman_Y(Vector3i* acc,Vector3i* gyro)
{
float Q_angle = 0.001; // 陀螺儀噪聲的協(xié)方差
float R_angle = 0.5; // 加速度計的協(xié)方差
float dt = 0.0002;
float Q_bias = 0; // Q_bias為陀螺儀漂移
float Q_bians = 0;
float k_0 =0,k_1 = 0; // 卡爾曼增益
float pdot[4] = {0,0,0,0};
float p[2][2] = {{1,0},{0,1}};
//1、卡爾曼第一個公式(狀態(tài)預測):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle_Y +=(gyro->y - Q_bias) * dt;
//2、卡爾曼第二個公式(計算誤差協(xié)方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡爾曼第三個公式(計算卡爾曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系統(tǒng)測量噪聲協(xié)方差
k_0 = p[0][0]/(p[0][0]+R_angle);
k_1 = p[1][0]/(p[0][0]+R_angle);
//4、卡爾曼第四個公式(修正估計):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle_Y += k_0 * ((atan2(-acc->x,acc->z) * RAD2DEG) - angle_Y);
Q_bians += k_1 * ((atan2(-acc->x,acc->z) * RAD2DEG) - angle_Y);
//5、卡爾曼第五個公式(更新誤差協(xié)方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
}
void Kalman_X(Vector3i* acc,Vector3i* gyro)
{
float Q_angle = 0.001; //陀螺儀噪聲的協(xié)方差
float R_angle = 0.5; //加速度計的協(xié)方差
float dt = 0.0002;
float Q_bias = 0; //Q_bias為陀螺儀漂移
float Q_bians = 0;
float k_0 =0,k_1 = 0; //卡爾曼增益
float pdot[4] = {0,0,0,0};
float p[2][2] = {{1,0},{0,1}};
//1、卡爾曼第一個公式(狀態(tài)預測):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle_X +=(gyro->x - Q_bias) * dt;
//2、卡爾曼第二個公式(計算誤差協(xié)方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡爾曼第三個公式(計算卡爾曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系統(tǒng)測量噪聲協(xié)方差
k_0 = p[0][0]/(p[0][0]+R_angle);
k_1 = p[1][0]/(p[0][0]+R_angle);
//4、卡爾曼第四個公式(修正估計):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle_X += k_0 * ((atan2(acc->y,acc->z) * RAD2DEG) - angle_X);
Q_bians += k_1 * ((atan2(acc->y,acc->z) * RAD2DEG) - angle_X);
//5、卡爾曼第五個公式(更新誤差協(xié)方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
}
void Kalman_Z(Vector3i* mag,Vector3i* gyro)
{
float Q_angle = 0.001; //陀螺儀噪聲的協(xié)方差
float R_angle = 0.5; //加速度計的協(xié)方差
float dt = 0.0002;
float Q_bias = 0; //Q_bias為陀螺儀漂移
float Q_bians = 0;
float k_0 =0,k_1 = 0; //卡爾曼增益
float pdot[4] = {0,0,0,0};
float p[2][2] = {{1,0},{0,1}};
//1、卡爾曼第一個公式(狀態(tài)預測):X(k|k-1)=AX(k-1|k-1)+BU(k)
angle_Z +=(gyro->z - Q_bias) * dt;
//2、卡爾曼第二個公式(計算誤差協(xié)方差)：AP(k-1|k-1)A' + Q
pdot[0] = Q_angle - p[0][1] + p[1][0];
pdot[1] = -p[1][1];
pdot[2] = -p[1][1];
pdot[3] = Q_bias;
p[0][0] += pdot[0] * dt;
p[0][1] += pdot[1] * dt;
p[1][0] += pdot[2] * dt;
p[1][1] += pdot[3] * dt;
//3、卡爾曼第三個公式(計算卡爾曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
//R --->系統(tǒng)測量噪聲協(xié)方差
k_0 = p[0][0]/(p[0][0]+R_angle);
k_1 = p[1][0]/(p[0][0]+R_angle);
//4、卡爾曼第四個公式(修正估計):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
angle_Z += k_0 * ((atan2(mag->x,mag->y) * RAD2DEG) - angle_Z);
Q_bians += k_1 * ((atan2(mag->x,mag->y) * RAD2DEG) - angle_Z);
//5、卡爾曼第五個公式(更新誤差協(xié)方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
p[0][0] = (1 - k_0) * p[0][0];
p[0][1] = (1 - k_0) * p[0][1];
p[1][0] = (1 - k_0) * p[1][0];
p[1][1] = (1 - k_0) * p[1][1];
}
運算宏定義如下：
#define MM2M 0.001f
#define DEG2RAD 0.017453292519943295769236907684886f // 角度轉(zhuǎn)弧度
#define RAD2DEG 57.295779513082320876798154814105f // 弧度轉(zhuǎn)角度
#define sq(a) ((a) * (a)) //平方、
#define sqrtf4(a,b,c,d) __sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
#define sqrtf3(a,b,c) __sqrtf(sq(a) + sq(b) + sq(c))
#define sqrtf2(a,b) __sqrtf(sq(a) + sq(b))
下載文檔
