角速度变化时四元数和旋转矩阵微分方程的证明

摘要： 本文证明了在角速度向量不是常数时，四元数和旋转矩阵微分方程依然成立，成立的条件和性质等，并指出了大部分资料里给出四元数微分方程时没有提到的一个关于角速度向量在哪个坐标系下表示的重要细节，最后给出仿真验证。

四元数和旋转矩阵微分方程分别指下面两式
$$\begin{gathered} \dot{Q}(t)=\frac{1}{2}Q(t)\circ W(t) \\ \dot{R}(t)=\vec{w}(t)\times R(t) \end{gathered}$$

1. 四元数微分方程的证明

  首先列出一些需要用到的四元数公式（基本知识见参考链接[1]），其中 $\circ$ 表示四元数乘法。
$$\begin{gathered} Q_1\circ Q_2=[s_1,{\vec{v}}_1]\circ [s_2,{\vec{v}}_2]=[s_1{s}_2-{\vec{v}}_1\cdot {\vec{v}}_2,s_1{\vec{v}}_2+s_2{\vec{v}}_1+{\vec{v}}_1\times {\vec{v}}_2] \\ Q=[s,\vec{v}],Q^{-1}=[s,-\vec{v}],Q\circ Q^{-1}=[1,0] \\ \frac{\text{d}Q}{\text{d}t}\circ Q^{-1}+Q\circ \frac{\text{d}(Q^{-1})}{\text{d}t}=0 \end{gathered}$$
四元数微分方程有两个等价形式，本文主要推导更常见的第一种形式
[ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ 0 − w x − w y − w z w x 0 − w z w y w y w z 0 − w x w z − w y w x 0 ] [ q 0 q 1 q 2 q 3 ]

=\frac{1}{2} ​q˙​0​q˙​1​q˙​2​q˙​3​​ ​=21​ ​0wx​wy​wz​​−wx​0wz​−wy​​−wy​−wz​0wx​​−wz​wy​−wx​0​ ​ ​q0​q1​q2​q3​​ ​
当把角速度向量写成四元数形式（即 $W=[0,\vec{w}]$）时还可以写作
$$\dot{Q}(t)=\frac{1}{2}W(t)\circ Q(t)$$
或写成向量形式
[ s ˙ v ⃗ ˙ ] = 1 2 [ 0 − w ⃗ T w ⃗ w ⃗ × ] [ s v ⃗ ]

[s˙v→˙]" role="presentation" style="position: relative;">[s˙v⃗ ˙]$$\left[\begin{array}{c}\dot{s}\\ \dot{\overrightarrow{v}}\end{array}\right]$$

=\frac{1}{2}

[0−w→Tw→w→×]" role="presentation" style="position: relative;">[0w⃗ −w⃗ Tw⃗ ×]$$\left[\begin{array}{cc}0& -{\overrightarrow{w}}^{\text{T}}\\ \overrightarrow{w}& {\overrightarrow{w}}^{\times }\end{array}\right]$$

[sv→]" role="presentation" style="position: relative;">[sv⃗ ]$$\left[\begin{array}{c}s\\ \overrightarrow{v}\end{array}\right]$$

[s˙v ˙​]=21​[0w ​−w Tw ×​][sv ​]
特别注意，当角速度在世界系下表示时，这里的 ${\vec{w}}^{\times }$ 前面的符号为正，大部分资料里给出的都是角速度在本体系下的表示，此时符号为负，后面会详细解释。

1.1 四元数旋转公式

  对任意单位四元数
$$Q=\left[\cos \frac{\theta }{2},\vec{n}\sin \frac{\theta }{2}\right]$$
和任意向量 $\vec{v}\in {\mathbb{R}}^3$，下面的公式（或算子）
$$L_q(\vec{v})=Q\circ V\circ Q^{-1}$$
等价于将向量 $\vec{v}$ 沿旋转轴 $\vec{n}$ 旋转了 $\theta$ 角度，其中 $V=[0,\vec{v}]$ 表示向量 $\vec{v}$ 的四元数形式。
证明： （见参考链接[2]，有空继续补充）

