互补滤波和卡尔曼滤波结果比较

张衍波

以下是部分代码：
//每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);
}
复制代码
转载