【急】请问为什么融合后的角度永远是负值？？

VisenYang

我是用的清华互补滤波的方案。
如图：刚开始的时候，融合角度会慢慢变小 降到-200左右稳定。

然后趋势 是正常的。可是为什么会是-200多呢？？不是归一化到-90到90了吗？

请大神解答解答

luckner

清华的方案实际上是一个定增益的卡尔曼滤波

VisenYang

luckner 发表于 2015-12-17 16:36
清华的方案实际上是一个定增益的卡尔曼滤波

请问 融合出来的角度不应该是 -90度到90度吗？ 为什么我这个静止时就是-200多度了呢？

VisenYang

程序如下：
/*******************************************************************************
                                                 直立宏定义
*******************************************************************************/
#define Angle_range        180.0
#define Gravity_max        2700
#define Gravity_min        770
#define Gra_Ratio      (Angle_range / (float)(Gravity_max - Gravity_min))//归一化系数，-90度到90度.
#define Gyro_Ratio         0.19 //陀螺仪比例系数
#define Gravity_Adjust_Time 3.0 //Tg  1-4
#define Gyroscope_Angle_Signe_Frequency (1000/5) //积分时间  5ms T=1/F



/*____________________________________________________________________________*/
/*                                              互补融合函数                                                 */
/*____________________________________________________________________________*/
/*功能：(参考清华代码)                                                                                  */
/*      1.计算出陀螺仪的角速度g_GyroscopeAngleSpeed                               */
/*      2.计算出加速度计的归一化角度g_GravityAngle                                     */
/*      3.得出最终融合角度g_CarAngle                                                            */
/*____________________________________________________________________________*/

void AngleCalculate(void)
{
  g_GyroscopeAngleSpeed = ((float)(Gyro_Voltage-Gyro_Offset))*Gyro_Ratio;//陀螺仪角速度
  g_GravityAngle = ((float)(Gra_Offset-Gra_Voltage))*Gra_Ratio;//加速度计归一化到-90~+90度
  g_CarAngle = g_GyroscopeAngleI;
  DeltaAngle = (g_GravityAngle-g_GyroscopeAngleI)/Gravity_Adjust_Time;
  g_GyroscopeAngleI += (g_GyroscopeAngleSpeed+DeltaAngle)/Gyroscope_Angle_Signe_Frequency;
}

天河紫星

用这个程序，有偏移是正常的，但是。。。不该这么大

VisenYang

天河紫星 发表于 2015-12-17 19:18
用这个程序，有偏移是正常的，但是。。。不该这么大

现在问题解决了。。。可是新的又来了。。可以再帮我看看吗? 为什么刚开始跟踪正常  后来就抽了呢？？

天河紫星

VisenYang 发表于 2015-12-17 20:14
现在问题解决了。。。可是新的又来了。。可以再帮我看看吗? 为什么刚开始跟踪正常  后来就抽了呢？？

没遇见过。。。这种抽法。

天河紫星

VisenYang 发表于 2015-12-17 20:14
现在问题解决了。。。可是新的又来了。。可以再帮我看看吗? 为什么刚开始跟踪正常  后来就抽了呢？？

加速度计权重太小了？

yangjiahui

用的卡尔曼。。。并无问题。。。