来秀个世上最简单的捷联惯导算法C源码
fkepdcjgd2011/09/30航天技术 IP:安徽
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;
}
来自:航空航天 / 航天技术
4
已屏蔽 原因:{{ notice.reason }}已屏蔽
{{notice.noticeContent}}
~~空空如也
fkepdcjgd 作者
13年4个月前 IP:未同步
325268
这个给不给力,先不说;想说的是,这个地方的网速显得特别的慢,同志们有没有同感?
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
riversfox
13年4个月前 IP:未同步
325269
网速确实不快.
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
justkaka
13年3个月前 IP:未同步
326337
应该有使用例子。
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
Colin胡
1年4个月前 IP:上海
923448

你这个算法是定轴转动条件下的递推公式吧,如果要上箭更新频率能保证误差不会大得离谱?

引用
评论(1)
1
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论

想参与大家的讨论?现在就 登录 或者 注册

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

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

空空如也

加载中...
详情
详情
推送到专栏从专栏移除
设为匿名取消匿名
查看作者
回复
只看作者
加入收藏取消收藏
收藏
取消收藏
折叠回复
置顶取消置顶
评学术分
鼓励
设为精选取消精选
管理提醒
编辑
通过审核
评论控制
退修或删除
历史版本
违规记录
投诉或举报
加入黑名单移除黑名单
查看IP
{{format('YYYY/MM/DD HH:mm:ss', toc)}}