原创力文档- 1、本文档共2页，可阅读全部内容。
- 2、原创力文档（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
- 3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击 这里二次下载。
- 4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。
- 5、该文档为VIP文档，如果想要下载，成为VIP会员后，下载免费。
- 6、成为VIP后，下载本文档将扣除1次下载权益。下载后，不支持退款、换文档。如有疑问请联系我们。
- 7、成为VIP后，您将拥有八大权益，权益包括：VIP文档下载权益、阅读免打扰、文档格式转换、高级专利检索、专属身份标志、高级客服、多端互通、版权登记。
- 8、VIP文档为合作方或网友上传，每下载1次， 网站将根据用户上传文档的质量评分、类型等，对文档贡献者给予高额补贴、流量扶持。如果你也想贡献VIP文档。上传文档
//叉积法融合陀螺和加速度。
voidmix_gyrAcc_crossMethod(quaternion_yuandian*attitude,constfloatgyr[3],constfloc[3],floatinterval)
{
conststaticfloatFACTOR=0001;//两个重力矢量叉积后所乘的系数p，用于和陀螺仪积分角度相叠加来修正陀螺仪（这里只用了比例p，没用积分i，）
//FACTOR为1，则完全信任加速度计，为0，则完全信任陀螺仪
floatw_q=attitude-w;//w=cos(alpha/2)
floatx_q=attitude-x;//x=ax*sin(alpha/2)
floaty_q=attitude-y;//y=ay*sin(alpha/2)
floatz_q=attitude-z;//z=az*sin(alpha/2)
floatx_q_2=x_q*2;
floaty_q_2=y_q*2;
floatz_q_2=z_q*2;
//
//加速度计的读数，单位化。
floata_rsqrt=math_rsqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
floatx_aa=acc[0]*a_rsqrt;
floaty_aa=acc[1]*a_rsqrt;
floatz_aa=acc[2]*a_rsqrt;
//
//载体坐标下的重力加速度常量，单位化。//用旋转矩阵将世界坐标系的单位化重力矢量(0,0,1）不是（0,0，-1）,mpu6050只感应非重力加速度)转换到机载坐标系中。
//机载坐标下的重力矢量旋转矩阵（坐标系转换矩阵的逆矩阵也就是转置矩阵，因为欧拉角解得的旋转矩阵必是正交阵）世界坐标下的重力矢量
//xcos(T)cos(K)cos(T)sin(C)-sin(C)0
//[y]=[sin(F)sin(T)cos(K)-cos(F)sin(K)sin(F)sin(T)sin(K)+cos(F)cos(K)sin(F)cos(T)]*[0]
//zcos(F)sin(T)cos(K)+sin(F)sin(K)cos(F)sin(T)sin(K)-sin(F)cos(K)cos(F)cos(T)1
//K是yaw,T是pitch,F是roll,旋转顺序为ZYX
//w^2+x^2-y^2-z^22*(x*y+w*z)2*(x*z-w*y)
//上式中的旋转矩阵用四元数表示即为：[2*(x*y-w*z)w^2-x^2+y^2-z^22*(y*z+w*x)]
//
文档评论（0）