STM32F103C8T6+MPU6050+kalman滤波

hayden · 发表于 2016-12-12 17:22:40

为了其他人不在走弯路，第一次发帖分享一下自己的经验，不喜勿喷，本人菜鸟一枚！
原子哥的iic,一开始who_am_i读数和其他数据都是0，这个可以确定iic不通，搞了两天才发现是自己马虎直接复制别人错误代码造成的

模拟iic用的是sda-PB6,scl-PB7，原子哥的本来是PB11 PB12,我改了之后用了网友的这段代码：
#define SDA_IN()  {GPIOB->CRH&=0X0FFFFFFF;GPIOB->CRL|=(u32)8<<28;}
#define SDA_OUT() {GPIOB->CRH&=0X0FFFFFFF;GPIOB->CRL|=(u32)3<<28;}

更改后：
#define SDA_IN()  {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=(u32)8<<28;}
#define SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=(u32)3<<28;}

终于见到久违的who_am_i:0x68
然后遇到的第二个问题是kalman滤波前的角度计算，网上的程序很多都很乱，小白看不懂
东平西凑后确定了这么一段求pitch和roll的代码：
[mw_shl_code=c,true]//角度计算
void Angle_Calcu(void)
{
//不自测，+-2000°/s
//范围为2g时，换算关系：16384 LSB/g
//deg = rad*180/3.14

Accel_x = GetData(ACCEL_XOUT_H); //x轴加速度值暂存
Accel_y = GetData(ACCEL_YOUT_H); //y轴加速度值暂存
Accel_z = GetData(ACCEL_ZOUT_H); //z轴加速度值暂存
Gyro_x  = GetData(GYRO_XOUT_H);  //x轴陀螺仪值暂存
Gyro_y  = GetData(GYRO_YOUT_H);  //y轴陀螺仪值暂存
Gyro_z  = GetData(GYRO_ZOUT_H);  //z轴陀螺仪值暂存
Temperature = GetData(TEMP_OUT_H); //x轴加速度值暂存
Temperature =(((double) Temperature )/340+36.53);  // 读取计算出温度

// printf(" AX=%f AY=%f AZ=%f  \n",Ax,Ay,Az);

Ax = (Accel_x-300)/16384.0;
Ay = (Accel_y-500)/16384.0;
Az = (Accel_z-366)/16384.0;

Gx=(Gyro_x+55)/ 16.4;
Gy=(Gyro_y-24)/ 16.4;
Gz=(Gyro_z-6)/ 16.4;

// printf(" AX=%f AY=%f AZ=%f  \n",Ax,Ay,Az);
// printf(" GX=%f GY=%f GZ=%f  \n",Gx,Gy,Gz);
// printf("temp-%f",Temperature);

//用加速度计算三个轴和水平面坐标系之间的夹角
//加速度计读出来的原始数据为 Ax Ay Az，假设绕Y为俯仰pitch 绕X为横滚roll。
  //俯仰角= —atan2(AX, AZ )  横滚角=atan2(AY, AZ)
Angle_x_temp = (atan2(Ax,Az)) * 180 / Pi;
Angle_y_temp = (atan2(Ay,Az)) * 180 / Pi;

Pitch=Kalman_Filter(Angle_x_temp, Gy);
Roll=Kalman_Filter(Angle_y_temp, Gx);

printf(" P=%f R=%f  \n",Pitch,Roll);
// printf(" AX=%f AY=%f AZ=%f  \n",Ax,Ay,Az);
// printf(" GX=%f GY=%f GZ=%f  \n",Gx,Gy,Gz);

} [/mw_shl_code]
前两个问题解决了，又冒出来一个，串口打印的pitch和roll居然是一个数值，变化相同，但数值没问题

未滤波的角度和角速度都正常
后来确定了是kalman的问题，仔细想想，终于恍然大悟，kalman是网上复制的他的参数是全局变量，被重复使用了，串口打印的结果自然也就是来自同一个变量
把全局变量都放进函数内，变成局部变量，生命周期变短，用一次销毁一次，完美解决
终于见到了久违的pitch和roll
下面是卡拉曼滤波：
[mw_shl_code=c,true]float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{

//卡尔曼滤波参数与函数
float dt=0.001;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;//角度和角速度
float P[2][2] = {{ 1, 0 },
   { 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

      angle+=(gyro_m-q_bias) * dt;
      angle_err = angle_m - angle;
      Pdot[0]=Q_angle - P[0][1] - P[1][0];
      Pdot[1]=- P[1][1];
      Pdot[2]=- P[1][1];
      Pdot[3]=Q_gyro;
      P[0][0] += Pdot[0] * dt;
      P[0][1] += Pdot[1] * dt;
      P[1][0] += Pdot[2] * dt;
      P[1][1] += Pdot[3] * dt;
      PCt_0 = C_0 * P[0][0];
      PCt_1 = C_0 * P[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 * P[0][1];
      P[0][0] -= K_0 * t_0;
      P[0][1] -= K_0 * t_1;
      P[1][0] -= K_1 * t_0;
      P[1][1] -= K_1 * t_1;
      angle += K_0 * angle_err; //最优角度
      q_bias += K_1 * angle_err;
      angle_dot = gyro_m-q_bias;//最优角速度

      return angle;
}
[/mw_shl_code]

现在准备把姿态角显示到4.06匿名上位机看一下kalman滤波效果和3d姿态
但是看了上位机上下载下来的下位机通信协议代码有点头疼，有谁写了简单的给共享一下，先在这里谢谢了！