基于DSP的室内惯性导航系统设计

的旋转角速度，同时也可以通过积分把角速度转换为小车的倾角。程序中，ITG3200的初始化如下：

　　unsigned char Init_ITG3200（void）

　　{

　　unsigned char Return1，Return2，Return3，Return4;

　　unsigned char Data;

　　Data = 0x00;

　　Return1 = IIC_WriteData（0xD0， 0x3E， &Data， 1）;

　　Data = 0x07;

　　Return2 = IIC_WriteData（0xD0， 0x15， &Data， 1）;

　　Data = 0x1E;

　　Return3 = IIC_WriteData（0xD0， 0x16， &Data， 1）;

　　Data = 0x00;

　　Return4 = IIC_WriteData（0xD0， 0x17， &Data， 1）;

　　if（Return1 && Return2 && Return3 && Return4）

　　return 1;

　　else

　　return 0;

　　}

　　其具体功能实现可以在主程序中通过SCI读取其值。所读取的值为角速度，不会受到小车运动的影响，因此该信号噪声很小，同时可以由它积分得到小车倾斜角度，可以平滑信号使其更加稳定。

　　由于装置是要在不同的室内区域进行勘测搜索，再加上未知的环境，所以角速度信号可能存在一定的偏差，会导致积分后的角度出现大的误差，无法得到实际的数值。为了消除这个由于偏差而产生的累积误差，装置上加上ADXL345三轴加速度传感器对于获得的角度信息进行校正。ADXL345初始化如下：

　　unsigned char Init_ADXL345（void）

　　{

　　unsigned char Return1，Return2，Return3，Return4;

　　unsigned char Data;

　　Data = 0x0b;

　　byReturn1 = IIC_WriteData（0xA6， 0x31， &Data， 1）;

　　Data = 0x08;

　　Return2 = IIC_WriteData（0xA6， 0x2c， &Data， 1）;

　　Data = 0x08;

　　Return3 = IIC_WriteData（0xA6， 0x2d， &Data， 1）;

　　Data = 0x80;

　　Return4 = IIC_WriteData（0xA6， 0x2e， &Data， 1）;

　　Data = 0x00;

　　Return4 = IIC_WriteData（0xA6， 0x1e， &Data， 1）;

　　Data = 0x00;

　　Return4 = IIC_WriteData（0xA6， 0x1f， &Data， 1）;

　　Data = 0x05;

　　Return4 = IIC_WriteData（0xA6， 0x20， &Data， 1）;

　　if（Return1&&Return2&&Return3&&Return4）

　　return 1;

　　else

　　return 0;

　　}

　　通过ADXL345所得到的角度，和陀螺仪积分后的角度进行对比，然后使用它们的偏差改变陀螺仪的输出，从而积分后的角度慢慢校正到实际的角度，如图5所示。

　　

　　图5 通过加速度传感器校正角度

　　HMC5883L三轴磁感应传感器的作用相当于罗盘，在水平情况下，无需借助其他传感器便可以计算出航向。其初始化如下：

　　unsigned char Init_HMC5883（void）

　　{

　　unsigned char Return1;

　　unsigned char Data;

　　// Bit4 Bit3 等于11时，选择2000度/秒的量程

　　Data = 0x00;

　　Return1 = IIC_WriteData（0x3C， 0x02， &Data， 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 }， {