1、* Author: x2d* Copyright (c) 2012 China*/#ifndef KalmanFilter_h#define KalmanFilter_h#include class KalmanFilterpublic:KalmanFilter();/*融合計算angle_m: 加速度計測量并通過atan2(ax,ay)方法計算得角度（弧度值）gyro_m:陀螺儀測量 角速度值（弧度值）dt:采樣時間(s)outAngle:融合計算出 角度（弧度值）outAngleDot:融合計算出 角速度（弧度值）*/void getValue(double angle_m, double

2、 gyro_m, double dt, double &outAngle, double&outAngleDot);private:*-original給 arduino濾波融合算法 非我只 封裝了算法另外難 問題應該給點分才厚道啊H 文件：/* KalmanFilter.h * Author: x2d* Copyright (c) 2012 China*/#include KalmanFilter.hKalmanFilter:KalmanFilter()C_0 = 1.0f;Q_angle = 0.001f;Q_gyro = 0.003f;R_angle = 0.5f;q_bias = an

3、gle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;angle = angle_dot = 0.0f;P00 = 1.0f;P01 = 0.0f;P10 = 0.0f;P11 = 1.0f;Pdot0 = 0.0f;Pdot1 = 0.0f;Pdot2 = 0.0f;Pdot3 = 0.0f;voidKalmanFilter:getValue(double angle_m, double gyro_m, double dt, double&outAngle, double &outAngleDot)*-originaldoubl

4、e C_0, Q_angle, Q_gyro, R_angle;double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;double angle, angle_dot;double P22;double Pdot4;CPP 文件：/* KalmanFilter.cpp*/angle+=(gyro_m-q_bias) * dt;angle_err = angle_m - angle;Pdot0 = Q_angle - P01 - P10;Pdot1 = -P11;Pdot2 = -P11;Pdot3 = Q_gyro;P00 +

5、= Pdot0 * dt;P01 += Pdot1 * dt;P10 += Pdot2 * dt;P11 += Pdot3 * dt;PCt_0 = C_0 * P00;PCt_1 = C_0 * P10;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 * P01;P00 -= K_0 * t_0;P01 -= K_0 * t_1;P10 -= K_1 * t_0;P11 -= K_1 * t_1;angle += K_0 * angle_err;q_bias += K_1 * angle_err;angle_dot = gyro_m-q_bias;outAngle = angle;outAngleDot = angle_dot;/*Serial.pr(;);Serial.pr(gyro_m);Serial.pr(gyro_m = );Serial.pr(;);Serial.pr(angle_m);Serial.pr(angle_m = );/*/#endifSerial.pr(;);Seria