1.2 正式推导

  这个推导来自参考链接[3]，与参考链接[4]相比我认为更简单，没有复杂的坐标系变换，只用了基本的求导，证明过程非常精彩。推导中为了方便起见省略了四元数乘法符号 $\circ$。
  根据四元数旋转公式
$$R(t)=Q(t)R_0{Q}^{-1}(t)$$
其中 $R(t)$ 表示向量 $\vec{R}$ 的四元数形式，于是
R ˙ ( t ) = Q ˙ ( t ) R 0 Q − 1 ( t ) + Q ( t ) R 0 d d t ( Q − 1 ( t ) ) = Q ˙ Q − 1 R Q Q − 1 + Q Q − 1 R Q d d t ( Q − 1 ) = Q ˙ Q − 1 R + R Q d d t ( Q − 1 ) = Q ˙ Q − 1 R − R Q ˙ Q − 1

R˙(t)====​Q˙​(t)R0​Q−1(t)+Q(t)R0​dtd​(Q−1(t))Q˙​Q−1RQQ−1+QQ−1RQdtd​(Q−1)Q˙​Q−1R+RQdtd​(Q−1)Q˙​Q−1R−RQ˙​Q−1​
其中第1行到第2行是因为 $R_0=Q^{-1}(t)R(t)Q(t)$。因为单位四元数模长为1，等式两边对 $t$ 求导
q 0 2 ( t ) + q 1 2 ( t ) + q 2 2 ( t ) + q 3 2 ( t ) = 1 2 q 0 q ˙ 0 + 2 q 1 q ˙ 1 + 2 q 2 q ˙ 2 + 2 q 3 q ˙ 3 = 0

q02(t)+q12(t)+q22(t)+q32(t)=12q0q˙0+2q1q˙1+2q2q˙2+2q3q˙3=0" role="presentation" style="position: relative;">q20(t)+q21(t)+q22(t)+q23(t)=12q0q˙0+2q1q˙1+2q2q˙2+2q3q˙3=0$$\begin{array}{rl}& q_0^2(t)+q_1^2(t)+q_2^2(t)+q_3^2(t)=1\\ & 2q_0{\dot{q}}_0+2q_1{\dot{q}}_1+2q_2{\dot{q}}_2+2q_3{\dot{q}}_3=0\end{array}$$

​q02​(t)+q12​(t)+q22​(t)+q32​(t)=12q0​q˙​0​+2q1​q˙​1​+2q2​q˙​2​+2q3​q˙​3​=0​
所以
Q ˙ Q − 1 = [ q ˙ s , q ⃗ v ˙ ] [ q s , − q ⃗ v ] = [ q ˙ s q s + q ⃗ v ˙ ⋅ q ⃗ v , q s q ⃗ v ˙ − q ˙ s q ⃗ v − q ⃗ ˙ v × q ⃗ v ] = [ 0 , q s q ⃗ v ˙ − q ˙ s q ⃗ v − q ⃗ ˙ v × q ⃗ v ]

Q˙Q−1=[q˙s,q→v˙][qs,−q→v]=[q˙sqs+q→v˙⋅q→v,qsq→v˙−q˙sq→v−q→˙v×q→v]=[0,qsq→v˙−q˙sq→v−q→˙v×q→v]" role="presentation" style="position: relative;">Q˙Q−1===[q˙s,q⃗ v˙][qs,−q⃗ v][q˙sqs+q⃗ v˙⋅q⃗ v,qsq⃗ v˙−q˙sq⃗ v−q⃗ ˙v×q⃗ v][0,qsq⃗ v˙−q˙sq⃗ v−q⃗ ˙v×q⃗ v]$$\begin{array}{rl}\dot{Q}{Q}^{-1}=& [{\dot{q}}_s,\dot{{\overrightarrow{q}}_v}][q_s,-{\overrightarrow{q}}_v]\\ =& [{\dot{q}}_s{q}_s+\dot{{\overrightarrow{q}}_v}\cdot {\overrightarrow{q}}_v,q_s\dot{{\overrightarrow{q}}_v}-{\dot{q}}_s{\overrightarrow{q}}_v-{\dot{\overrightarrow{q}}}_v\times {\overrightarrow{q}}_v]\\ =& [0,q_s\dot{{\overrightarrow{q}}_v}-{\dot{q}}_s{\overrightarrow{q}}_v-{\dot{\overrightarrow{q}}}_v\times {\overrightarrow{q}}_v]\end{array}$$

