两个旋转矩阵相乘的李代数扰动求导

有一些非常有意思的求导方式,特此记录下来

1. 

\frac{\partial R_1R_2}{\partial R_1} =\underset{\phi\rightarrow0}{lim}{}\frac{log(R_1exp(\phi)R_2\ominus (R_1R_2))}{\phi} \displaystyle \\=lim\frac{log((R_1R_2)^TR_1exp(\phi)R_2)}{\phi} \\=lim\frac{log(R_2^Texp(\phi)R_2)}{\phi}\\ =lim\frac{log(exp(R_2^T\phi))}{\phi} \\=lim\frac{R_2^T\phi}{\phi}\\=R_2^T

2.

\frac{\partial{R_1R_2}}{\partial{R_1}}=lim\frac{log(R_1exp(\phi)R_2)-log(R_1R_2)}{\phi}\\=lim\frac{log(R_1R_2exp(R_2^T\phi))-log(R_1R_2)}{\phi}\\=lim\frac{log(R_1R_2)+J_r^{-1}(R_1R_2)log(exp(R_2^T\phi))-log(R_1R_2)}{\phi}\\=lim\frac{J_r^{-1}(R_1R_2)R_2^T\phi}{\phi}\\=J_r^{-1}(R_1R_2)R_2^T

所以看起来J_r^{-1}是个小量,可以去掉的

3.

\frac{\partial{R_1R_2}}{\partial{R_2}}=lim\frac{log(R_1R_2exp(\phi)\ominus (R_1R_2))}{\phi}\\=lim\frac{log((R_1R_2)^TR_1R_2exp(\phi))}{\phi}\\=lim\frac{log(exp(\phi))}{\phi}\\=lim\frac{\phi}{\phi}\\=I

4.

\frac{\partial{R_1R_2}}{\partial{R_2}}=lim\frac{log(R_1R_2exp(\phi))-log(R_1R_2)}{\phi}\\=lim\frac{log(R_1R_2)-J_r^{-1}(R_1R_2)log(exp(\phi))-log(R_1R_2)}{\phi}\\=lim\frac{J_r^{-1}(R_1R_2)\phi}{\phi}\\=J_r^{-1}(R_1R_2)

同样,J_r^{-1}是个小量

5. 关于wheel积分与的一些感想

通常wheel只能积分出在xy平面的平移和旋转,但通常我们的状态量是三维空间的李代数,那么对于旋转来讲,它的残差可以这样去设置

e={\begin{bmatrix} 0 & 0 & 1 \end{bmatrix}}^T

res = {yawrate} * dt - e^T * so3

它的含义是,so3是轴角,他是一个三维向量,那么它与e^T的点积是一个浮点数,代表了它在z轴上的投影量

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

神气爱哥

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值