[原创] 互补滤波算法（双轮平衡车）

本人菜鸟，最近在调融合互补滤波算法。我对算法的理解大概如下。陀螺仪的短期测得的值比较准，但是由于存在温漂，进过积分，长期就不能用了。所以要通过加速度计进行修正角度，但是加速度计的短期不行，噪声比较大，所以要将陀螺仪高通，加速度计低通后进行一定比列的融合。才能比较准确的反应角度的变化。
我是这样做的：陀螺仪和加速度计分别进行了递推和一阶滤波，得到的滤波结果再按一定的比例进行拟合，采样周期为0.3ms。但是出来的效果不理想。最终直立控制量=P*（融合滤波后的角度）+D*(角速度（即陀螺仪的值）)
angle=(int)(0.99*(angle+(gyro-2940)*0.008)+0.01*(y_acc-3150))
int PIDCalc( unsigned int NextPoint )
{
  int PID;
  Error = standard - NextPoint;       // 偏差E(t)
  iError+=Error;                    //E(t)+E(t-1)+...+E(1)
  dError=Error-LastError;         //E(t)-E(t-1)
  PID=(int)(Proportion * Error        //比例
        +Integral*iError        //积分
            + Derivative * dError);          // 微分项
  PrevError = LastError;
  LastError = Error;
  return (
            PID
                        );  
}
void PIDBEGIN()
{
  Proportion = 6; // Set PID Coefficients 10
  Integral=0.002;
  Derivative = 120;         // 4
}
我用的传感器是mma-7361,enc-03
小车现象是抖动，站立不了多久，总是往一边倒去，不知道是不是陀螺仪温漂
然后传感器采的是AD值，不知道和角度的具体关系，所以不知道怎么调

陀螺仪是什么型号的??AD采集的是陀螺仪?
对采集的角速度进行积分...
如果震荡的话,试试增加采样率

陀螺仪enc-03
主要是互补滤波算法，网上给的是角度，而我采的是AD值，虽然大概成正比，但是还是不太懂怎么调
这些参数都是看效果瞎试的