Q˙​Q−1===​[q˙​s​,q ​v​˙​][qs​,−q ​v​][q˙​s​qs​+q ​v​˙​⋅q ​v​,qs​q ​v​˙​−q˙​s​q ​v​−q ​˙​v​×q ​v​][0,qs​q ​v​˙​−q˙​s​q ​v​−q ​˙​v​×q ​v​]​
将 $\dot{Q}{Q}^{-1}$ 的向量部分简记作 ${\vec{v}}_1=q_s\dot{{\vec{q}}_v}-{\dot{q}}_s{\vec{q}}_v-{\dot{\vec{q}}}_v\times {\vec{q}}_v$（后面也不会再拆开了），继续推导
R ˙ ( t ) = [ 0 , v ⃗ 1 ] [ 0 , r ⃗ ] − [ 0 , r ⃗ ] [ 0 , v ⃗ 1 ] = [ 0 , 2 v ⃗ 1 × r ⃗ ]

R˙(t)=[0,v→1][0,r→]−[0,r→][0,v→1]=[0,2v→1×r→]" role="presentation" style="position: relative;">R˙(t)==[0,v⃗ 1][0,r⃗ ]−[0,r⃗ ][0,v⃗ 1][0,2v⃗ 1×r⃗ ]$$\begin{array}{rl}\dot{R}(t)=& [0,{\overrightarrow{v}}_1][0,\overrightarrow{r}]-[0,\overrightarrow{r}][0,{\overrightarrow{v}}_1]\\ =& [0,2{\overrightarrow{v}}_1\times \overrightarrow{r}]\end{array}$$

R˙(t)==​[0,v 1​][0,r ]−[0,r ][0,v 1​][0,2v 1​×r ]​
根据角速度公式得到
$$\dot{\vec{r}}=\vec{w}\times \vec{r}=2{\vec{v}}_1\times \vec{r}$$
因为该式对刚体上的任意向量 $\vec{r}$ 都成立，所以必然有 $\vec{w}=2{\vec{v}}_1$，写成四元数形式为
$$W=2\dot{Q}{Q}^{-1},\dot{Q}=\frac{1}{2}WQ$$
式中的 $W$ 和 $Q$ 都是四元数，与矩阵形式等价。

1.3 角速度不变的特殊情况

  角速度不变时，角速度向量与四元数的虚部方向相同
Q ( t ) = [ cos ⁡ θ ( t ) 2 i sin ⁡ θ ( t ) 2 j sin ⁡ θ ( t ) 2 k sin ⁡ θ ( t ) 2 ] = [ cos ⁡ w t 2 w x w sin ⁡ w t 2 w y w sin ⁡ w t 2 w z w sin ⁡ w t 2 ] = [ cos ⁡ w t 2 ( 1 w sin ⁡ w t 2 ) w ⃗ ] Q(t)=

= = Q(t)= ​cos2θ(t)​isin2θ(t)​jsin2θ(t)​ksin2θ(t)​​ ​= ​cos2wt​wwx​​sin2wt​wwy​​sin2wt​wwz​​sin2wt​​ ​=[cos2wt​(w1​sin2wt​)w ​]
其中 $w=\sqrt{w_x^2+w_y^2+w_z^2}$ 是角速度大小。
W Q ( t ) = [ 0 − w ⃗ T w ⃗ w ⃗ × ] [ cos ⁡ w t 2 ( 1 w sin ⁡ w t 2 ) w ⃗ ] = [ − ( 1 w sin ⁡ w t 2 ) w ⃗ T w ⃗ w ⃗ cos ⁡ w t 2 + ( 1 w sin ⁡ w t 2 ) w ⃗ × w ⃗ ] = [ − w sin ⁡ w t 2 w ⃗ cos ⁡ w t 2 ] = 2 Q ˙ ( t )

