基于DSP的室內慣性導航系統設計

　　HMC5883L三軸磁感應傳感器的作用相當于羅盤，在水平情況下，無需借助其他傳感器便可以計算出航向。其初始化如下：

　　unsigned char Init_HMC5883(void)

　　{

　　unsigned char Return1;

　　unsigned char Data;

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

　　Data = 0x00;

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

　　if(Return1)

　　return 1;

　　else

　　return 0;

　　}

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

　　3.2卡爾曼濾波算法應用

　　于是裝置在室內區域進行勘測搜索，小車的運行特點與一般的飛機、船、車不同，它的運動變化快，軌跡不定，而且要適用于不同的環境下工作，因此常用的卡爾曼濾波算法需要進一步改進才能應用。卡爾曼過濾是用前一個估計值和最近一個觀察數據，來估計信號的當前值，它是用狀態方程和遞推的方法進行估計的，它的解是以估計值形式給出的。其運用在加速度器和陀螺儀上的卡爾曼濾波程序如下：

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

　　//float incAngle：加速度器測得的角度值

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

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

　　#define Q_angle 0.0001//過程噪聲的協方差

　　#define Q_gyro 0.0003 //過程噪聲的協方差過程噪聲協方差為一個一行兩列矩陣

　　float kalmanUpdate(const float gyro_m，const float incAngle

　　{

　　float K0;//含有卡爾曼增益的另外一個函數，用于計算最優估計值

　　float K1;//含有卡爾曼增益的函數，用于計算最優估計值的偏差

　　float Y0;

　　float Y1;

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

　　float Pdot[4];//過程協方差矩陣的微分矩陣

　　float angle_err;//角度偏量

　　float E;//計算的過程量

　　static float angle = 0; //下時刻最優估計值角度

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

　　static float n[2][2] = {{ 1， 0 }， { 0， 1 }};//過程協方差矩陣

　　Rate = gyro_m - q_bias;

　　//計算過程協方差矩陣的微分矩陣

　　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; //計算協方差矩陣

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

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

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

　　angle_err = incAngle - angle; //計算角度偏差

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

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

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

　　Y0 = n[0][0];

　　Y1 = n[0][1];

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

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

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

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

　　angle += K0 * angle_err; //給出最優估計值

　　q_bias += K1 * angle_err;//跟新最優估計值偏差

　　return angle;

　　｝

　　通過濾波時數據平滑將加速度輸出電壓附近產生的波動噪聲濾掉。