基于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;

　　｝

　　通过滤波时数据平滑将加速度输出电压附近产生的波动噪声滤掉。