已屏蔽 原因:{{ notice.reason }}已屏蔽
{{notice.noticeContent}}
~~空空如也
void init_Q(void)
{////////pitch  roll  yaw
q0=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(yaw/2)*sin(yaw/2);

}
void angle()  //姿态矩阵
{
float DX,DX1,DX2,DX3,DY,DY1,DY2,DY3,DZ,DZ1,DZ2,DZ3;
float model,h0,h1,h2,h3;  //h表示增量四元数
float q0,q1,q2,q3;          //q表示更新前的四元数
float model_d,d0,d1,d2,d3;   //d表示更新后的四元数
float T11,T12,T13,T21,T22,T23,T31,T32,T33;

//三子样采样优化 D表示角增量
DX=DX1+DX2+DX3+(9/20)*(DY1*DZ3+DZ1*DY3)+(27/40)*(DY2*(DZ3-DZ1)+DX2*(DX3-DX1));

DY=DY1+DY2+DY3+(9/20)*(DY1*DY3+DX1*DZ3)+(27/40)*(DZ2*(DX3-DX1)+DY2*(DY3-DY1));

DZ=DZ1+DZ2+DZ3+(9/20)*(DZ1*DZ3+DY1*DX3)+(27/40)*(DX2*(DY3-DY1)+DY2*(DZ3-DZ1));

//计算增量四元数h
model=sqrt(DX*DX+DY*DY+DZ*DZ);
if(model==0)
{
  h0=1;  
  h1=0;
  h2=0;
  h3=0;
}
else
{
  h0=cos(model/2);
  h1=(DX/model)*sin(model/2);
  h2=(DY/model)*sin(model/2);    
  h3=(DZ/model)*sin(model/2);    
}

//四元数更新
d0=q0*h0-q1*h1-q2*h2-q3*h3;
///////////////////////////
d1=q1*h0+q0*h1-q3*h2+q2*h3;      
d2=q2*h0+q3*h1+q0*h2-q1*h3;    
d3=q3*h0-q2*h1+q1*h2+q0*h3;
//进行规范化
model_d=sqrt(d0*d0+d1*d1+d2*d2+d3*d3);
d0=d0/model_d;
d1=d1/model_d;
d2=d2/model_d;
d3=d3/model_d;
//将更新和规范化之后的四元数d赋给q,以便下一次的更新
q0=d0;
q1=d1;
q2=d2;
q3=d3;

//将四元数转化成方向余弦矩阵
T11=q0*q0+q1*q1-q2*q2-q3*q3;
T12=2*(q1*q2-q0*q3);
T13=2*(q1*q3+q0*q2);

T21=2*(q1*q2+q0*q3);
T22=q0*q0-q1*q1+q2*q2-q3*q3;
T23=2*(q2*q3-q0*q1);

T31=2*(q1*q3-q0*q2);
T32=2*(q2*q3+q0*q1);
T33=q0*q0-q1*q1-q2*q2+q3*q3;

//提取姿态角
pitch=asin(T32);
if(T22==0&&T12>0)
yaw=pi/2;
if(T22==0&&T12<0)
yaw=-pi/2;
if(T22>0&&T12>0)
yaw=atan(-T31/T33);
if(T22>0&&T12<0)
yaw=atan(-T31/T33);
if(T22<0&&T12>0)
yaw=atan(-T31/T33)+2*pi;
if(T22<0&&T12<0)
yaw=atan(-T31/T33)-2*pi;
}
文号 / 328174

浪迹天涯
名片发私信
学术分 0
总主题 5 帖总回复 18 楼拥有证书:笔友
注册于 2011-09-20 22:32最后登录 2018-01-10 00:48
主体类型:个人
所属领域:无
认证方式:邮箱
IP归属地:未同步

个人简介

暂未填写
文件下载
加载中...
{{errorInfo}}
{{downloadWarning}}
你在 {{downloadTime}} 下载过当前文件。
文件名称:{{resource.defaultFile.name}}
下载次数:{{resource.hits}}
上传用户:{{uploader.username}}
所需积分:{{costScores}},{{holdScores}}下载当前附件免费{{description}}
积分不足,去充值
文件已丢失

当前账号的附件下载数量限制如下:
时段 个数
{{f.startingTime}}点 - {{f.endTime}}点 {{f.fileCount}}
视频暂不能访问,请登录试试
仅供内部学术交流或培训使用,请先保存到本地。本内容不代表科创观点,未经原作者同意,请勿转载。
音频暂不能访问,请登录试试
投诉或举报
加载中...
{{tip}}
请选择违规类型:
{{reason.type}}

空空如也

插入资源
全部
图片
视频
音频
附件
全部
未使用
已使用
正在上传
空空如也~
上传中..{{f.progress}}%
处理中..
上传失败,点击重试
等待中...
{{f.name}}
空空如也~
(视频){{r.oname}}
{{selectedResourcesId.indexOf(r.rid) + 1}}
处理中..
处理失败
插入表情
我的表情
共享表情
Emoji
上传
注意事项
最大尺寸100px,超过会被压缩。为保证效果,建议上传前自行处理。
建议上传自己DIY的表情,严禁上传侵权内容。
点击重试等待上传{{s.progress}}%处理中...已上传,正在处理中
空空如也~
处理中...
处理失败
加载中...
草稿箱
加载中...
此处只插入正文,如果要使用草稿中的其余内容,请点击继续创作。
{{fromNow(d.toc)}}
{{getDraftInfo(d)}}
标题:{{d.t}}
内容:{{d.c}}
继续创作
删除插入插入
插入公式
评论控制
加载中...
文号:{{pid}}
加载中...
详情
详情
推送到专栏从专栏移除
设为匿名取消匿名
查看作者
回复
只看作者
加入收藏取消收藏
收藏
取消收藏
折叠回复
置顶取消置顶
评学术分
鼓励
设为精选取消精选
管理提醒
编辑
通过审核
评论控制
退修或删除
历史版本
违规记录
投诉或举报
加入黑名单移除黑名单
查看IP
{{format('YYYY/MM/DD HH:mm:ss', toc)}}
ID: {{user.uid}}