P17四元数微分方程求解

姿态计算四元数精确解1三、四元数微分方程式及其解由第一章，四元数微分方程式：qqb对的类似处理b32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx精确解：)0(2sin2cos)(000qItq其中：21ttbdt姿态计算四元数精确解2)0(2sin2cos)(000qItq其中：21ttbdt0000xyzxzyyzxzyx22220ZYX姿态实时计算概述四元数的实时计算因假定“数学平台”跟踪地理坐标系，因此biEbibbEb四元数微分方程：)()()(tqtqbiEbib增量算法四元数2、四元数微分方程的计算：)0(][2sin2cos)(0000qItq其中，I为单位四元数，[Δθ]如（6-24）所示：21ttbdt0000xyzxzyyzxzyx写成迭代形式：)(][2sin2cos)1(0000nqInq增量算法四元数设2cos0C002sinS一阶算法：)(][21)1(nqInq增量算法四元数)(][21)1(nqInq)(1212121211212121211212121211nqXYZXZYYZXZYX增量算法四元数同理，可得二阶算法：)(][21)81()1(20nqInq三阶算法：)(])[4821()81()1(2020nqInq四阶算法：)(])[4821()38481()1(204020nqInq数值积分1阶一般式用一阶~四阶龙格-库塔积分四元数微分方程1、一阶龙格-库塔法(Runge-Kutta)一个矩阵微分方程)](),([)(ttXftX当初始条件已知，其一阶龙格-库塔的解为:)](),([)()(ttXTftXTtX数值积分1阶四元数（2）四元数微分方程qqb32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx或一阶龙格-库塔解)()()()(tqtTtqTtqb数值积分2阶一般式2、二阶龙格-库塔法对一阶算法适当改进，使平均斜率更准确一些)](),([1ttXfK1)(TKtXY)](,[2TtYfK二阶龙格-库塔算法的解：][2)()(21KKTtXTtX数值积分2阶四元数（2）四元数微分方程)()(1tqtKb1)(TKtqYYTtKb)(2)(2)()(21KKTtqTtq数值积分4阶一般式3、四阶龙格-库塔法)](),([)(ttXftX)](),([1ttXfK)]2(,2)([12TtTKtXfK)]2(,2)([23TtTKtXfK)]2(,)([34TtTKtXfK则解]22[6)()(4321KKKKTtXTtX数值积分4阶四元数（2）四元数微分方程qq)()(1tqtKb]2)()][2([12TKtqTtKb]2)()][2([23TKtqTtKb])()][([34TKtqTtKb则解]22[6)()(4321KKKKTtqTtq