#include?“imu.h“

#define?Kp?2.0f????????????????	//?proportional?gain?governs?rate?of?convergence?to?accelerometer/magnetometer
#define?Ki?0.00f????????????????	//?0.001??integral?gain?governs?rate?of?convergence?of?gyroscope?biases
#define?T?0.02

#define?IMU_INTEGRAL_LIM??（?2.0f?*DEG_TO_RAD?）
//用于校準的數(shù)據(jù)誤差
float?ax_cali=0ay_cali=0az_cali=0;
short?gx_cali=0gy_cali=0gz_cali=0;
//測出加速度計誤差并保存待用，50次取平均，校準過程中須保持水平
//返回值，0:校準未完成，1：校準完成
//自創(chuàng)的向量校正方法
u8?Acc_Calibrate（short?axshort?ayshort?azu8?enable）
{
	static?u8?i=0;
	float?norm;
	static?s32?axcount=0;
	static?s32?aycount=0;
	static?s32?azcount=0;
	float?array_xarray_yarray_z;
	if（enable!=1）
	{
		i=0;
		axcount=0;aycount=0;azcount=0;
		return?0;
	}
	else?if（i<50）
	{
		axcount+=ax;aycount+=ay;azcount+=az;
		i++;
		return?0;
	}
	else
	{
		array_x=axcount/50.0;array_y=aycount/50.0;array_z=azcount/50.0;//實際向量
		norm=Q_rsqrt（array_x*array_x+array_y*array_y+array_z*array_z）;//均方倒數(shù)歸一化
		array_x*=norm;array_y*=norm;array_z*=norm;//實際單位向量
		ax_cali-=array_x;//以下為誤差校正向量，即實際單位向量和單位重力向量之間的向量差
		ay_cali-=array_y;
		az_cali+=1-array_z;
		i=0;
		axcount=0;aycount=0;azcount=0;
		return?1;
	}
}
//測出陀螺儀誤差并保存待用，50次取平均，校準過程中須保持靜止
//返回值?0:校準未完成，1：校準完成
u8?Gyro_Calibrate（short?gxshort?gyshort?gzu8?enable）
{
	static?u8?i=0;
	static?s32?gxcount=0;
	static?s32?gycount=0;
	static?s32?gzcount=0;
	if（enable!=1）
	{
		i=0;
		gxcount=0;gycount=0;gzcount=0;
		return?0;
	}
	else?if（i<50）
	{
		gxcount+=gx;gycount+=gy;gzcount+=gz;
		i++;
		return?0;
	}
	else
	{
		gxcount/=50;gycount/=50;gzcount/=50;
		gx_cali+=gxcount;
		gy_cali+=gycount;
		gz_cali+=gzcount;
		i=0;
		gxcount=0;gycount=0;gzcount=0;
		return?1;
	}
}
//用保存的誤差參數(shù)校正加速度計原始數(shù)據(jù)
//陀螺儀的誤差參數(shù)另外作為卡爾曼濾波的狀態(tài)變量在濾波中校正而不在此函數(shù)中
void?IMU_Calibrate（short?*axshort?*ayshort?*az）
{
	short?accx=*ax;
	short?accy=*ay;
	short?accz=*az;
	float?norm;
	//用校準參數(shù)校正加速度計原始數(shù)據(jù)
	norm=my_sqrt（accx*accx+accy*accy+accz*accz）;
	accx+=ax_cali*norm;
	accy+=ay_cali*norm;
	accz+=az_cali*norm;
	*ax?=?accx;
	*ay?=?accy;
	*az?=?accz;
}
float?sigma_a=0.01sigma_g=0.01;//陀螺儀驅(qū)動噪聲方差和加速度計觀測噪聲方差
//卡爾曼增益
float?K_roll[2];
float?K_pitch[2];
//最小均方誤差矩陣M[n|n]或M[n-1|n-1]
float?mmse_roll[2][2]?=?{?{?1?0?}{?0?1?}?};
float?mmse_pitch[2][2]?=?{?{?1?0?}{?0?1?}?};
//最小預測均方誤差矩陣M[n|n-1]
float?mPmse_roll[2][2];
float?mPmse_pitch[2][2];
//六軸融合卡爾曼濾波算法
void?IMUupdate（short?*gxshort?*gyshort?*gzshort?axshort?ayshort?azfloat?*rollfloat?*pitchfloat?*yaw）
{
	float?temp;//為減少計算量而暫時保存數(shù)據(jù)，無實際意義
	float?roll_temppitch_temp;//狀態(tài)變量預測值s[n|n-1]
	//預測
	*gx-=gx_cali;
	*gy-=gz_cali;
	*gz-=gz_cali;
	*yaw+=GYRO_TO_DEG（*gz）*T;
	roll_temp=*roll+GYRO_TO_DEG（*gx）*T;
	pitch_temp=*pitch+GYRO_TO_DEG（*gy）*T;
	//最小預測MSE
	mPmse_roll[0][0]=mmse_roll[0][0]+（mmse_roll[1][1]+sigma_g）*T*T-（mmse_roll[0][1]+mmse_roll[1][0]）*T;
	mPmse_roll[0][1]=mmse_roll[0][1]-mmse_roll[1][1]*T;
	mPmse_roll[1][0]=mmse_roll[1][0]-mmse_roll[1][1]*T;
	mP

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件????????505??2018-09-09?11:37??imu.h

?????文件???????4619??2018-09-18?16:12??imu.c

-----------?---------??----------?-----??----

?????????????????5124????????????????????2