四元数转旋转矩阵公式推导

容易知以下公式成立：
$$\frac{\partial Rp}{\partial p}=R$$
上面公式用四元数表示如下：
$$\frac{\partial \left(q\otimes p\otimes q^{\ast }\right)}{\partial p}=R$$
由于$\frac{\partial \left(q\otimes p\otimes q^{\ast }\right)}{\partial p}$是用四元数的形式表示的，所以即可完成四元数到旋转矩阵的转换公式推导，详细推导如下：
设$q=\left[q_0,q_v\right]$，$q_0$为实部，$q_v$为虚部，$q_v=\left[q_1,q_2,q_3\right]$，$q^{\ast }$是 $q$的共轭四元数。

$$q\otimes p\otimes q^{\ast }=\lfloor q{\rfloor }_L\lfloor {q^{\ast }|}_{R}p$$
所以：
$$\frac{\partial \left(q\otimes p\otimes q^{\ast }\right)}{\partial p}=\lfloor q{\rfloor }_L{\lfloor q^{\ast }\rfloor }_R$$
$$\begin{array}{rl}\lfloor q{\rfloor }_L{\lfloor q^{\ast }\rfloor }_R& =\left[\begin{array}{cc}{q}_0& -q_v^T\\ q_v& q_{\omega }{I}_{3\times 3}+{\left[q_v\right]}_{\times }\end{array}\right]\left[\begin{array}{cc}{q}_0& q_v^T\\ -q_v& q_0{I}_{3\times 3}+{\left[q_v\right]}_{\times }\end{array}\right]\\ & =[\begin{array}{cc}1& q_0{q}_v^T-q_v^T{q}_0-q_v^T{\left[q_v\right]}_{\times }\\ q_v{q}_0-q_0{q}_v-{\left[q_v\right]}_{\times }{q}_v& \left(q_v{q}_v^T+q_0^2{I}_{3\times 3}+2q_0{\left[q_v\right]}_{\times }+{\left[q_v\right]}_{\times }{\left[q_v\right]}_{\times }\right]\end{array}\end{array}$$
容易推出:
$$\begin{array}{rl}{q}_0{q}_v^T-q_v^T{q}_0-q_v^T{\left[q_v\right]}_{\times }& =0_{1\times 3}\\ q_v{q}_0-q_0{q}_v-{\left[q_v\right]}_{\times }{q}_v& =0_{3\times 1}\\ q_v{q}_v^T+{\left[q_v\right]}_{\times }{\left[q_v\right]}_{\times }& =-\Vert q_v\Vert I_{3\times 3}+2q_v{q}_v^T\end{array}$$
所以可以化简为：
$$\lfloor q{\rfloor }_L{\lfloor q^{\ast }\rfloor }_R=\left[\begin{array}{cc}1& 0\\ 0& \left(q_0^2-\Vert q_v\Vert \right)I_{3\times 3}+2q_v{q}_v^T+2q_0{\left[q_v\right]}_{\times }\end{array}\right]$$
所以：
$$R=\left(q_0^2-\Vert q_v\Vert \right)I_{3\times 3}+2q_v{q}_v^T+2q_0{\left[q_v\right]}_{\times }$$
上述公式推导的结果和matlab推导的结果是一样的，主要代码如下：