A drone doesn't just have a position, it has an orientation, and orientation hides more traps than position ever will. Order matters. Three numbers can silently weld two axes together. The fix robotics actually ships is a four-number object most people only recognize from video games.
A quadrotor's flight computer wants to correct its attitude. The ground-station message says: pitch forward 30°, then yaw right 20°. A junior firmware engineer refactors the correction loop and, thinking it's a harmless simplification, applies yaw first and pitch second, same two numbers, just reordered. The drone tips the wrong way. Nobody changed a single number. They changed the order, and in three dimensions, order is not a bookkeeping detail. It is part of the answer.
Compare that to position. If you walk 3 meters east and then 4 meters north, you land in exactly the same spot as walking 4 meters north and then 3 meters east. Addition of positions commutes, A + B always equals B + A. Orientation does not work like that. Rotating a book 90° around its vertical axis and then 90° around its horizontal axis leaves it in a different pose than doing those same two turns in the opposite order. Try it with any rectangular object on your desk right now: mark one corner, spin it one way, note where the corner ends up, then reset and spin it the other order. The corner lands somewhere else.
Here is the simulation to prove it to yourself before we write a single formula. Below is a small labeled block, think of it as a phone, with one face marked so you can track which way is "up" and which way is "forward." Press one button to apply a 90° pitch-then-90° yaw sequence; press the other to apply the same two rotations, yaw first. Watch where the marked face ends up.
The warm face marks "top", the teal face marks "front". Apply the same two 90° turns in opposite orders and compare the final pose.
Both sequences use the identical two 90° rotations. Both end after two moves. And they land in different final poses, you can see it in the block's marked faces. This is not a rounding error or a simulation bug; it's the central fact of three-dimensional rotation. It's also why "just average two orientation estimates" or "just add a small correction to my rotation matrix", both tempting shortcuts, quietly break, a trap we'll meet properly in Chapter 7.
So what do we do? This lesson is a tour of the honest ways engineers represent "which way is this thing facing," in the order you'd naturally invent them if you were solving this problem from scratch. First, the 2×2 rotation matrix in a plane, the one case where composition is just adding angles, and the foundation everything else builds on (Chapter 1). Then the 3×3 matrices that rotate 3D space and the composition rule that replaces "add angles" (Chapter 2). Then the natural but flawed idea of three angles, roll, pitch, yaw, and the exact moment it fails: gimbal lock, the failure mode that once triggered real cockpit warnings on Apollo missions (Chapter 3). Then two representations engineered specifically to have no lock: axis-angle (Chapter 4) and quaternions (Chapter 5). Then how to chain rotations and translations across a robot's frames, world, body, camera, sensor, with a single matrix multiply (Chapter 6). Then the trick every state estimator and optimizer uses to nudge an orientation without breaking it (Chapter 7). And finally a showcase lab where you build a real 3-link frame chain and watch every representation update live (Chapter 8), before a cheat-sheet chapter tells you which one to reach for and when (Chapter 9).
| Quantity | Composes by | Order matters? |
|---|---|---|
| Position | Vector addition | No |
| Orientation | Function composition (matrix / quaternion multiply) | Yes |
Before tackling 3D, master the easy case completely, by hand. Take a point in the plane and rotate it counter-clockwise by an angle θ around the origin. Where does it land?
The cleanest way to derive this is to ask what happens to the two basis arrows. The arrow pointing east, (1, 0), rotated by θ, sweeps around to (cosθ, sinθ), that's the literal definition of cosine and sine on the unit circle. The arrow pointing north, (0, 1), is already 90° ahead of east, so after the same θ rotation it lands at (−sinθ, cosθ), 90° ahead of wherever east landed. Any point (x, y) is just x copies of the east arrow plus y copies of the north arrow, and rotation is linear, so the rotated point is x times the rotated east arrow plus y times the rotated north arrow:
Packed into a matrix, that's the 2D rotation matrix:
Let's grind two hand examples so the formula stops being symbols and becomes arithmetic you trust. Rotate the point (3, 0) by 90°. cos90° = 0, sin90° = 1. x′ = 3·0 − 0·1 = 0. y′ = 3·1 + 0·0 = 3. Result: (0, 3). That matches intuition perfectly, the point due east, spun a quarter turn counter-clockwise, ends up due north, same distance from the origin.
Now a less obvious one: rotate (1, 1) by 45°. Here cos45° = sin45° = √2÷2 ≈ 0.7071. x′ = 1·0.7071 − 1·0.7071 = 0. y′ = 1·0.7071 + 1·0.7071 = 1.4142. Result: (0, 1.4142). Does that make sense? The point (1,1) already sits at 45° from the x-axis, at distance √2 ≈ 1.414 from the origin. Rotating it another 45° puts it at 90°, straight up, at the same distance from the origin, since rotation never changes length. (0, 1.414) is exactly that. Rotation preserves distance from the origin; only the direction changes.
| Arrow | Before rotation | After rotating by θ |
|---|---|---|
| East | (1, 0) | (cosθ, sinθ) |
| North | (0, 1) | (−sinθ, cosθ) |
Rotate by θ₁, then rotate the result by θ₂. Does that equal one rotation by θ₁ + θ₂? In 2D, yes, and this special case is exactly why 2D feels so effortless and 3D will feel like a trap. Multiply the matrices and the angle-addition identities from trigonometry, cos(a+b) = cos a cos b − sin a sin b and sin(a+b) = sin a cos b + cos a sin b, fall right out of the matrix product entry by entry. Let's verify numerically rather than symbolically. Take θ₁ = 30°, θ₂ = 20°. Rotating (1, 0) by 30° first: (cos30°, sin30°) = (0.8660, 0.5). Now rotate that result by 20°: x″ = 0.8660·cos20° − 0.5·sin20° = 0.8660·0.9397 − 0.5·0.3420 = 0.8138 − 0.1710 = 0.6428. y″ = 0.8660·0.3420 + 0.5·0.9397 = 0.2962 + 0.4699 = 0.7660. Now compare to rotating (1,0) directly by 50°: (cos50°, sin50°) = (0.6428, 0.7660). Identical, to four decimal places.
Drag both angle sliders. The blue arrow applies θ₁ then θ₂ as two steps; the orange arrow applies θ₁+θ₂ directly. In 2D they always land on top of each other.
python import math def rotate2d(x, y, theta_deg): t = math.radians(theta_deg) c, s = math.cos(t), math.sin(t) return (x*c - y*s, x*s + y*c) # two steps vs. one combined step p1 = rotate2d(*rotate2d(1, 0, 30), 20) p2 = rotate2d(1, 0, 50) print(p1, p2) # (0.6428, 0.7660) (0.6428, 0.7660) -- match
This isn't a coincidence that only shows up in one numeric example; it's baked into the algebra. Multiplying R(θ₂)·R(θ₁) symbolically and comparing term by term against R(θ₁+θ₂) is exactly the trigonometry identity cos(a+b) = cos·a·cos·b − sin·a·sin·b (and the matching one for sine), rearranged into matrix form. The identity you memorized in a trig class and the "rotations compose by adding angles" rule are the same fact, wearing two different outfits.
Extending Chapter 1's matrix to three dimensions is almost mechanical: pick one axis to leave untouched, and apply the 2D rotation formula to the other two coordinates. Rotating by γ around the z-axis leaves z alone and rotates x,y exactly like Chapter 1:
By the same logic, cycling which axis is "left alone" (x→y→z→x) gives rotation about the other two axes:
These three, Rx, Ry, Rz, are the elementary building blocks. Every 3D rotation this lesson discusses is either one of these directly or a product of several. Two properties are worth proving by hand right now, because they're the exact litmus test for "is this a valid rotation matrix" that Chapter 7 leans on hard.
Property 1: orthogonal, RTR = I. Take Rz(90°) = [[0,−1,0],[1,0,0],[0,0,1]]. Its transpose is Rz(90°)T = [[0,1,0],[−1,0,0],[0,0,1]]. Multiply them: row 1 of RT is [0,1,0], dotted with column 1 of R which is [0,1,0], gives 0·0+1·1+0·0 = 1. Row 1 dotted with column 2 [−1,0,0] gives 0·(−1)+1·0+0·0 = 0. Carry this out for all nine entries and you get the identity matrix exactly. Geometrically this says the matrix's three columns are mutually perpendicular unit vectors, a rotation matrix is nothing but "where do the three original axes end up," stacked as columns.
Property 2: determinant +1. For Rz(90°), det = 0·(0·1−0·0) − (−1)·(1·1−0·0) + 0·(…) = 0 + 1 + 0 = 1. A determinant of +1 says the transform preserves volume and handedness, no flipping. A mirror reflection is also orthogonal (RTR = I holds) but has determinant −1; that's what separates an honest rotation from a reflection. Recall the eigenvalues lesson's "matrix that only stretches", a rotation matrix is the opposite extreme: every eigenvalue has magnitude exactly 1 (it may be complex, encoding a rotation angle rather than a real stretch factor). A rotation is a matrix that stretches nothing, ever.
Compose Rx(90°) applied first, then Rz(90°), acting on v = (1, 0, 0). Step 1: Rx(90°)·(1,0,0). Since Rx only touches y,z and v has y=z=0, the result is unchanged: (1, 0, 0). Step 2: Rz(90°)·(1,0,0) = (0·1−1·0, 1·1+0·0, 0·1) = (0, 1, 0). Final answer: (0, 1, 0). Now reverse the order, Rz(90°) first, then Rx(90°). Step 1: Rz(90°)·(1,0,0) = (0,1,0) (same as before, x-axis rotates into y-axis). Step 2: Rx(90°)·(0,1,0) = (0, 0·1−1·0, 0·0+1·1) wait, carefully: Rx(90°)·(0,1,0) = (0, cos90°·1−sin90°·0, sin90°·1+cos90°·0) = (0, 0, 1). Final answer: (0, 0, 1). Two different final answers, (0,1,0) versus (0,0,1), from the identical pair of 90° rotations, the exact non-commutativity Chapter 0 demonstrated, now pinned down in coordinates you computed yourself.
Set the three angles, then compare applying them in X→Y→Z order versus Z→Y→X order (both meaning: rightmost-applied-first on a fixed world frame). Watch the readout confirm RTR stays the identity and det(R) stays 1, no matter the order.
python import numpy as np def Rz(g): c,s = np.cos(g), np.sin(g) return np.array([[c,-s,0],[s,c,0],[0,0,1]]) def Rx(a): c,s = np.cos(a), np.sin(a) return np.array([[1,0,0],[0,c,-s],[0,s,c]]) v = np.array([1.,0.,0.]) out1 = Rz(np.pi/2) @ (Rx(np.pi/2) @ v) # X first, then Z out2 = Rx(np.pi/2) @ (Rz(np.pi/2) @ v) # Z first, then X print(out1.round(3)) # [0. 1. 0.] print(out2.round(3)) # [0. 0. 1.] -- different!
One more check worth running, because it's the exact litmus test the next few chapters lean on: no matter which order you multiply Rx, Ry, Rz in, and no matter what angles you pick, the product is always orthogonal with determinant 1. Composition order changes which rotation you end up with, never whether the result is a valid rotation at all.
python # any product of Rx, Ry, Rz (any order, any angles) stays a valid rotation import numpy as np rng = np.random.default_rng(0) for _ in range(5): a,b,g = rng.uniform(0, 2*np.pi, 3) R = Rz(g) @ Rx(a) @ Rz(b) # any order at all orth_err = np.abs(R.T @ R - np.eye(3)).sum() print(round(orth_err, 6), round(np.linalg.det(R), 6)) # ~0.0 1.0 every time
| Symbol | Meaning |
|---|---|
| R | A rotation matrix (3×3), orthogonal with det = +1 |
| Rx(α), Ry(β), Rz(γ) | Elementary rotations about one fixed coordinate axis |
| RT | Transpose of R, equals R−1 exactly when R is a valid rotation |
| det(R) | +1 for a rotation, −1 for a reflection |
| θ | A rotation angle, always in radians inside the math, degrees in this lesson's prose |
Three rotation axes, three degrees of freedom to describe any orientation, roll around x, pitch around y, yaw around z. Stack them as R = Rz(yaw)·Ry(pitch)·Rx(roll), the aerospace "3-2-1" convention, and it feels like the obviously correct answer: three numbers for three degrees of freedom, done. Pilots, drone operators, and phone orientation sensors all report attitude this way because it's human-readable, "nose up 15°, banked right 10°" means something instantly, while a 3×3 matrix or a quaternion does not.
Now break it. Set pitch to exactly 90°. Ry(90°) = [[0,0,1],[0,1,0],[−1,0,0]]. Multiply out Rz(yaw)·Ry(90°)·Rx(roll) for two very different angle choices and watch what happens. Try roll = 0°, yaw = 0° first. Rx(0°) = I, so we need Rz(0°)·Ry(90°) = I·Ry(90°) = [[0,0,1],[0,1,0],[−1,0,0]]. Now try roll = 30°, yaw = −30°. Working through the full triple product (multiplying Rz(−30°), then Ry(90°), then Rx(30°), term by term) produces the exact same matrix: [[0,0,1],[0,1,0],[−1,0,0]]. Two completely different (roll, yaw) pairs, one identical final orientation.
That's not a coincidence for those two particular numbers, at pitch = 90°, every (roll, yaw) pair with roll − yaw held constant produces the same matrix. You fed the formula two independent numbers and it only listened to their difference. One entire degree of freedom silently vanished. This is gimbal lock.
Numerically, the trap resurfaces the moment you try to go the other way: given a rotation matrix, recover (roll, pitch, yaw). The standard formulas involve pitch = −arcsin(R[2][0]) and roll, yaw from arctan2 expressions divided by cos(pitch). At pitch = ±90°, cos(pitch) = 0 and those divisions blow up, the conversion has no unique answer, mirroring exactly what the forward direction showed. Software that naively runs this conversion near ±90° pitch produces noisy, jittery roll/yaw readings even when the true orientation is perfectly smooth, because it's dividing noise by a number near zero.
python import numpy as np def euler_to_R(roll, pitch, yaw): cr,sr,cp,sp,cy,sy = np.cos(roll),np.sin(roll),np.cos(pitch),np.sin(pitch),np.cos(yaw),np.sin(yaw) Rx = np.array([[1,0,0],[0,cr,-sr],[0,sr,cr]]) Ry = np.array([[cp,0,sp],[0,1,0],[-sp,0,cp]]) Rz = np.array([[cy,-sy,0],[sy,cy,0],[0,0,1]]) return Rz @ Ry @ Rx p90 = np.pi/2 R_a = euler_to_R(0, p90, 0) # roll=0, yaw=0 R_b = euler_to_R(np.radians(30), p90, np.radians(-30)) # roll=30, yaw=-30 print(np.allclose(R_a, R_b)) # True -- two "different" angle triples, IDENTICAL matrix # the reverse conversion blows up: dividing by cos(pitch) ~ 0 print(np.cos(p90)) # ~6.1e-17 -- effectively zero, any noise here explodes roll/yaw
Three nested rings: roll (inner), pitch (middle), yaw (outer). Drag pitch toward ±90° and watch the roll and yaw axes swing into alignment.
| Angle | Axis it spins about | Ring in the gimbal | Applied |
|---|---|---|---|
| Roll | x (forward) | Innermost | First |
| Pitch | y (sideways) | Middle | Second |
| Yaw | z (vertical) | Outermost | Third |
Euler angles broke because three separately-varying angles can conspire to alias each other. What if instead of three angles about three fixed axes, we describe a rotation as one turn, of angle θ, about one single axis n̂? This isn't a hack, it's a theorem, due to Euler: any 3D rotation is equivalent to some single rotation about some fixed axis. A tumbling, multi-step orientation change, no matter how it was built, is always reproducible as one spin around one line through the origin.
To turn that into a formula we need one new piece of machinery: the hat operator, which turns a 3-vector into a matrix that performs the cross product. For an axis k = (kx, ky, kz):
Check it does what's promised with k = (0,0,1), v = (1,1,0). Cross product directly: k×v = (0·0−1·1, 1·1−0·0, 0·1−0·1) = (−1, 1, 0). Now via the matrix: [k]× = [[0,−1,0],[1,0,0],[0,0,0]], and [k]×·v = (0·1−1·1, 1·1+0·1, 0) = (−1, 1, 0). Identical, the hat operator really is "cross product with k," repackaged as ordinary matrix multiplication.
Split any vector v into a piece parallel to the axis and a piece perpendicular to it: v = v∥ + v⊥, where v∥ = (k·v)k. Rotating about k leaves v∥ completely alone, it's sitting right on the axis. The perpendicular piece v⊥ lives in a 2D plane, and rotating it by θ in that plane is exactly Chapter 1's formula, dressed in 3D coordinates: v⊥,rot = cosθ·v⊥ + sinθ·(k×v⊥), because k×v⊥ is v⊥ rotated 90° within that same plane. Since k×v∥ = 0 (a vector crossed with something parallel to it vanishes), k×v⊥ equals k×v. Add the untouched parallel piece back in and simplify:
That's Rodrigues' rotation formula. Every term has a job: the first term is "mostly just keep v," the second term supplies the actual spin using the cross product, and the third term corrects for the parallel component not being fully captured by the first two terms alone.
Hand-check it on a 90° turn: rotate v = (1, 0, 0) by θ = 90° about k = (0, 0, 1). k×v = (0,0,1)×(1,0,0) = (0·0−1·0, 1·1−0·0, 0·0−0·1) = (0, 1, 0). k·v = 0. cos90°=0, sin90°=1. vrot = (1,0,0)·0 + (0,1,0)·1 + (0,0,1)·0·1 = (0, 1, 0). That's the x-axis rotated a quarter turn about z, landing exactly on the y-axis, matches intuition and matches Chapter 2's Rz(90°) result for the same input.
A second, less trivial check: rotate v = (1, 1, 0) by 90° about the same axis. k×v = (0,0,1)×(1,1,0) = (0·0−1·1, 1·1−0·0, 0·1−0·1) = (−1, 1, 0). k·v = 0. vrot = (1,1,0)·0 + (−1,1,0)·1 + 0 = (−1, 1, 0). The point (1,1,0) sits at 45° in the xy-plane; rotating 90° counter-clockwise about z should land at 135°, and (−1,1,0) is precisely that direction, at the same radius √2. Confirmed.
Now try a genuinely 3D case, where the axis isn't lined up with any coordinate direction, so nothing can be waved away as "obvious." Rotate v = (0, 0, 1) by 120° about the axis k = (1,1,1)÷√3 ≈ (0.577, 0.577, 0.577). First, k·v = 0.577·0 + 0.577·0 + 0.577·1 = 0.577. Next, k×v = (0.577·1−0.577·0, 0.577·0−0.577·1, 0.577·0−0.577·0) = (0.577, −0.577, 0). With θ=120°, cosθ=−0.5, sinθ=0.866. Assemble term by term: v·cosθ = (0,0,−0.5). (k×v)·sinθ = (0.577·0.866, −0.577·0.866, 0) = (0.5, −0.5, 0). k(k·v)(1−cosθ) = (0.577,0.577,0.577)·0.577·1.5 = (0.5, 0.5, 0.5). Summing all three: (0+0.5+0.5, 0−0.5+0.5, −0.5+0+0.5) = (1, 0, 0). Rotating straight-up by a third of a full turn about the (1,1,1) diagonal lands exactly on the x-axis, and by the same symmetry, another 120° would land on the y-axis, and a third 120° would return to z. That threefold symmetry, cycling x→y→z→x, is exactly what rotating about the cube's main diagonal should do, which is a good independent sanity check that the arithmetic is right.
python, plain, no libraries import math def rodrigues(k, theta, v): kx,ky,kz = k n = math.sqrt(kx*kx+ky*ky+kz*kz) or 1e-9 kx,ky,kz = kx/n, ky/n, kz/n c,s = math.cos(theta), math.sin(theta) kdotv = kx*v[0]+ky*v[1]+kz*v[2] kxv = (ky*v[2]-kz*v[1], kz*v[0]-kx*v[2], kx*v[1]-ky*v[0]) return tuple(v[i]*c + kxv[i]*s + [kx,ky,kz][i]*kdotv*(1-c) for i in range(3)) print(rodrigues((0,0,1), math.pi/2, (1,0,0))) # (~0, 1, 0)
numpy import numpy as np def rodrigues_np(k, theta, v): k = k / (np.linalg.norm(k) + 1e-9) c, s = np.cos(theta), np.sin(theta) return v*c + np.cross(k, v)*s + k*np.dot(k, v)*(1-c) print(rodrigues_np(np.array([0.,0.,1.]), np.pi/2, np.array([1.,0.,0.])).round(4)) # [0. 1. 0.]
pytorch import torch def rodrigues_torch(k, theta, v): k = k / (torch.linalg.norm(k) + 1e-9) c, s = torch.cos(theta), torch.sin(theta) return v*c + torch.linalg.cross(k, v)*s + k*torch.dot(k, v)*(1-c) k = torch.tensor([0.,0.,1.]); v = torch.tensor([1.,0.,0.]) print(rodrigues_torch(k, torch.tensor(torch.pi/2), v)) # tensor([0., 1., 0.])
Everything above rotated a single vector. The matrix form of the same idea, R = I + sinθ·[k]× + (1−cosθ)·[k]×2, builds the whole 3×3 rotation matrix at once, which is what Chapter 7's exp map will need:
python import numpy as np def skew(k): return np.array([[0,-k[2],k[1]],[k[2],0,-k[0]],[-k[1],k[0],0]]) def rodrigues_matrix(k, theta): k = k / np.linalg.norm(k) K = skew(k) return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K @ K) R = rodrigues_matrix(np.array([0.,0.,1.]), np.pi/2) print(R.round(3)) # matches Rz(90 deg) from Chapter 2 exactly print((R @ np.array([1.,0.,0.])).round(3)) # [0. 1. 0.]
The purple arrow is the fixed axis k̂. The orange arrow is v; drag θ and watch it sweep around the axis, tracing the cone Rodrigues' formula predicts.
Axis-angle fixed the lock, but it introduced a new problem: composing two axis-angle rotations has no clean formula. To combine "spin about k₁ by θ₁" with "spin about k₂ by θ₂," you'd have to convert both to matrices, multiply, and convert back, there's no direct axis-angle-to-axis-angle composition rule. We want a representation that (1) has no lock, (2) composes with a single clean multiplication like matrices do, and (3) interpolates smoothly between two orientations. That representation is the quaternion.
A quaternion is four numbers, q = (w, x, y, z), and a unit quaternion (w²+x²+y²+z² = 1) encodes a rotation via:
Notice the half-angle, not θ, but θ÷2. This is the single most surprising fact in the lesson, and it's not a typo. A unit quaternion and its negative, q and −q, represent the exact same rotation (both produce the identical rotated output below), so a full 360° physical turn only needs to move the quaternion halfway around its own space before it represents the same physical orientation again. This "double cover" is the same topological fact behind the belt trick performers use to show a 360° twist in a belt (or your arm) isn't actually undone until you go around twice, a genuinely strange but well-documented property of 3D rotations that quaternions expose directly in their arithmetic.
To rotate a vector v with a quaternion, embed it as a "pure" quaternion p = (0, vx, vy, vz) and compute:
Under the hood this expands to exactly Rodrigues' formula from Chapter 4, different notation, same geometric answer. What's new is the multiplication rule that makes composition trivial. Two quaternions multiply via the Hamilton product:
Let's grind one fully by hand. q1 = 90° about z: (cos45°, 0, 0, sin45°) = (0.7071, 0, 0, 0.7071). q2 = 90° about x: (cos45°, sin45°, 0, 0) = (0.7071, 0.7071, 0, 0). Compute q1q2 term by term. w: 0.7071·0.7071 − 0·0.7071 − 0·0 − 0.7071·0 = 0.5 − 0 − 0 − 0 = 0.5. x: 0.7071·0.7071 + 0·0.7071 + 0·0 − 0.7071·0 = 0.5. y: 0.7071·0 − 0·0 + 0·0.7071 + 0.7071·0.7071 = 0.5. z: 0.7071·0 + 0·0 − 0·0.7071 + 0.7071·0.7071 = 0.5. Result: (0.5, 0.5, 0.5, 0.5). Check it's still unit length: 0.5²×4 = 0.25×4 = 1. ✓
Let's confirm that q = (0.5, 0.5, 0.5, 0.5) really does rotate v = (1,0,0) to (0,0,1), by hand, using vrot = q·p·q−1 with p = (0, 1, 0, 0) (v embedded as a pure quaternion). First q·p, using the Hamilton product: w = 0.5·0−0.5·1−0.5·0−0.5·0 = −0.5. x = 0.5·1+0.5·0+0.5·0−0.5·0 = 0.5. y = 0.5·0−0.5·0+0.5·0+0.5·1 = 0.5. z = 0.5·0+0.5·0−0.5·1+0.5·0 = −0.5. So q·p = (−0.5, 0.5, 0.5, −0.5). Now multiply that by q−1 = the conjugate = (0.5, −0.5, −0.5, −0.5): w = (−0.5)(0.5) − (0.5)(−0.5) − (0.5)(−0.5) − (−0.5)(−0.5) = −0.25+0.25+0.25−0.25 = 0. x = (−0.5)(−0.5) + (0.5)(0.5) + (0.5)(−0.5) − (−0.5)(−0.5) = 0.25+0.25−0.25−0.25 = 0. y = (−0.5)(−0.5) − (0.5)(−0.5) + (0.5)(0.5) + (−0.5)(−0.5) = 0.25+0.25+0.25+0.25 = 1. z = (−0.5)(−0.5) + (0.5)(−0.5) − (0.5)(−0.5) + (−0.5)(0.5) = 0.25−0.25+0.25−0.25 = 0. Final result: (w,x,y,z) = (0, 0, 1, 0), a zero real part (as any correctly-rotated pure vector must have) and a vector part of exactly (0, 0, 1). Confirmed, term by term.
One practical wrinkle worth knowing before you ship this: floating-point arithmetic slowly drifts a unit quaternion's length away from exactly 1 after enough multiplications, the same way a rotation matrix's columns slowly drift away from orthogonal. The fix is cheap and the same in spirit as the matrix case, periodically renormalize, dividing the whole quaternion by its own length so w²+x²+y²+z² snaps back to 1. A real flight controller does this every single update cycle, as routine housekeeping, not as a fix for a bug.
Quaternions and rotation matrices describe the exact same rotations, so converting between them freely is routine, and worth seeing once so it never feels like a black box:
python import numpy as np def quat_to_mat3(q): w,x,y,z = q return np.array([ [1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)], [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)], [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]]) q = (0.5, 0.5, 0.5, 0.5) # our composed rotation from above print(quat_to_mat3(q).round(3)) # matches Rz(90) then Rx(90) from Chapter 2
Suppose you have a starting orientation q0 and an ending orientation q1, and you want the poses in between, a camera easing from one keyframe to the next, or a robot arm's commanded trajectory. The naive approach, nlerp (normalized linear interpolation), blends component-by-component and renormalizes: q(t) = normalize((1−t)q0 + t·q1). It's cheap, but it does not move at constant angular speed, unit quaternions live on the surface of a 4D sphere, and a straight-line chord between two points on a sphere cuts across the middle faster (in angle-per-t) than it moves near the endpoints.
Slerp (spherical linear interpolation) instead walks along the great-circle arc connecting q0 and q1 at constant angular velocity:
Two orientation frames mark the endpoints. Drag t and compare the slerp path (great-circle arc, constant angular speed) against the nlerp path (straight chord, speeds up mid-interpolation).
python, plain, no libraries def qmul(a, b): w1,x1,y1,z1 = a w2,x2,y2,z2 = b return ( w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 + x1*w2 + y1*z2 - z1*y2, w1*y2 - x1*z2 + y1*w2 + z1*x2, w1*z2 + x1*y2 - y1*x2 + z1*w2, ) q1 = (0.7071, 0, 0, 0.7071) # 90deg about z q2 = (0.7071, 0.7071, 0, 0) # 90deg about x print(qmul(q1, q2)) # (0.5, 0.5, 0.5, 0.5)
numpy import numpy as np def qmul_np(a, b): w1,x1,y1,z1 = a w2,x2,y2,z2 = b return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 + x1*w2 + y1*z2 - z1*y2, w1*y2 - x1*z2 + y1*w2 + z1*x2, w1*z2 + x1*y2 - y1*x2 + z1*w2]) q1 = np.array([0.7071,0,0,0.7071]); q2 = np.array([0.7071,0.7071,0,0]) print(qmul_np(q1, q2).round(3)) # [0.5 0.5 0.5 0.5]
pytorch import torch def qmul_torch(a, b): w1,x1,y1,z1 = a.unbind() w2,x2,y2,z2 = b.unbind() return torch.stack([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 + x1*w2 + y1*z2 - z1*y2, w1*y2 - x1*z2 + y1*w2 + z1*x2, w1*z2 + x1*y2 - y1*x2 + z1*w2]) q1 = torch.tensor([0.7071,0.,0.,0.7071]); q2 = torch.tensor([0.7071,0.7071,0.,0.]) print(qmul_torch(q1, q2)) # tensor([0.5, 0.5, 0.5, 0.5])
Everything so far rotates things about the origin. A real robot needs more: a world frame, a body frame offset and rotated from it, a camera bolted to the body at its own offset and rotation, an IMU chip bolted somewhere else again. We need one clean way to say "rotate, then also translate," and to chain several of those through a whole robot without the bookkeeping exploding.
The trick is homogeneous coordinates: pad every 3D point with an extra 1, turning (x,y,z) into (x,y,z,1). A rigid transform, rotation R plus translation t, becomes a single 4×4 matrix:
Why bother padding with a 1 at all? Because "rotate then add a constant offset" is an affine operation, not a linear one, and affine operations don't compose by simple matrix multiplication the way linear ones do. Padding the vector turns the affine operation into an honest linear operation on 4D vectors, and linear operations compose by matrix product. The set of all such 4×4 matrices is called SE(3), the special Euclidean group in 3D, "special" for the same det = +1 reason as SO(3), "Euclidean" because it includes translation.
Composition works exactly like the rotation-only case: chaining transform Tbc (base to camera) after Twb (world to base) is just the matrix product Twc = Twb·Tbc. Multiplying out the block structure gives a clean recipe: Rwc = Rwb·Rbc, and twc = Rwb·tbc + twb.
| Symbol | Meaning |
|---|---|
| T | A 4×4 homogeneous transform, packages a rotation and a translation together |
| Twb | "World from base": converts base-frame coordinates into world-frame coordinates |
| SE(3) | The set of all valid rigid transforms, every T with an orthogonal R block and det(R) = +1 |
| p̃ | A point padded with an extra 1, e.g. (x, y, z, 1), so T·p̃ is one matrix multiply |
World-to-base: rotate 90° about z, Rwb = Rz(90°) = [[0,−1,0],[1,0,0],[0,0,1]], translate twb = (1, 0, 0), the base sits 1 meter along world-x, rotated a quarter turn. Base-to-camera: no rotation, Rbc = I, just a mount offset tbc = (0, 0, 0.5), the camera is 0.5 m straight up from the base origin, in the base's own frame.
Compose: Rwc = Rwb·I = Rwb. twc = Rwb·(0,0,0.5) + (1,0,0). Compute Rwb·(0,0,0.5): row 1, [0,−1,0]·(0,0,0.5) = 0. Row 2, [1,0,0]·(0,0,0.5) = 0. Row 3, [0,0,1]·(0,0,0.5) = 0.5. So Rwb·tbc = (0, 0, 0.5), and twc = (0,0,0.5) + (1,0,0) = (1, 0, 0.5). The camera sits at world position (1, 0, 0.5), rotated 90° about z.
Now use that chain to place a point. A feature sits at pcam = (0.2, 0, 0) in the camera's own frame, 0.2 m straight ahead if the camera looks along its local x-axis. Where is it in world coordinates? pworld = Rwc·pcam + twc = Rz(90°)·(0.2,0,0) + (1,0,0.5). Rz(90°)·(0.2,0,0) = (0·0.2−1·0, 1·0.2+0·0, 0) = (0, 0.2, 0). pworld = (0,0.2,0) + (1,0,0.5) = (1, 0.2, 0.5). One point, converted through two chained frames, using nothing but the same matrix multiply from every earlier chapter.
python import numpy as np def se3(R, t): T = np.eye(4) T[:3,:3] = R T[:3, 3] = t return T def Rz(g): c,s = np.cos(g), np.sin(g) return np.array([[c,-s,0],[s,c,0],[0,0,1]]) T_wb = se3(Rz(np.pi/2), np.array([1.,0.,0.])) T_bc = se3(np.eye(3), np.array([0.,0.,0.5])) T_wc = T_wb @ T_bc # chain by plain matrix product print(T_wc[:3,3]) # [1. 0. 0.5] -- matches the hand calc p_cam = np.array([0.2, 0., 0., 1.]) # homogeneous: pad with a 1 p_world = T_wc @ p_cam print(p_world[:3]) # [1. 0.2 0.5] -- matches the hand calc
Adjust the base's yaw and the camera's mount pitch. Watch the fixed point (the dot) get carried through the chain, and read its coordinates in each frame below.
Every filter (EKF, UKF) and every optimizer (Gauss-Newton, gradient descent) needs to nudge a current orientation estimate by a small correction, every single iteration. The obvious move, just add a small matrix δ to R, giving Rnew = R + δ, is exactly the shortcut Chapter 0 warned about, and here is the concrete proof it fails.
Take R = Rz(60°) = [[0.5,−0.866,0],[0.866,0.5,0],[0,0,1]]. Add a "small" perturbation δ = [[0,−0.3,0],[0.3,0,0],[0,0,0]] elementwise: R+δ = [[0.5,−1.166,0],[1.166,0.5,0],[0,0,1]]. Check the first column's length: √(0.5²+1.166²) = √(0.25+1.36) = √1.61 ≈ 1.269, not 1. The columns of R+δ are no longer unit length, let alone mutually perpendicular. R+δ fails the orthogonality test from Chapter 2 outright; it is not a rotation matrix at all, and there is no angle it corresponds to.
| Operation | Result always a valid rotation? |
|---|---|
| R + δ (naive addition) | No |
| R · exp(ω̂) (multiply by the exponential map) | Yes, guaranteed |
The fix reuses Chapter 4's machinery, reinterpreted. Instead of adding δ directly to R, represent the small correction as a rotation vector ω = θk̂ (axis times angle, one compact 3-vector), map it through the exponential map from Chapter 4 to get a genuine small rotation matrix exp([ω]×), and compose by multiplication instead of addition:
This is guaranteed valid for a reason you already proved: the product of two orthogonal, determinant-+1 matrices is itself orthogonal with determinant +1. (Quick proof: if A and B are both orthogonal, (AB)T(AB) = BTATAB = BTIB = BTB = I, and det(AB) = det(A)det(B) = 1·1 = 1.) No matter how large or small ω is, Rold·exp([ω]×) can never leave the space of valid rotations. The naive R+δ had no such guarantee at all.
The reverse operation is the log map: given a rotation matrix R, recover the rotation vector ω that generated it. The angle comes from the trace: θ = arccos((trace(R)−1)÷2). Check it on Rz(30°): trace = cos30°+cos30°+1 = 0.866+0.866+1 = 2.732. θ = arccos((2.732−1)÷2) = arccos(0.866) = 30°. Exactly right.
The axis comes from the skew-symmetric part of R: (R−RT)÷(2sinθ), the same hat-operator machinery from Chapter 4, run backwards. For Rz(30°) = [[0.866,−0.5,0],[0.5,0.866,0],[0,0,1]], its transpose is [[0.866,0.5,0],[−0.5,0.866,0],[0,0,1]]. Subtracting gives R−RT = [[0,−1,0],[1,0,0],[0,0,0]]. Divide by 2sin30° = 2·0.5 = 1, leaving the matrix unchanged: [[0,−1,0],[1,0,0],[0,0,0]]. Match this against the hat-operator pattern [[0,−kz,ky],[kz,0,−kx],[−ky,kx,0]] entry by entry: −kz=−1 gives kz=1, and every other entry gives kx=ky=0. The recovered axis is k = (0, 0, 1), exactly the z-axis, exactly what you'd expect from a matrix that was built as a rotation about z in the first place.
python import numpy as np def skew(k): return np.array([[0,-k[2],k[1]],[k[2],0,-k[0]],[-k[1],k[0],0]]) def expmap(omega): theta = np.linalg.norm(omega) if theta < 1e-8: return np.eye(3) k = omega / theta K = skew(k) return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K@K) R0 = Rz(np.radians(60)) omega = np.radians(10) * np.array([1.,0.,0.3]) / np.linalg.norm([1.,0.,0.3]) R_naive = R0 + skew(omega) # WRONG: naive addition R_proper = R0 @ expmap(omega) # RIGHT: multiply by the exponential map print(np.linalg.norm(R_naive[:,0])) # != 1.0 -- not a valid rotation print(np.linalg.norm(R_proper[:,0])) # 1.0 -- always, by construction
| Term | What it is |
|---|---|
| SO(3) | The group of all valid 3D rotation matrices, orthogonal, determinant +1 |
| so(3) | The tangent space at the identity: all 3×3 skew-symmetric matrices, one per rotation vector ω |
| exp( ) | Maps a tangent vector ω to a genuine rotation, Rodrigues' formula, Chapter 4 |
| log( ) | The inverse: recovers ω from a rotation matrix, via the trace and the skew-symmetric part |
| ⊕ | The safe update rule: R ⊕ ω = R·exp(ω̂), always stays on SO(3) |
Increase the perturbation magnitude and watch each column's length. The naive R+δ drifts off length 1 immediately; R·exp(δ̂) stays exactly on the unit circle, always.
Here is the payoff for the whole lesson. Below is a 3-link chain, World → Base → Camera → IMU, exactly the structure behind Chapter 6's scenario, now fully interactive. Drag the three joint sliders to pose the chain. Pick which frame a target point is fixed in. Switch the orientation readout between Euler angles, axis-angle, and quaternion, live, on the very frames you're posing, and watch every representation from this lesson update from the same underlying rotation matrices.
Three joints, three frames beyond World. The pink dot is a target point, fixed in whichever frame you select below. Its coordinates are recomputed in every frame, live, by chaining 4×4 transforms exactly like Chapter 6.
| If the point is fixed in… | Its local coordinates are |
|---|---|
| World | (1.9, 0.3, 0.5) |
| Base | (0.5, 0, 0.2) |
| Camera | (0.3, 0, 0) |
| IMU | (0.1, 0, 0) |
Why this specific chain, and not a simpler one? Because it's the smallest structure that forces every idea in this lesson to cooperate at once. Three joints means three genuine chances for order to matter (Chapter 2). A camera mounted with its own pitch means the chain can be steered straight into gimbal lock (Chapter 3) on command. A point expressible in any of four frames exercises the full compose-and-invert machinery of Chapter 6. And switching the readout between three representations of the very same underlying rotation matrix is Chapters 4, 5, and 3 made simultaneously visible, on the same numbers, at the same instant, which is exactly the comparison a table or a single static example could never show as convincingly as watching it happen live.
Push the camera pitch slider toward ±90° while the readout is set to Euler. Watch the base or IMU angles do something ugly, jumping, or refusing to settle, exactly the divide-by-cos(pitch)-near-zero breakdown from Chapter 3. Now switch the readout to quaternion and sweep through the same range: the four numbers change smoothly, continuously, with no special angle anywhere. Same underlying orientation, same physical pose of the chain, and one representation panics while the other doesn't even notice. That contrast, seen live on a chain you built yourself, is the entire argument of this lesson compressed into one demo.
Every number in the readout below comes from exactly the code you've already seen in earlier chapters, just run three times in a row and fed into three different conversion routines:
python, the whole lab, end to end import numpy as np T_wb = se3(Rz(yaw), np.array([1.4,0.,0.])) T_bc = se3(Ry(pitch), np.array([0.,0.,0.7])) T_ci = se3(Rx(roll), np.array([0.3,0.,0.15])) T_wc = T_wb @ T_bc # chain, exactly like chapter 6 T_wi = T_wc @ T_ci p_local = {'imu': np.array([0.1,0.,0.,1.])}[frame] p_world = T_wi @ p_local # or T_wb / T_wc for other frames # the SAME rotation, three ways -- pick your readout euler = mat3_to_euler(T_wi[:3,:3]) # glitches near pitch = 90 quat = mat3_to_quat(T_wi[:3,:3]) # never glitches axang = mat3_to_axis_angle(T_wi[:3,:3]) # never glitches
Four representations, four honest trade-offs, no single winner. Here's the map.
| Representation | Numbers | Locks? | Composes via | Interpolates smoothly? | Reach for it when… |
|---|---|---|---|---|---|
| Euler angles (roll-pitch-yaw) | 3 | Yes, at ±90° pitch | Three matrix products | No, visibly wrong near lock | Human-readable logs, UI sliders, a single known-safe orientation |
| Axis-angle (rotation vector) | 3 | No hard lock (singular only at θ=0) | Awkward, convert first | Poor directly | A single well-defined turn, or the small ⊕ correction in a filter/optimizer |
| Unit quaternion | 4 (norm = 1) | None | One Hamilton product | Yes, via slerp | Real-time attitude estimation, animation, anything composing thousands of rotations per second |
| Rotation matrix (3×3) | 9 (orthogonal) | None | Matrix product | Poor directly | Rotating many points at once, math and proofs, output straight from a solver |
| SE(3) (rotation + translation) | R’s count + 3 | Inherits R’s | Homogeneous 4×4 product | Inherits R’s (translation itself is easy) | Chaining frames, world→base→camera→IMU, robot kinematics, camera extrinsics |
Notice the pattern underneath every row: the representation with the fewest numbers (3, for Euler and axis-angle) is the one that's cheapest to store and read but has a singularity somewhere. The representation with the awkward extra number (quaternion's 4, constrained to unit length) is the one built specifically to avoid a singularity anywhere. There is no free representation with 3 clean numbers and no trap, a fact that traces back to the topology of the rotation group itself, not to a missed engineering opportunity.
This lesson built the vocabulary; three lessons put it to work immediately. The Extended Kalman Filter tracks an orientation state that lives on SO(3), and its update step is literally the R ⊕ ω retraction from Chapter 7, without it, the filter's covariance update would slowly corrupt R into an invalid matrix. Classical SLAM estimates a robot's full SE(3) pose over time, and every loop closure is a rigid-transform composition exactly like Chapter 6's chain. Classical VIO fuses camera and IMU measurements using precisely the camera-IMU extrinsic SE(3) transform from Chapter 6's scenario card, get that calibration wrong and the whole estimator drifts. And recall the very first connection this lesson drew: a rotation matrix's eigenvalues always have magnitude exactly 1, the matrix that stretches nothing, only turns.
Two more threads worth pulling. The ⊕ retraction from Chapter 7 is a special case of the general "linearize on a tangent space, then map back" strategy taught in Jacobians & Linearization, there, the tangent space is the derivative of an arbitrary nonlinear function; here, it's specifically the tangent space of the rotation manifold. And every camera you'll meet again in a computer-vision lesson needs exactly the extrinsics [R | t] built in Chapter 6 to place its lens in the world, a rotation matrix and a translation vector, packaged as one SE(3) transform, is precisely what turns a floating lens into a located camera.
One closing thought to carry forward. Every representation this lesson covered is answering the exact same physical question, which way is this thing facing, and every trap you met (order, lock, the half-angle, naive addition) came from mismatching a representation's assumptions against how it was actually used. There is no universally correct choice, only a choice that fits how the numbers will be read, combined, and updated. That's not a compromise; it's the whole discipline.