因仑“3+1”工程特种兵精英论坛

标题: 互补滤波和卡尔曼滤波结果比较 [打印本页]

作者: 张衍波 时间: 2015-6-10 08:58
标题: 互补滤波和卡尔曼滤波结果比较
以下是部分代码：
//每10ms执行一次
if((millis() - time) >= 10)
{
digitalWrite(14, HIGH);//用于测试程序运行时间

time = millis();

//读传感器数据
mpu_get_data();

digitalWrite(13, HIGH);

//互补滤波
angle_hb = (0.98) * (angle_hb - mpu_data.y_gyro_f * 0.01) + (0.02)*(mpu_data.x_accel_f);

//卡尔曼滤波
Kalman_Filter(mpu_data.x_accel_f, -mpu_data.y_gyro_f);

digitalWrite(13, LOW);

//串口发送数据
Serial3.print(-mpu_data.y_gyro_f);
Serial3.print(",");
Serial3.print(mpu_data.x_accel_f);
Serial3.print(",");
Serial3.print(angle_hb);
Serial3.print(",");
Serial3.print(angle);
Serial3.println("");

digitalWrite(14, LOW);
}
复制代码
转载

欢迎光临因仑“3+1”工程特种兵精英论坛 (http://bbs.enlern.com/)