2012/5/28

德国人代码中的姿态检测算法： 
首先，将陀螺仪和加速度及的测量值减常值误差，得到角速度和加速度，并对角速度进行积分，然后对陀螺仪积分和加速度计的数值进行融合。融合分为两部分，实时融合和长期融合,实时融合每一次算法周期都要执行，而长期融合每256个检测周期执行一次，（注意检测周期小于控制周期的2ms） 
实时融合： 
1.将陀螺仪积分和加表滤波后的值做差； 
2.按照情况对差值进行衰减，并作限幅处理； 
3.将衰减值加入到角度中。 
长期融合： 
长期融合主要包括两个部分，一是对角速度的漂移进行估计（估计值是要在每一个控制周期都耦合到角度中的），二是对陀螺仪的常值误差（也就是陀螺仪中立点）进行实时的修正。 
1.将陀螺仪积分的积分和加速度积分做差（PS：为什么这里要使用加表积分和陀螺仪积分的积分，因为在256个检测周期内，有一些加速度计的值含有有害的加速度分量，如果只使用一个时刻的加表值对陀螺仪漂移进行估计，显然估计值不会准确，使用多个周期的值进行叠加后做座平均处理，可以减小随机的有害加速度对估计陀螺仪漂移过程中所锁产生的影响） 
2.将上面两个值进行衰减，得到估计的陀螺仪漂移 
3.对使考虑了陀螺仪漂移和不考虑陀螺仪漂移得到的角度做差，如果这两个值较大，说明陀螺仪在前段时间内测到的角速率不够准确，需要对差值误差（也就是陀螺仪中立点）进行修正，修正幅度和差值有关 
长期融合十分关键，如果不能对陀螺仪漂移做修正，则系统运行一段时间后，速率环的稳定性会降低