先複習以前寫過的相關文章：
- 三維空間旋轉
- 關於三維空間轉換的更多筆記
- Quaternion 四元數簡介
- 李群、李代數與三維空間旋轉 Lie Group & Lie Algebra
- SO(3) 上的李代數求導數
- 三維空間轉換與李理論（一）
- 三維空間轉換與李理論（二）
本文將以參考資料 [1, 2] 的內容為基礎整理出一份完整的筆記。
結合兩個 Pose
首先要討論的是 3D+YPR，由於沒有簡單的解，建議的做法是轉換成 3D+Quat 形式再結合兩個 pose。而 covariance 的計算也需要利用 chain rule 來將以下轉換的 jabobian 考慮進去：
- 3D+YPR 轉換至 3D+Quat 的 covariance：(Eq. 4)。
- 結合兩個 3D+Quat pose 的 covariance：馬上會介紹。
- 3D+Quat 轉換至 3D+YPR 的 covariance：(Eq. 6)。
結合兩個 3D+Quat Pose
兩個 pose 為 \(\mathbf{p_1}\)、\(\mathbf{p_2}\)，則結合之後的 pose 為 \(\mathbf{p} = \mathbf{p_1} \oplus \mathbf{p_2}\)。(Eq. 10)：
\[
p=\begin{bmatrix}
x\\
y\\
z\\
q_r\\
q_x\\
q_y\\
q_Z
\end{bmatrix}
=
f_{qn}
\begin{bmatrix}
f_{qr}(\mathbf{p_1},[x_2,y_2,z_2]^T)\\
q_{r1}q_{r2}-q_{x1}q_{x2}-q_{y1}q_{y2}-q_{z1}q_{z2}\\
q_{r1}q_{x2}+q_{r2}q_{x1}+q_{y1}q_{z2}-q_{y2}q_{z1}\\
q_{r1}q_{y2}+q_{r2}q_{y1}+q_{z1}q_{x2}-q_{z2}q_{x1}\\
q_{r1}q_{z2}+q_{r2}q_{z1}+q_{x1}q_{y2}-q_{x2}q_{y1}
\end{bmatrix}
\]
其中 \(f_{qr}(\mathbf{p_1},[x_2,y_2,z_2]^T)\) 為 (Eq. 8)，\(f_{qn}\) 為 quaternion 的 normalization。
結合兩個 3D+Quat Pose 的 covariance
兩個 pose 為 \(\mathbf{p_1}\)、\(\mathbf{p_2}\)，則結合之後的 pose 為 \(\mathbf{p} = \mathbf{p_1} \oplus \mathbf{p_2}\)。若 \(\mathbf{p_1}\) 為 \(N(\bar{\mathbf{p_1}}, cov(\mathbf{p_1}))\)，\(\mathbf{p_2}\) 為 \(N(\mathbf{\bar{p_2}}, cov(\mathbf{p_2}))\)，則 \(\mathbf{p}\) 也會是個常態分布：
- \(\mathbf{\bar{p}}=\mathbf{\bar{p_1}} \oplus \mathbf{\bar{p_2}}\)。
- \(cov(\mathbf{p})\) 的細節為 [1] 中的式 (5.7)。
Pose 的 inverse
對於 3D+YPR 來說，建議的做法也是轉換至 3D+Quat 再進行其他的轉換。
3D+Quat 的 inverse
式子可以參考 [1] 中的式 (6.1)，而 covariance 可以參考 [1] 中的式 (6.3)。
參考資料
[2] A micro Lie theory for state estimation in robotic
沒有留言:
張貼留言