WQ(t)=[0−w→Tw→w→×][cos⁡wt2(1wsin⁡wt2)w→]=[−(1wsin⁡wt2)w→Tw→w→cos⁡wt2+(1wsin⁡wt2)w→×w→]=[−wsin⁡wt2w→cos⁡wt2]=2Q˙(t)" role="presentation" style="position: relative;">WQ(t)===[0w⃗ −w⃗ Tw⃗ ×][coswt2(1wsinwt2)w⃗ ][−(1wsinwt2)w⃗ Tw⃗ w⃗ coswt2+(1wsinwt2)w⃗ ×w⃗ ][−wsinwt2w⃗ coswt2]=2Q˙(t)$$\begin{array}{rl}WQ(t)=& \left[\begin{array}{cc}0& -{\overrightarrow{w}}^{\text{T}}\\ \overrightarrow{w}& {\overrightarrow{w}}^{\times }\end{array}\right]\left[\begin{array}{c}\cos \frac{wt}{2}\\ \left(\frac{1}{w}\sin \frac{wt}{2}\right)\overrightarrow{w}\end{array}\right]\\ =& \left[\begin{array}{c}-\left(\frac{1}{w}\sin \frac{wt}{2}\right){\overrightarrow{w}}^{\text{T}}\overrightarrow{w}\\ \overrightarrow{w}\cos \frac{wt}{2}+\left(\frac{1}{w}\sin \frac{wt}{2}\right)\overrightarrow{w}\times \overrightarrow{w}\end{array}\right]\\ =& \left[\begin{array}{c}-w\sin \frac{wt}{2}\\ \overrightarrow{w}\cos \frac{wt}{2}\end{array}\right]=2\dot{Q}(t)\end{array}$$

WQ(t)===​[0w ​−w Tw ×​][cos2wt​(w1​sin2wt​)w ​][−(w1​sin2wt​)w Tw w cos2wt​+(w1​sin2wt​)w ×w ​][−wsin2wt​w cos2wt​​]=2Q˙​(t)​
但是当角速度变化时，也就是 $\theta (t)\ne wt$ 时，这种关系不成立，角速度向量与四元数的虚部不再同向。这两者之间的关系有待进一步研究。
  进一步研究发现，不需要严格满足角速度是常数，只需要保持角速度的方向不变，四元数的虚部就与角速度方向相同。这一点也可以直观理解，四元数可以近似看作角速度积分后的结果，如果角速度一直在变的话，四元数虚部的方向就可以看作是，从0时刻开始到当前时刻的过程中一直在变化的方向的"平均方向"，而角速度是当前时刻的瞬时方向，所以当且仅当角速度方向不变时，四元数虚部与角速度方向相同。

1.4 叉乘项的符号问题

  前面推出当角速度在世界系下表示时的四元数微分方程为
$$\dot{Q}=\frac{1}{2}WQ$$
现在用四元数将角速度从世界系转换到本体系就可以得到主流的公式。旋转矩阵的一个含义是，将一个向量在本体系下的坐标左乘旋转矩阵就可以得到该向量在世界系下的表示，同理四元数的含义是将一个向量在本体系下的坐标进行四元数旋转操作就可以得到该向量在世界系下的表示，即
$$W_I=Q{W}_b{Q}^{-1}$$
代入四元数微分方程得到
Q ˙ = 1 2 W I Q = 1 2 Q W b Q − 1 Q = 1 2 Q W b = 1 2 [ s , v ⃗ ] [ 0 , w ⃗ ] = [ − v ⃗ T w ⃗ , s w ⃗ − w ⃗ × v ⃗ ] = 1 2 [ 0 − w ⃗ T w ⃗ − w ⃗ × ] [ s v ⃗ ] = 1 2 W Q

Q˙​====​21​WI​Q=21​QWb​Q−1Q=21​QWb​21​[s,v ][0,w ]=[−v Tw ,sw −w ×v ]21​[0w ​−w T−w ×​][sv ​]21​WQ​
也就是说，叉乘项 ${\vec{w}}^{\times }$ 的符号取决于角速度在哪个坐标系下描述惯性系下为正，本体系下为负。旋转矩阵微分方程也有类似的性质，后面细说。

