小车陀螺仪输出波形图卡曼滤波

Arduino 智能平衡 小车 模块篇
小车主视图：
小车主控制板： Arduino uno r3
小车陀螺仪： MPU6050
可以输出当前模块X Y Z 轴的角度 角速度 更集 成温度传感器 能读取温 度
小车陀螺仪：MPU6050 上位机读取数据程序 使用通讯方式为IIC
#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #include "Wire.h“
小车陀螺仪：输出波形图
卡曼滤波算法：
（3） void Kalman_Filter(double angle_m,double gyro_m) { 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;
// blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); }
小车陀螺仪： MPU6050 通 过IDE自带串 口工具
小车陀螺仪：输出波形图
当得到六个原始量（ax ay az gx gy gz）以后，我们可以通过三种方式得到当前角度 并通过上位机得到输出波形
方式一：低阶滤波（一阶滤波，二阶 滤波）
方式二：高阶滤波（卡曼滤波）
方式三：DMP直接得到角度（启动 MPU6050内部运算，读取DMP存储 器中数据）
小车陀螺仪：输出波形图
一阶滤波/二阶滤波：
void getangle()
{ accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数 据 angleAx=atan2(ax,az)*180/PI;//计算与 x轴夹角 gyroGy=-gy/131.00;//计算角速度 （1）Yijielvbo(angleAx,gyroGy);//一 阶滤波 （2）Erjielvbo(angleAx,gyroGy);//二 阶滤波 （3）
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;//最优角速 度
Kalman_Filter(angleAx,gyroGy); // 卡尔曼滤波
}
（1）void Yijielvbo(float angle_m, float gyro_m) { angle1 =Leabharlann BaiduK1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt); } //一阶滤波
停止信号：处理器让SCL时钟保持高电平，然后让SDA数据信号由低变高就表 示一个停止信号。同时IIC总线上的设备检测到这个停止信号它就知道处理器已经 结束了数据传输，我们就可以各忙各个的了，如休眠等。
小车陀螺仪：MPU6050 上位机读取数据程序 使用通讯方式为IIC
如果要读取MPU-60X0 寄存器的值，首先由主设备产生开始信号（S），然后发 送从设备地址位和一个写数据位，然后发送寄存器地址，才能开始读寄存器。紧接 着，收到应答信号后，主设备再发一个开始信号，然后发送从设备地址位和一个读 数据位。然后，作为从设备的MPU-60X0 产生应答信号并开始发送寄存器数据。 通信以主设备产生的拒绝应答信号（NACK）和结束标志（P）结束。拒绝应答信 号（NACK）产生定义为SDA 数据在第9 个时钟周期一直为高。
小车陀螺仪：MPU6050 上位机读取数据程序
Arduion IIC接口： Sda：A4 Sck：A5
void loop() { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gyro_y=-(gy-10.58)*rgyro;
Serial.print("a/g:\t"); Serial.print(ax); Serial.print("\t"); Serial.print(ay); Serial.print("\t"); Serial.print(az); Serial.print("\t"); Serial.print(gx); Serial.print("\t"); Serial.print(gy); Serial.print("\t"); Serial.println(gz);Serial.print("\t"); Serial.print(gyro_y);Serial.print("\t");
（2）void Erjielvbo(float angle_m,float gyro_m) { x1=(angle_m-angle2)*(1-K2)*(1-K2); y1=y1+x1*dt; x2=y1+2*(1-K2)*(angle_mangle2)+gyro_m; angle2=angle2+ x2*dt; }//二阶滤波
Wire.begin(); I2C 总线序列
mpu.initialize(); MPU6050
//加入 //初始化
小车陀螺仪：MPU6050 上位机读取数据程序 使用通讯方式为IIC
开始（S）和结束（P）标志 开 始信号：处理器让SCL时钟保持高电平，然后让SDA数据信号由高变低就
表示一个开始信号。同时IIC总线上的设备检测到这个开始信号它就知道处理器要 发送数据了。