卡爾曼濾波器 MPU6050

基于MPU6050及卡爾曼濾波的角度測量

MPU6050的基本應(yīng)用

卡爾曼濾波工作原理及應(yīng)用MichaelBeechanMPU6050▲MPU-6050：是6軸運(yùn)動處理傳感器，集成了3軸陀螺儀傳感器和3軸加速度傳感器；▲MPU6050共有117個寄存器，設(shè)計中主要運(yùn)用到的寄存器有：1、分頻寄存器0x192、配置寄存器0x1A3、陀螺儀配置寄存器0x1B4、加速度計配置寄存器0x1C5、加速度計數(shù)據(jù)寄存器0x3B—0x406、陀螺儀數(shù)據(jù)寄存器0x43—0x487、電源管理寄存器10x6B8、我是誰寄存器0x75用于標(biāo)識設(shè)備的身份▲MPU6050的函數(shù)初始化：解除休眠（0x00）——分頻設(shè)置（0x07）——寄存器配置（0x06）——陀螺儀配置（0x18）——加速度計配置（0x01）▲數(shù)據(jù)處理：………I2C總線通信▲應(yīng)用：四旋翼飛行器、平衡小車、行人導(dǎo)航儀、手持型游戲產(chǎn)品、3D遙控器。

MichaelBeechan內(nèi)部工作MichaelBeechan卡爾曼濾波器◆卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值——是一種收斂性◆應(yīng)用：機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤、計算機(jī)圖像處理（頭臉識別、圖像分割）MichaelBeechanKalman概念：卡爾曼濾波器是最優(yōu)化自回歸數(shù)據(jù)處理算法。一、符號說明：K：系統(tǒng)的現(xiàn)在狀態(tài)；Kg：卡爾曼增益二、核心內(nèi)容：5條公式首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用一個線性隨機(jī)微分方程來描述：

X(k)=AX(k-1)+BU(k)+W(k)

再加上系統(tǒng)的測量值：

Z(k)=HX(k)+V(k)

W(k)和V(k)分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的covariance（協(xié)方差）分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。、MichaelBeechan基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)：

X(k|k-1)=AX(k-1|k-1)+BU(k)………..(1)

式(1)中，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。

我們用P表示covariance：

P(k|k-1)=AP(k-1|k-1)A’+Q………(2)

現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(3)

其中Kg為卡爾曼增益(KalmanGain)：

Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(4)

卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance：

P(k|k)=（I-Kg(k)H）P(k|k-1)………(5)

其中I為1的矩陣，對于單模型單測量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時，P(k