2. 旋转矩阵微分方程的证明

旋转矩阵 $R(t)$ 满足
$$\dot{R}(t)=\vec{w}(t)\times R(t)$$
其中 $\vec{w}$ 为世界系下描述的角速度。

2.1 角速度不变的推导

  这个推导来自参考文献[5]。书中是在假设角速度不变的前提下推导的，而且也没有说明角速度向量的实际含义。
  旋转矩阵 $R(t)$ 是正交矩阵，满足
$$R{R}^{\text{T}}=I$$
等式两边同时对时间 $t$ 求导得到
$$\dot{R}{R}^{\text{T}}+R{\dot{R}}^{\text{T}}=\dot{R}{R}^{\text{T}}+(\dot{R}{R}^{\text{T}}{)}^{\text{T}}=0$$
可以看出 $\dot{R}(t)R^{\text{T}}(t)$ 是个反对称矩阵，用 $M(t)$ 表示。等式两边同时右乘 $R(t)$ 得到
$$\begin{gathered} \dot{R}(t)R^{\text{T}}(t)=M(t) \\ \dot{R}(t)=M(t)R(t) \end{gathered}$$
式中的 $M(t)$ 实际上就是角速度对应的反对称矩阵 $w^{\times }$，当角速度不变时，$M(t)$ 为常数，旋转矩阵的解为
$$\begin{gathered} \dot{R}(t)=w^{\times }R(t) \\ R(t)={\text{e}}^{w^{\times }t}{R}_0 \end{gathered}$$

例如，绕z轴的旋转矩阵为
R = [ cos ⁡ t − sin ⁡ t 0 sin ⁡ t cos ⁡ t 0 0 0 1 ] R=

R= ​costsint0​−sintcost0​001​ ​
则
w × R ( t ) = [ 0 − 1 0 1 0 0 0 0 0 ] [ cos ⁡ t − sin ⁡ t 0 sin ⁡ t cos ⁡ t 0 0 0 1 ] = [ − sin ⁡ t − cos ⁡ t 0 cos ⁡ t − sin ⁡ t 0 0 0 1 ] = R ˙ ( t ) w^\times R(t)=

[0−10100000]" role="presentation" style="position: relative;">⎡⎣⎢010−100000⎤⎦⎥$$\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]$$

[cos⁡t−sin⁡t0sin⁡tcos⁡t0001]" role="presentation" style="position: relative;">⎡⎣⎢costsint0−sintcost0001⎤⎦⎥$$\left[\begin{array}{ccc}\cos t& -\sin t& 0\\ \sin t& \cos t& 0\\ 0& 0& 1\end{array}\right]$$

=

[−sin⁡t−cos⁡t0cos⁡t−sin⁡t0001]" role="presentation" style="position: relative;">⎡⎣⎢−sintcost0−cost−sint0001⎤⎦⎥$$\left[\begin{array}{ccc}-\sin t& -\cos t& 0\\ \cos t& -\sin t& 0\\ 0& 0& 1\end{array}\right]$$

=\dot R(t) w×R(t)= ​010​−100​000​ ​ ​costsint0​−sintcost0​001​ ​= ​−sintcost0​−cost−sint0​001​ ​=R˙(t)

2.2 角速度变化的推导

  当角速度变化时,旋转矩阵微分方程仍然成立。我想到的不严谨的证明方法是只需要证明角速度公式
$$\dot{\vec{r}}(t)=\vec{\omega }(t)\times \vec{r}(t)$$
成立即可（证明见参考链接[6]），然后旋转矩阵等价于矩阵中的3个正交坐标轴向量在世界系下的坐标，因此当3个坐标轴成立时旋转矩阵就成立
[ e ⃗ x ˙ e ⃗ y ˙ e ⃗ z ˙ ] = ω ⃗ ( t ) × [ e ⃗ x e ⃗ y e ⃗ z ]

=\vec\omega(t)\times [e x​˙​​e y​˙​​e z​˙​​]=ω (t)×[e x​​e y​​e z​​]
不知道有没有严谨证明。

2.3 角速度的符号问题

  与四元数类似，当角速度在本体系下描述时且旋转矩阵表示成世界系到本体系时，角速度取负号，于是有了3种等价的旋转矩阵微分方程表示方法
