圆点博士小四轴算法快速入门[转].docVIP

飞控的算法代码一般包括下面三个部分：滤波，姿态，PID1，滤波可以用互补滤波来实现，互补滤波的资料很多，大家随便就能找到。基本公式是：?2，滤波完就是四元数拉。直接用老外Madgwick的IMU就可以。超级简单unsigned char BS004_IMU_Update(float ax,float ay,float az,float gx,float gy,float gz)?{? ? ? ? float norm;? ? ? ? float vx, vy, vz;? ? ? ? float ex, ey, ez;????//? ? ? ??? ? ? ? //圆点博士:四元数乘法运算? ? ? ? float q0q0 = q0 * q0;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??? ? ? ? float q0q1 = q0 * q1;? ? ? ? float q0q2 = q0 * q2;? ? ? ? float q1q1 = q1 * q1;? ? ? ? float q1q3 = q1 * q3;? ? ? ? float q2q2 = q2 * q2;? ? ? ? float q2q3 = q2 * q3;? ? ? ? float q3q3 = q