直立两轮平衡车核心代码
两轮平衡车相关算法 卡尔曼滤波函数 /************************************************************** 函数名称:float KalmanFilter (float ACC_Angle, float GYRO_RealY) 函数功能:加速度计和陀螺仪角度融合(卡称曼滤波) 输入参数: ACC_ Angle ( 加速度计计算出的角度) GYRO RealY (陀螺仪测量的角速度) 输出参数: Attitude_ Angle_ Y融合角度 **************************************************************/ //卡尔曼滤波的部分参数 #define Peried 1/500.0f //此参数基本不改变 #define Q 20.2f //增加Q，可以增加高频响应，但会影响融合曲线的平滑性 #define R 35.999f // float KalmanGain =1.0f; // float KalmanFilter(float ACC_Angle,float GYRO_RealY) { //卡尔曼滤波局部参量 static float Priori_Estimation = 0; //先验估计 static float Posterior_Estimation =