四元数-第5讲

本部分雅克比矩阵较为复杂，需要结合第4讲的导数公式进行理解，请结合第4讲进行学习，后期会根据大家的反应，使用MATLAB写一些实例进行理解；

---

4.3实用的旋转雅克比

4.3.1 关于向量的雅克比

4.3.2 关于四元数的雅克比

4.3.3 SO(3)的右雅克比

4.3.4 关于旋转向量的雅克比

---

---

4.3实用的旋转雅克比

首先让我们思考一个向量的旋转，即旋转绕轴u旋转角度θ，我们可以用三种等效的旋转方式表达，即

我们对旋转后向量关于不同维度量的雅克比非常感兴趣。

4.3.1 关于向量的雅克比

旋转后向量关于原向量的微分是比较简单，

4.3.2 关于四元数的雅克比

旋转后向量关于四元数q的微分是极为复杂。为叙述方便，我们采用更为方便的四元数形式，q=[ w  v ]=w+v。

通过第一讲公式，

可以得到如下基于四元数的旋转公式：

基于上式，我们可以提取旋转后向量关于w和关于v的偏导数，

因此，

4.3.3 SO(3)的右雅克比

首先让我们思考一个属于SO(3)的元素R和一个旋转向量θ，例如R=Exp(θ)。

当θ增加δθ时，元素R也随之改变。用旋转向量δφ在SO(3)的正切空间表达R的变化量，我们可以得到

也可写为，

通过求极限，δφ关于δθ的变化率作为雅克比矩阵，（结合上式）

这个表达式恰是第4讲4.2.2部分的一个实例，即它是f(θ)=Exp(θ)的微分，即从R3到SO(3)。这个雅克比矩阵正式SO(3)的右雅克比，即

该表示独立于具体变量形式，但可以具体使用具体的变量形式，如旋转矩阵或四元数：（具体可参考第4讲4.1中SO(3)和四元数的减法算法，并结合偏导和极限的定义）

右雅克比及其逆可通过下面公式计算：

SO(3)右雅克比矩阵具有下列性质，对于任何θ和δθ，

4.3.4 关于旋转向量的雅克比

旋转后的向量关于旋转向量关系是一个R3到R3的函数。旋转后的向量关于旋转向量θ的微分可使用下式，（其中结合上面右雅克比的性质）

其中R{θ}=Exp(θ)

即，

4.3.5 旋转顺序的雅克比

思考SO(3)的元素顺序，P=Q○R，这里我们可以采用四元数或者矩阵形式，

此处，元素的顺序意味着在正切空间里向量扰动的位置；参考第4讲4.2.2；

第一个公式，对元素1-Q求偏导，第二个公式对元素2-R求偏导，