基于DSP的室內(nèi)慣性導(dǎo)航系統(tǒng)設(shè)計(jì)
HMC5883L三軸磁感應(yīng)傳感器的作用相當(dāng)于羅盤(pán),在水平情況下,無(wú)需借助其他傳感器便可以計(jì)算出航向。其初始化如下:
本文引用地址:http://m.butianyuan.cn/article/274609.htmunsigned 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è)搜索,小車的運(yùn)行特點(diǎn)與一般的飛機(jī)、船、車不同,它的運(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)噪聲濾掉。
評(píng)論