當前位置:首頁 > 技術學院 > 技術前線
[導讀]無人機底層開發(fā)-MPU6050卡爾曼濾波的簡單理解

卡爾曼濾波器由一系列遞歸數(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))

本站聲明: 本文章由作者或相關機構授權發(fā)布,目的在于傳遞更多信息,并不代表本站贊同其觀點,本站亦不保證或承諾內(nèi)容真實性等。需要轉(zhuǎn)載請聯(lián)系該專欄作者,如若文章內(nèi)容侵犯您的權益,請及時聯(lián)系本站刪除。
換一批
延伸閱讀

9月2日消息,不造車的華為或?qū)⒋呱龈蟮莫毥谦F公司,隨著阿維塔和賽力斯的入局,華為引望愈發(fā)顯得引人矚目。

關鍵字: 阿維塔 塞力斯 華為

加利福尼亞州圣克拉拉縣2024年8月30日 /美通社/ -- 數(shù)字化轉(zhuǎn)型技術解決方案公司Trianz今天宣布,該公司與Amazon Web Services (AWS)簽訂了...

關鍵字: AWS AN BSP 數(shù)字化

倫敦2024年8月29日 /美通社/ -- 英國汽車技術公司SODA.Auto推出其旗艦產(chǎn)品SODA V,這是全球首款涵蓋汽車工程師從創(chuàng)意到認證的所有需求的工具,可用于創(chuàng)建軟件定義汽車。 SODA V工具的開發(fā)耗時1.5...

關鍵字: 汽車 人工智能 智能驅(qū)動 BSP

北京2024年8月28日 /美通社/ -- 越來越多用戶希望企業(yè)業(yè)務能7×24不間斷運行,同時企業(yè)卻面臨越來越多業(yè)務中斷的風險,如企業(yè)系統(tǒng)復雜性的增加,頻繁的功能更新和發(fā)布等。如何確保業(yè)務連續(xù)性,提升韌性,成...

關鍵字: 亞馬遜 解密 控制平面 BSP

8月30日消息,據(jù)媒體報道,騰訊和網(wǎng)易近期正在縮減他們對日本游戲市場的投資。

關鍵字: 騰訊 編碼器 CPU

8月28日消息,今天上午,2024中國國際大數(shù)據(jù)產(chǎn)業(yè)博覽會開幕式在貴陽舉行,華為董事、質(zhì)量流程IT總裁陶景文發(fā)表了演講。

關鍵字: 華為 12nm EDA 半導體

8月28日消息,在2024中國國際大數(shù)據(jù)產(chǎn)業(yè)博覽會上,華為常務董事、華為云CEO張平安發(fā)表演講稱,數(shù)字世界的話語權最終是由生態(tài)的繁榮決定的。

關鍵字: 華為 12nm 手機 衛(wèi)星通信

要點: 有效應對環(huán)境變化,經(jīng)營業(yè)績穩(wěn)中有升 落實提質(zhì)增效舉措,毛利潤率延續(xù)升勢 戰(zhàn)略布局成效顯著,戰(zhàn)新業(yè)務引領增長 以科技創(chuàng)新為引領,提升企業(yè)核心競爭力 堅持高質(zhì)量發(fā)展策略,塑強核心競爭優(yōu)勢...

關鍵字: 通信 BSP 電信運營商 數(shù)字經(jīng)濟

北京2024年8月27日 /美通社/ -- 8月21日,由中央廣播電視總臺與中國電影電視技術學會聯(lián)合牽頭組建的NVI技術創(chuàng)新聯(lián)盟在BIRTV2024超高清全產(chǎn)業(yè)鏈發(fā)展研討會上宣布正式成立。 活動現(xiàn)場 NVI技術創(chuàng)新聯(lián)...

關鍵字: VI 傳輸協(xié)議 音頻 BSP

北京2024年8月27日 /美通社/ -- 在8月23日舉辦的2024年長三角生態(tài)綠色一體化發(fā)展示范區(qū)聯(lián)合招商會上,軟通動力信息技術(集團)股份有限公司(以下簡稱"軟通動力")與長三角投資(上海)有限...

關鍵字: BSP 信息技術
關閉
關閉