R ˙ T = − w ⃗ × R T R ˙ = R w ⃗ × R ˙ = ( R w ⃗ ) × R

R˙T=R˙=R˙=​−w ×RTRw ×(Rw )×R​
其中 $\vec{w}$ 为本体系下描述的角速度，与前面不同。其中第一种是主流教材中的表示，第二种是一种较为简洁的形式，第三种的意思是先把角速度从本体系转换到世界系。下面证明这3种表示方法等价。
R ˙ = − ( w ⃗ × R T ) T = − R ( w ⃗ × ) T = R w ⃗ × R ˙ = ( R w ⃗ ) × R = R ( w ⃗ × I ) = R w ⃗ ×

R˙=−(w→×RT)T=−R(w→×)T=Rw→×R˙=(Rw→)×R=R(w→×I)=Rw→×" role="presentation" style="position: relative;">R˙=R˙=−(w⃗ ×RT)T=−R(w⃗ ×)T=Rw⃗ ×(Rw⃗ )×R=R(w⃗ ×I)=Rw⃗ ×$$\begin{array}{rl}\dot{R}=& -(\overrightarrow{w}\times R^{\text{T}}{)}^{\text{T}}=-R({\overrightarrow{w}}^{\times }{)}^{\text{T}}=R{\overrightarrow{w}}^{\times }\\ \dot{R}=& (R\overrightarrow{w})\times R=R(\overrightarrow{w}\times I)=R{\overrightarrow{w}}^{\times }\end{array}$$

R˙=R˙=​−(w ×RT)T=−R(w ×)T=Rw ×(Rw )×R=R(w ×I)=Rw ×​
其中第二行用到了向量叉乘的乘法分配律，见 刚体姿态动力学推导与进动现象仿真 的附录。

3. 参考

1. Understanding Quaternions -3D Game Engine Programming
2. Quaternions and Rotations
3. Quaternion differentiation -euclideanspace
4. 四元数微分方程的推导和解算实现 -知乎
5. 高博. 视觉SLAM十四讲[M]. 电子工业出版社.
6. How to derive the formula for angular velocity? -stackexchange
7. 汤锡生. 载人飞船轨道确定和返回控制. 国防工业出版社.
8. 屠善澄. 卫星姿态动力学与控制[M]. 宇航出版社.

• 四元数完全解析及资料汇总
• 例说姿态解算与导航09(姿态阵及四元数微分方程，姿态解算) -知乎
• 欧拉角、旋转矩阵、四元数角运动微分方程 -知乎
• 章仁为. 卫星轨道姿态动力学与控制[M].

4. 仿真验证

  下面的验证程序的思路是，同时对四元数和旋转矩阵两个微分方程
R ˙ ( t ) = w × ( t ) R ( t ) Q ˙ ( t ) = 1 2 W ( t ) Q ( t )

​R˙(t)=w×(t)R(t)Q˙​(t)=21​W(t)Q(t)​
输入变化的角速度（可以自己另外设置）
w ⃗ ( t ) = [ 1 ln ⁡ ( t + 1 ) cos ⁡ ( t ) ] \vec w(t)=

[1ln⁡(t+1)cos⁡(t)]" role="presentation" style="position: relative;">⎡⎣⎢1ln(t+1)cos(t)⎤⎦⎥$$\left[\begin{array}{c}1\\ \ln (t+1)\\ \cos (t)\end{array}\right]$$

w (t)= ​1ln(t+1)cos(t)​ ​
在 $t=1$ 到 $10$ 的10个时刻时，将输出的四元数 $Q(t)$ 转换成旋转矩阵 $R_q(t)$ 与输出的旋转矩阵 $R(t)$ 比较，可以验证两者相等，并且旋转矩阵微分方程的三种表示方法等价。
  同时可以进一步验证，当角速度的方向不固定时，四元数的虚部与角速度向量不同向。方法是把这两个向量叉乘看是否为0即可。
  simucpp 代码如下

