|
|
void Angle_Calcu(void)
{
reg_MMA(1,ZOUT8,0,&MMAData); //读出MMA7445 z轴加速度值
Kalman_Filter(anglehandle_MMA(MMAData),Get_Gyro()); //
}
float Get_Gyro()
{
/*************************
读取 陀螺仪 Z轴
有符号整形
**************************/
uint gyro_L,gyro_H;
int gyro_temp;
gyro_H = Single_ReadL3G4200D(OUT_Z_H);
gyro_L = Single_ReadL3G4200D(OUT_Z_L);
gyro_temp = (gyro_H<<8 | gyro_L);
return ((gyro_temp + 50)*0.000152); //去除偏移量 计算角速度 8.75mdps/lsb*3.14/180
}
//***倾角传感器是MMA7445以下是将其数据转换为角度***
static unsigned char angle_flag=0; //标志角度的正负
float anglehandle_MMA(unsigned char angle8)
{
float angleData;
angle8=angle8-28; //28为垂直时Z轴的读数
if(angle8<=63)
{
angle_flag=0; //正角度
angleData=asin(angle8*0.015873); // 0.015873=（1/63)
}
else
{
angle_flag=1; //负角度
angleData=asin((256-angle8)*0.015873);
}
return angleData;
}
//定时器中断10MS 读取速度
我试了卡尔曼之前的角度和角速度数据，比较靠谱，但滤波后的角度值，只显示00，偶尔FF
卡尔曼滤波程序是抄论坛各位大神的，是不是MSP430f149运行滤波程序出了差错？
//******卡尔曼参数************
static const float Q_angle=0.001;
static const float Q_gyro=0.003;
static const float R_angle=0.5;
static const float dt=0.01; //dt为kalman滤波器采样时间;
static const char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
//*********************************************************
// 卡尔曼滤波
// 输出：Angle Gyro_zf
//*********************************************************
//Kalman滤波
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_zf = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
本人第一次做，希望得到各位大神指导，谢谢了 |
|
发表于 2013-3-26 12:00:00
楼主
