转载：我的自平衡小车D3——滤波算法
https://www.geek-/thread-681-1-1.html
(出处: 极客工坊)
#include <Wire.h>#define Acc 0x1D#define Gyr 0x69#define Mag 0x1E#define Gry_offset -13 // 陀螺仪偏移量#define Gyr_Gain 0.07// 满量程2000dps时灵敏度(dps/digital)#define pi 3.14159float Com_angle;float y1, Com2_angle;float Klm_angle;#define Q_angle 0.01// 角度数据置信度#define Q_omega 0.0003 // 角速度数据置信度#define R_angle 0.01// 方差噪声float bias = 0;float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;float angleG;long timer = 0; // 采样时间void setup() {sensor_init();// 配置传感器Serial.begin(19200); // 开启串口以便监视数据delay(1000);}void loop() {long o_timer = timer;// 上一次采样时间(ms)float Y_Accelerometer = gDat(Acc, 1);// 获取向前的加速度float Z_Accelerometer = gDat(Acc, 2);// 获取向下的加速度float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;// 根据加速度分量得到的角度(degree)timer = millis();// 当前时间(ms)float omega = Gyr_Gain * (gDat(Gyr, 0) + Gry_offset);float dt = (timer - o_timer) / 1000.0; // 微分时间(s)angleG = angleG + omega * dt; // 对角速度积分得到的角度(degree)// 一阶互补算法float K;K = 0.075; // 对加速度计取值的权重float A = K / (K + dt);Com_angle = A * (Com_angle + omega * dt) + (1 - A) * angleA;// 二阶互补算法K = 0.5;float x1 = (angleA - Com2_angle) * K * K;y1 = y1 + x1 * dt;float x2 = y1 + 2 * K * (angleA - Com2_angle) + omega;Com2_angle = Com2_angle + x2 * dt;// 卡尔曼滤波Klm_angle += (omega - bias) * dt; // 先验估计P_00 += -(P_10 + P_01) * dt + Q_angle * dt;P_01 += -P_11 * dt;P_10 += -P_11 * dt;P_11 += +Q_omega * dt; // 先验估计误差协方差float K_0 = P_00 / (P_00 + R_angle);float K_1 = P_10 / (P_00 + R_angle);bias += K_1 * (angleA - Klm_angle);Klm_angle += K_0 * (angleA - Klm_angle);// 后验估计P_00 -= K_0 * P_00;P_01 -= K_0 * P_01;P_10 -= K_1 * P_00;P_11 -= K_1 * P_01;// 后验估计误差协方差Serial.print(timer);Serial.print(",");Serial.print(angleA, 6);Serial.print(",");Serial.print(angleG, 6);Serial.print(",");Serial.print(Com_angle, 6);Serial.print(",");Serial.print(Com2_angle, 6);Serial.print(",");Serial.print(Klm_angle, 6);Serial.print(";"); // 输出数据delay(50);}int gDat(int device, int axis) {// 读九轴姿态传感器寄存器函数// For Arduino, by 黑马// 调用参数表// type deviceaxis//0 1 2// ADXL345Acc x y z// L3G4200D Gyr x y z// HMC5883L Mag x z y// Example// 00 #include <Wire.h>// 01 #define Acc 0x1D;// 02 #define Gyr 0x69;// 03 #define Mag 0x1E;// 04// 05 void setup() {// 06 sensor_init();// 07 delay(1000);// 08 }// 09// 10 void loop() {// 11 int Z-Gyroscope;// 12 Z-Gyroscope = gDat(Gyr, 2);// 13 delay(50);// 14 }int v;byte vL, vH, address; // 存放byte数值if (device == Acc) address = 0x32; // ADXL345的读数地址if (device == Gyr) address = 0xA8; // L3G4200D的读数地址if (device == Mag) address = 0x03; // HMC5883L的读数地址address = address + axis * 2; // 数据偏移-坐标轴Wire.beginTransmission(device); // 开始传输数据Wire.send(address); // 发送指针Wire.requestFrom(device, 2);// 请求2 byte数据while (Wire.available() < 2);// 成功获取前等待vL = Wire.receive();vH = Wire.receive(); // 读取数据Wire.endTransmission(); // 结束传输if (device == Mag) v = (vL << 8) | vH;else v = (vH << 8) | vL; // 将byte数据合并为Intreturn v; // 返回读书值}void sensor_init() {// 配置九轴姿态传感器writeRegister(Acc, 0x2D, 0b00001000);// 测量模式// 配置ADXL345writeRegister(Gyr, 0x20, 0b00001111);// 设置睡眠模式、x, y, z轴使能writeRegister(Gyr, 0x21, 0b00000000);// 选择高通滤波模式和高通截止频率writeRegister(Gyr, 0x22, 0b00000000);// 设置中断模式writeRegister(Gyr, 0x23, 0b00110000);// 设置量程(2000dps)、自检状态、SPI模式writeRegister(Gyr, 0x24, 0b00000000);// FIFO & 高通滤波// 配置L3G4200D(2000 deg/sec)writeRegister(Mag, 0x02, 0x00); // 连续测量// 配置HMC5883L}void writeRegister(int device, byte address, byte val) {// 写寄存器Wire.beginTransmission(device);Wire.send(address);Wire.send(val);Wire.endTransmission();}
如果觉得《转载：我的自平衡小车D3——滤波算法(出处: 极客工坊)》对你有帮助，请点赞、收藏，并留下你的观点哦！
