Frames and Transformations
Points & vectors & transformations, oh my!
\[ {}^{B}p = {}^{B}R_{A}\,{}^{A}p + {}^{B}t_{A} \]
| \(\phi\) | \(r\) | \(\theta\) | |
|---|---|---|---|
| \(T\) | \(\pi/2\) | \(3\) | \(3\pi/4\) |
| \(T^{-1}\) | \(-\pi/2\) | \(3\) | \(-3\pi/4\) |
Lorem ipsum dolor sit amet, consectetur adipiscing elit. Integer posuere, augue sed posuere efficitur, sem velit egestas urna, non luctus velit nisl eu turpis. Suspendisse potenti. Vestibulum ante ipsum primis in faucibus orci luctus et ultrices posuere cubilia curae; the terminology summary in Table 2 is included here only to demonstrate dynamic table references.
Lorem ipsum dolor sit amet, consectetur adipiscing elit. Sed non suscipit nunc. Praesent ut lectus sed nibh tristique tristique. Mauris ultrices enim sed nisi gravida, in tincidunt mauris posuere. Integer dictum, massa et tincidunt iaculis, lectus nisl convallis lectus, quis fermentum libero libero vitae lectus. The transformation comparison in Table 3 is referenced the same way and will renumber automatically if table order changes.
Lorem ipsum dolor sit amet, consectetur adipiscing elit. Curabitur consequat, tortor at efficitur aliquet, arcu nibh condimentum neque, ut faucibus velit nisl nec augue. Nulla facilisi. Donec ut eros sed velit eleifend faucibus non non dui. Pellentesque habitant morbi tristique senectus et netus et malesuada fames ac turpis egestas.
| Element | Role | Notes |
|---|---|---|
| Basis | Defines coordinate directions | Rotation-only object |
| Reference frame | Defines axes and origin | Rotation plus translation |
| Point | Identifies a position | Transforms affinely |
| Free vector | Identifies direction and magnitude | Transforms linearly |
| Viewpoint | What changes | Typical rule |
|---|---|---|
| Passive transform | Coordinates relative to a new frame | \(p' = Rp + t\) |
| Basis change for vectors | Components relative to rotated axes | \(v' = Rv\) |
| Active transform | Point position inside one fixed frame | Same matrix, different interpretation |
| Rigid-body motion | Pose of one frame relative to another | \(T \in SE(3)\) |
Reference Frames
A basis is defined as something that transforms under \(SO(3)\) (rotation only). A reference frame is defined as something that transforms under \(SE(3)\) (rotation and [translation] a chosen origin).
- Point coordinates transform affinely under a frame change: \(p' = Rp + t\).
- Free-vector coordinates transform by a linear map (or basis change) induced by the frame change: \(v' = Rv\).
\[ (R(p_2) + t) - (R(p_1) + t) = R(p_2) - R(p_1) = p_2' - p_1' = R(p_2 - p_1) \implies v' = Rv \]
- For rigid 3D frames (i.e., rigid-body transforms where there is no shear, scale, or reflection), that induced linear map is an \(SO(3)\) rotation.
body frame, \(B\)
The body frame, with origin \(O_B\) and basis vectors \((\mathbf{e}_{B,1}, \mathbf{e}_{B,2}, \mathbf{e}_{B,3})\), is the right-handed orthonormal coordinate frame rigidly attached to the body. This frame is collocated with the IMU.
IMU frame, \(I\)
The IMU frame is identical to the body frame. For simplicity, the body frame will be used throughout this document wherever the IMU frame is applicable (i.e., for math involving IMU-reported vectors).
global frame, \(G\)
The global frame, with origin \(O_G\) and basis vectors \((\mathbf{e}_{G,1}, \mathbf{e}_{G,2}, \mathbf{e}_{G,3})\), is the right-handed orthonormal coordinate frame (which will be treated as inertial) in which the pose of the body frame is expressed. This frame, which will be an ENU frame, serves as the project’s global reference frame.
gravity-aligned frame, \(A\)
The gravity-aligned frame, with origin \(O_A\) and basis vectors \((\mathbf{e}_{A,1}, \mathbf{e}_{A,2}, \mathbf{e}_{A,3})\), is the right-handed orthonormal coordinate frame whose \(+z\) axis points up; its yaw is arbitrary and not map-aligned.
Point Transformations
Figure: Anti-pattern: Active transformation of ego vehicle’s sensor frame origin and body frame origin all within the body frame. Needless to say, this transformation doesn’t make sense when these origins correspond to physical points that can’t move relative to the ego vehicle.
Figure: Correct: Passive transformation of ego vehicle’s sensor frame origin and body frame origin from body frame to global frame. In practical calculations, one normally only needs to transform the coordinates of \(p_S\) and \(p_B\) from their body frame representation to their global frame representation without giving any thought to the physical reference-frame basis-vectors/axes. But it is the transform of the physical reference-frame basis-vectors/axes shown in this figure that people are talking about when they say “passive is opposite of active”.
Vector Transformations
Displacement Vectors
Displacement vectors are defined as vectors that start and end at two definite physical points in the scene.
For displacement vectors such as the GPS initialization baseline
\[ \Delta p_{0\to K} = \begin{bmatrix} x_{i_K}-x_{i_0}\\ y_{i_K}-y_{i_0} \end{bmatrix}, \]
and the GPS innovation \(y=z-h\), define a generic displacement in frame \(B\) as
\[ d_B = A_B - B_B. \]
Under the same rigid-body transform,
\[ d_G = A_G - B_G = T_{GB}(A_B)-T_{GB}(B_B) = R_{GB}(A_B-B_B) = R_{GB}d_B. \]
Translation cancels because it is applied to both endpoints. See the Displacement case plots (Frame U/V, plot i and plot ii). The anti-pattern \(T_{GB}(d_B)=R_{GB}d_B+t_{GB}\) treats \(d_B\) as a point and is not the displacement-vector coordinate transform.
Newtonian free vectors
Newtonian free vectors that only rotate under coordinate changes are now considered.
For vectors such as \(\omega\), \(f\), \(g\), \(v\), and \(a\), coordinate re-expression is rotation-only:
\[ u_G = R_{GB}u_B. \]
Equivalently,
\[ u_G = T_{GB}(u_B)-T_{GB}(0), \]
which makes the cancellation of translation explicit. See the Newtonian Vector case plots. (Vectors involving moments are excluded from this subsection.)
Quaternions
Quaternion Multiplication
To define quaternion multiplication \(w_1,w_2,w_3,x_1,y_1,z_1, x_2,y_2,z_2 \in \mathbb{R}^1\) and \(\mathbf{v}_1,\mathbf{v}_2,\mathbf{v}_3 \in \mathbb{R}^3\) will be used.
Let quaternion \(q_1=(w_1,x_1,y_1,z_1)=(w_1,\mathbf{v}_1)\) and quaternion \(q_2=(w_2,x_2,y_2,z_2)=(w_2,\mathbf{v}_2)\).
And let \(\mathbf{v}_1\cdot\mathbf{v}_2\) and \(\mathbf{v}_1\times\mathbf{v}_2\) represent the standard dot product and cross product, respectively, on \(\mathbb{R}^3\).
The following (Hamilton) convention for quaternion multiplication will be used:
\[ q_3 = q_1 \otimes q_2=\Big(w_1w_2-\mathbf{v}_1\cdot\mathbf{v}_2,\;\; w_1\mathbf{v}_2+w_2\mathbf{v}_1+\mathbf{v}_1\times\mathbf{v}_2\Big). \]
Equivalently,
\[ q_3 = q_1 \otimes q_2 = (w_3,\mathbf{v}_3) \]
with
\[ w_3 = w_1w_2-\mathbf{v}_1\cdot\mathbf{v}_2,\quad \mathbf{v}_3 = w_1\mathbf{v}_2+w_2\mathbf{v}_1+\mathbf{v}_1\times\mathbf{v}_2. \]
Note that the Shuster/JPL quaternion multiplication convention is not being used, where the last term in the sum making the vector component of the product would be subtracted from the first two terms as in: \(\ldots - \mathbf{v}_1\times\mathbf{v}_2\) rather than added: \(\ldots + \mathbf{v}_1\times\mathbf{v}_2\) as has been done here.
Quaternion Rotation
To demonstrate the two methods used to rotate a vector with a quaternion, a unit quaternion \(q_{GB} = (w_{GB},x_{GB},y_{GB},z_{GB})\) –or, for simplicity, \(q_{GB} = (w_{GB},x,y,z)\)– will be used to do a passive rotation on \(v_B\in\mathbb{R}^3\) to transform it into \(v_G\in\mathbb{R}^3\). Note that for a unit quaternion, \(q_{GB}^{-1} = (w,-x,-y,-z)\).
Method One: Direct action via conjugation
Turn the vector into a pure quaternion \((0,v_B)\), then
\[ (0,v_G) \;=\; q_{GB}\ \otimes\ (0,v_B)\ \otimes\ q_{GB}^{-1}, \]
and take the vector part as \(v_G\).
Method Two: Convert the quaternion into a rotation matrix, then multiply
\[ v_G = R(q_{GB})\,v_B, \]
with
\[ R(q_{GB})= \begin{bmatrix} 1-2(y^2+z^2) & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx)\\ 2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) \end{bmatrix}. \]
Quaternion composition order
An immediate consequence of the Quaternion Rotation and Quaternion Multiplication definitions is that if \(q_1\) represents rotation 1 and \(q_2\) represents rotation 2, then the combined rotation
\[ q = q_2 \otimes q_1 \]
is rotation 1 followed by rotation 2. I.e., the rightmost rotation applies first, matching the matrix composition \(R_2 \times R_1\).
Matrices
\(R_{GB}\)
The [passive] rotation matrix \(R_{GB}\in \mathrm{SO}(3)\) that turns a vector expressed in body-frame coordinates into a vector expressed in global-frame coordinates. In other words, \(R_{GB}\) is the change-of-basis matrix from the body frame to the global frame:
\[ v_G = R_{GB}\,v_B \]