/**************************************************************
// 验证四元数和旋转矩阵微分方程
simucpp版本：2.1.14
**************************************************************/
#include 
#include 
#include "simucpp.hpp"
using namespace simucpp;
using namespace zhnmat;
using namespace std;

double Matrix_Error(Mat materr) {
    double sumerr = 0;  // 误差平方和
    for (uint32_t i = 0; i < 3; i++)
        for (uint32_t j = 0; j < 3; j++)
            sumerr += materr.at(i, j)*materr.at(i, j);
    return sumerr;
}

int main() {
    Simulator sim1;
    auto intR = new MStateSpace(&sim1, BusSize(3, 3), true, "intR");  // 旋转矩阵R
    auto intQ = new MStateSpace(&sim1, BusSize(4, 1), true, "intQ");  // 姿态四元数Q
    auto misoR = new MFcnMISO(&sim1, BusSize(3, 3), "misoR");  // 旋转矩阵导数R'
    auto misoQ = new MFcnMISO(&sim1, BusSize(4, 1), "misoQ");  // 四元数导数Q'
    auto muxw = new Mux(&sim1, BusSize(3, 1), "muxw");  // 本体系下的角速度向量w
    UInput **inws = new UInput*[3];  // 三轴角速度
    for (uint32_t i = 0; i < 3; i++) {
        inws[i] = new UInput(&sim1, "inw"+to_string(i));
        sim1.connectU(inws[i], muxw, BusSize(i, 0));
    }
    sim1.connectM(muxw, misoR);
    sim1.connectM(intR, misoR);
    sim1.connectM(misoR, intR);
    sim1.connectM(muxw, misoQ);
    sim1.connectM(intQ, misoQ);
    sim1.connectM(misoQ, intQ);
    inws[0]->Set_Function([](double t){ return 1; });  // 设置三轴角速度函数
    inws[1]->Set_Function([](double t){ return log(t+1); });
    inws[2]->Set_Function([](double t){ return cos(t); });
    intR->Set_InitialValue(eye(3));
    intQ->Set_InitialValue(Mat(vecdble{1, 0, 0, 0}));
    misoR->Set_Function([](Mat *u){  // u[0]为本体系下的角速度向量，u[1]为旋转矩阵
        // 下面给出了三种方法
        Mat ans = Antisymmetric(u[1]*u[0])*u[1];
        // Mat ans = (Mat(3, 3)-Antisymmetric(u[0]))*u[1].T(); ans = ans.T();
        // Mat ans = u[1]*Antisymmetric(u[0]);
        return ans;
    });
    misoQ->Set_Function([](Mat *u){  // u[0]为本体系下的角速度向量，u[1]为四元数
        Mat W(4, 4, vecdble{  // 角速度向量对应的四元数乘法左矩阵
            0, -u[0].at(0, 0), -u[0].at(1, 0), -u[0].at(2, 0),
            u[0].at(0, 0), 0, u[0].at(2, 0), -u[0].at(1, 0),
            u[0].at(1, 0), -u[0].at(2, 0), 0, u[0].at(0, 0),
            u[0].at(2, 0), u[0].at(1, 0), -u[0].at(0, 0), 0,
        });
        return 0.5*W*u[1];
    });
    sim1.Set_SimStep(0.01);  // 步长0.01
    sim1.Initialize();
    Vector3d vecw, vecrefw;
    for (uint32_t i = 0; i < 2; i++) {
        for (uint32_t j = 0; j < 99; j++)  // 仿真100步为1秒
            sim1.Simulate_OneStep();
        sim1.Simulate_OneStep();
        Mat matr = intR->Get_OutValue();  // 旋转矩阵
        Mat matq = intQ->Get_OutValue();  // 四元数
        vecw = Vector3d(muxw->Get_OutValue());  //本体系下的角速度向量
        double q0 = matq.at(0, 0);
        double q1 = matq.at(1, 0);
        double q2 = matq.at(2, 0);
        double q3 = matq.at(3, 0);
        Mat matqr = Mat(3, 3, vecdble{  // 四元数转旋转矩阵
            q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2,
            2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1,
            2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3,
        });
        cout << "err:" << Matrix_Error(matr - matqr) << endl;  // 两个旋转矩阵的误差矩阵
    }
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81