upadte四元数(1)-----一阶龙格库塔法

已知一四元数这里写图片描述,对时间微分得(其中这里写图片描述这里写图片描述):
这里写图片描述
这里写图片描述,则有:
这里写图片描述

由一阶龙格库塔法这里写图片描述,其中这里写图片描述,可得微分方程这里写图片描述的解:
这里写图片描述
,进而可推导出:
这里写图片描述

可得出更新四元数的程序代码为:
q0=q0+(-wx*q1-wy*q2-wz*q3)*halt_T;
q1=q1+(wx*q0+wz*q2-wy*q3)*halt_T;
q2=q2+(wy*q0-wz*q1+wx*q3)*halt_T;
q3=q3+(wz*q0+wy*q1-wx*q2)*halt_T;

评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值