HMC5883L三軸磁感應(yīng)傳感器的作用相當(dāng)于羅盤(pán)，在水平情況下，無(wú)需借助其他傳感器便可以計(jì)算出航向。其初始化如下：

　　unsigned char Init_HMC5883(void)

　　{

　　unsigned char Return1;

　　unsigned char Data;

　　// Bit4 Bit3等于11時(shí)，選擇2000度/秒的量程

　　Data = 0x00;

　　Return1 = IIC_WriteData(0x3C， 0x02， 1);

　　if(Return1)

　　return 1;

　　else

　　return 0;

　　}

　　由于裝置是要在不同環(huán)境下進(jìn)行工作的，所以其并不能保持時(shí)刻水平，就需要加速度傳感器來(lái)糾正由于傾斜引起的誤差。

　　3.2卡爾曼濾波算法應(yīng)用

　　于是裝置在室內(nèi)區(qū)域進(jìn)行勘測(cè)搜索，小車(chē)的運(yùn)行特點(diǎn)與一般的飛機(jī)、船、車(chē)不同，它的運(yùn)動(dòng)變化快，軌跡不定，而且要適用于不同的環(huán)境下工作，因此常用的卡爾曼濾波算法需要進(jìn)一步改進(jìn)才能應(yīng)用?？柭^(guò)濾是用前一個(gè)估計(jì)值和最近一個(gè)觀察數(shù)據(jù)，來(lái)估計(jì)信號(hào)的當(dāng)前值，它是用狀態(tài)方程和遞推的方法進(jìn)行估計(jì)的，它的解是以估計(jì)值形式給出的。其運(yùn)用在加速度器和陀螺儀上的卡爾曼濾波程序如下：

　　// float gyro_m：陀螺儀測(cè)得的量(角速度)

　　//float incAngle：加速度器測(cè)得的角度值

　　#define dt 0.0015//卡爾曼濾波采樣頻率

　　#define R_angle 0.71 //測(cè)量噪聲的協(xié)方差(即是測(cè)量偏差)

　　#define Q_angle 0.0001//過(guò)程噪聲的協(xié)方差

　　#define Q_gyro 0.0003 //過(guò)程噪聲的協(xié)方差過(guò)程噪聲協(xié)方差為一個(gè)一行兩列矩陣

　　float kalmanUpdate(const float gyro_m，const float incAngle

　　{

　　float K0;//含有卡爾曼增益的另外一個(gè)函數(shù)，用于計(jì)算最優(yōu)估計(jì)值

　　float K1;//含有卡爾曼增益的函數(shù)，用于計(jì)算最優(yōu)估計(jì)值的偏差

　　float Y0;

　　float Y1;

　　float Rate;//去除偏差后的角速度

　　float Pdot[4];//過(guò)程協(xié)方差矩陣的微分矩陣

　　float angle_err;//角度偏量

　　float E;//計(jì)算的過(guò)程量

　　static float angle = 0; //下時(shí)刻最優(yōu)估計(jì)值角度

　　static float q_bias = 0; //陀螺儀的偏差

　　static float n[2][2] = {{ 1， 0 }， { 0， 1 }};//過(guò)程協(xié)方差矩陣

　　Rate = gyro_m - q_bias;

　　//計(jì)算過(guò)程協(xié)方差矩陣的微分矩陣

　　Pdot[0] = Q_angle - P[0][1] - P[1][0];

　　Pdot[1] = - n[1][1];

　　Pdot[2] = - n[1][1];

　　Pdot[3] = Q_gyro;

　　angle += Rate * dt; //角速度積分得出角度

　　n[0][0] += Pdot[0] * dt; //計(jì)算協(xié)方差矩陣

　　n[0][1] += Pdot[1] * dt;

　　n[1][0] += Pdot[2] * dt;

　　n[1][1] += Pdot[3] * dt;

　　angle_err = incAngle - angle; //計(jì)算角度偏差

　　E = R_angle + P[0][0];

　　K0 = n[0][0] / E; //計(jì)算卡爾曼增益

　　K1 = n[1][0] / E;

　　Y0 = n[0][0];

　　Y1 = n[0][1];

　　n[0][0] -= K0 * Y0; //跟新協(xié)方差矩陣

　　n[0][1] -= K0 * Y1;

　　n[1][0] -= K1 * Y0;

　　n[1][1] -= K1 * Y1;

　　angle += K0 * angle_err; //給出最優(yōu)估計(jì)值

　　q_bias += K1 * angle_err;//跟新最優(yōu)估計(jì)值偏差

　　return angle;

　?。?/p>

　　通過(guò)濾波時(shí)數(shù)據(jù)平滑將加速度輸出電壓附近產(chǎn)生的波動(dòng)噪聲濾掉。