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