Pen and Paper Exercises Coordinate Systems

1. ATTITUDE FROM THE GRAVITY VECTOR

Context:

Consider that there is an IMU mounted on the claw.

Attitude from the gravity vector: Approximating the attitude of the claw by using the gravity vector. We know which way is up, and which is down.

• Consider a drone that is flying and experiencing rotations. The drone has a claw that hangs from a cord and swings with 3 rotational DoF.
• The drone has an IMU and the palm of the claw is equipped with an IMU as well.
• Question: What is the attitude of the claw, relative to the drone?
  • The attitude can be estimated by using the gravity vector
  • Calculate a relative position, assume that the string is non-elastic (3 rotational DOF, of which yaw and pitch are the relevant ones, if we assume the gripper is symmetric w.r.t. Roll angle)

Solution:

So we have the following setup:

• A claw hangs from a drone on a cord with 3 rotational degrees of freedom
• IMUs on both the drone and claw measure acceleration
• The claw’s attitude (orientation) relative to the drone needs to be determined

So we know the gripper is symmetric w.r.t. Roll angle and the string is non-elastic. This makes it much easier as we only need the drone’s IMU.

What we need to do is calculate the angle between the IMU’s z-axis and the gravity vector using the accelerometer readings.

$$cos(\varphi )=\frac{\text{z-axis}\cdot \text{gravity}}{|\text{z-axis}|\times |\text{gravity}|}$$

• The z-axis is simply $[0,0,1]$, so the dot product is just $a_z$.
• The magnitude of gravity is $\sqrt{a_x^2+a_y^2+a_z^2}$

In the end, the angle between the IMU’s z-axis and the gravity vector is:

$$\varphi =arccos\left(\begin{array}{c}\frac{a_z}{\sqrt{a_x^2+a_y^2+a_z^2}}\end{array}\right)$$

Can we measure the angular velocities of the claw?

Yes, but only if the claw IMU is on the center of rotation.

Why: An IMU measures angular velocity using its gyroscope. However, if the IMU is offset from the center of rotation, the accelerometer will also pick up centripetal acceleration from the rotation, which results in big drift. When the IMU is at the center of rotation, there’s no linear acceleration due to rotation — only pure angular velocity is measured by the gyroscope, giving you clean angular velocity data.

---

2. Where to mount the IMU on an autonomous car?

A) On the roof - Used for sensor fusion with GNSS (GPS)

B) Rear wheel axis - Benefits:

• No acceleration along the rear wheel axis (assuming no wheel slip)
• Can omit one accelerometer axis bias
• Simplifies to a point-like robot model for yaw angle
• Angular velocity comes from road inclination in the velocity direction

C) Center of mass - Sounds intuitive but offers no advantages

D) Anywhere - Funnily enough, this actually works

The idea is that the placement doesn’t affect rotation measurements. Angular velocity $\omega$ and forces $f$ transform the same regardless of location:

• ${\omega }^B=R_S^B{\omega }^S$
• $f^B=R_S^B{f}^S$

However, the acceleration at the body frame origin does depend on IMU position:

• $a_{OB}^B=f^B+g^B+{\alpha }^B\times r^B-{\omega }^B\times ({\omega }^B\times r^B)$
• This depends on the IMU position vector $r^B$ and angular acceleration ${\alpha }^B$.

---

3. IMU-to-Body Frame Transformations

Consider an autonomous car driving on a road. We define a car body frame B

• Origin OB: center of the rear-wheel axis.
• x-axis: forward along the car's longitudinal axis
• y-axis: to the left of the car
• z-axis: upwards, completing a right-handed frame

An IMU is mounted somewhere inside the car, at an arbitrary orientation and position.

Part 1: Single IMU on the Car

Context: The IMU has its own sensor frame S. The rigid-body relationship between S and the car body frame B is known from calibration:

• Rotation from IMU frame to body frame: $R_S^B\in SO(3)$ (rotates vectors from S to B)
• Position of the IMU origin in the body frame: $r^B\in R^3$ (vector from OB to the IMU)

At time $t_k$, the IMU measures the angular rate and specific force, expressed in S:

• ${\omega }^S(t_k)\in R^3$
• $f^S(t_k)\in R^3$

(a) Transforming to the car body frame

Question: Write the expressions for ${\omega }^B(t_k)$ and $f^B(t_k)$ in the car body frame B, in terms of $R_S^B$ and ${\omega }^S$.

Solution:

Since we know the IMU measures ${\omega }^S(t_k)$ and $f^S(t_k)$ each $t_k$, then we covered earlier that we can go from the IMU sensor frame S to the car’s body frame B by simply applying the rotation matrix $R_S^B$

Thus, the solution is:

• ${\omega }^B(t_k)=R_S^B{\omega }^S(t_k)$
• $f^B(t_k)=R_S^B{f}^S(t_k)$

---

Question: Explain in one or two sentences why it is important for vehicle state estimation (e.g. Kalman Filter) to express these quantities in the car body frame B rather than in the IMU frame S.

My response: In vehicle state estimation we usually talk about kinematics — and the kinematics we want to model is of the car’s. Since the car is our reference frame in this case, it makes perfect sense to transform everything into car body frame B and it further helps with sensor fusioning.

From professor: Using IMU-frame quantities directly would introduce frame-dependent biases and incorrect coupling between states.

---

(b) From specific force at the IMU to acceleration at the rear-axle

Context: Let the body have body-frame angular velocity ${\omega }^B(t_k)$, angular acceleration ${\alpha }^B(t_k)$, and linear acceleration of the rear axle origin $a_{OB}^B(t_k)$, all expressed in B.

Let the car’s orientation in the world be $R_B^W(t_k)$ and let gravity in the world frame be

• $g^W=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$
• $g^B$ = ${R_B^W}^T{g}^W$

The rigid-body relation for the acceleration at the IMU origin (expressed in B) is:

$$a_{IMU}^B=a_{OB}^B+{\alpha }^B\times r^B+{\omega }^B\times ({\omega }^B\times r^B)$$

Moreover, the IMU accelerometer measures specific force:

• $f^S=R_B^S(a_{IMU}^B-g^B)$
• $R_B^S={R_S^B}^T$

Question: Show that the specific force in the body frame is $f^B=a_{IMU}^B-g^B$

Solution:

We know that

$$f^S=R_B^S(a_{IMU}^B-g^B)$$

So, naturally:

$$R_S^B{f}^S=R_S^B{R}_B^S(a_{IMU}^B-g^B)$$

And we know that Rotation Matrices are orthogonal, so $R{R}^T=I$. And since $R_B^S={R_S^B}^T$:

$$f^B=a_{IMU}^B-g^B,q.e.d.$$

---

Question: Use the rigid-body relation to derive an expression for the acceleration of the rear-axle center in the body frame (you may assume ${\omega }^B$ and ${\alpha }^B$ are known from other sensors or from numerical differentiation of the gyroscope data):

$$a_{OB}^B=f^B+g^B-{\alpha }^B\times r^B-{\omega }^B\times ({\omega }^B\times r^B)$$

Solution:

We know that $f^B=a^B-g^B$, and therefore, $a^B=f^B+g^B$ and the follow:

$$a_{IMU}^B=a_{OB}^B+{\alpha }^B\times r^B+{\omega }^B\times ({\omega }^B\times r^B)$$

So, in conclusion:

$$a_{OB}^B=f^B+g^B-{\alpha }^B\times r^B-{\omega }^B\times ({\omega }^B\times r^B)$$

---

Part 2: Two arbitrarily aligned IMUs on the same car

Context: Now suppose there are two IMUs, with frames S${}_1$ and S${}_2$, each with known $R_{B{I}_i}$ and $r_i^B$. For each IMU $i\in \{1,2\}$, the relations from Part 1 hold with the corresponding index.

Question: Write the body-frame specific force for each IMU:

$$f_i^B=a_{OB}^B-g^B+{\alpha }^B\times r_i^B+{\omega }^B\times ({\omega }^B\times r_i^B)$$

Subtract the two equations to extract

$$\Delta f^B=f_2^B-f_1^B={\alpha }^B\times (r_2^B-r_1^B)+{\omega }^B\times ({\omega }^B\times (r_2^B-r_1^B))$$

Assume a planar car motion so that

$${\omega }^B=\left[\begin{array}{c}0\\ 0\\ {\omega }_z\end{array}\right],{\alpha }^B=\left[\begin{array}{c}0\\ 0\\ {\alpha }_z\end{array}\right]$$

and define $d^B=r_2^B-r_1^B=[d_x,d_y,d_z{]}^T$. Compute ${\alpha }^B\times d^B$ and ${\omega }^B\times ({\omega }^B\times d^B)$, and show that the x and y components of $\Delta f^B$ yield two scalar equations involving ${\alpha }_z$ and ${\omega }_z^2$

Solution:

I need to use the cross product. Here I covered that to compute the cross product between two vectors v and w, we do

$$\mathbf{v}\times \mathbf{w}=[\mathbf{v}{]}_{\mathbf{x}}\mathbf{w}=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right]\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]$$

And so, we calculate

$${\alpha }^B\times d^B=\left[\begin{array}{ccc}0& -{\alpha }_z& 0\\ {\alpha }_z& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{d}_x\\ d_y\\ d_z\end{array}\right]=\left[\begin{array}{c}-{\alpha }_z{d}_y\\ -{\alpha }_z{d}_x\\ 0\end{array}\right]$$ $$({\omega }^B{)}^2\times d^B=\left[\begin{array}{ccc}-{\omega }_z^2& 0& 0\\ 0& -{\omega }_z^2& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{d}_x\\ d_y\\ d_z\end{array}\right]=\left[\begin{array}{c}-{\omega }_z^2{d}_x\\ -{\omega }_z^2{d}_y\\ 0\end{array}\right]$$ $$\Delta f^B=\left[\begin{array}{c}-{\alpha }_z{d}_y\\ -{\alpha }_z{d}_x\\ 0\end{array}\right]+\left[\begin{array}{c}-{\omega }_z^2{d}_x\\ -{\omega }_z^2{d}_y\\ 0\end{array}\right]=\left[\begin{array}{c}-{\alpha }_z{d}_y-{\omega }_z^2{d}_x\\ -{\alpha }_z{d}_x-{\omega }_z^2{d}_y\\ 0\end{array}\right]$$

Therefore, the x and y components are two scalar eq. involving ${\alpha }_z$ and ${\omega }_z^2$.

---

Part 3: Attitude Update Using Rodrigues’ Formula

Context: Rodrigues’ formula provides a closed-form, minimal-parameter, numerically stable way to update orientation from an angular velocity vector without using quaternions or integrating differential equations. Let ${\hat{\omega }}^B$ be an estimate of the body-frame angular velocity, obtained e.g. from the IMUs.

We define

• The single-axis pair rotation $\theta ={\hat{\omega }}^B\Delta t$
• The rotation magnitude $\varphi =||\theta ||$
• The skew-symmetric matrix $[\theta {]}_{\mathbf{x}}=\left(\begin{array}{ccc}0& -{\theta }_z& {\theta }_y\\ {\theta }_z& 0& -{\theta }_x\\ -{\theta }_y& {\theta }_x& 0\end{array}\right)$

The exact attitude is

$$R_B^W(t_{k+1})=R_B^W(t_k)exp([\theta {]}_{\mathbf{x}})$$

---

Question: Using Rodrigues’ Rotation Formula, show that

$$\exp ([\theta {]}_{\times })=I+\frac{\sin \varphi }{\varphi }[\theta {]}_{\times }+\frac{1-\cos \varphi }{{\varphi }^2}[\theta {]}_{\times }^2$$

Solution: To show this, we need to use lie algebra to go from SO(3) to $so(3)$. I covered this whole explanation in Mechanization.

For any matrix $A$, the exponential is defined by

$$exp(A)=\frac{A^n}{n!}=I+A+\frac{A^2}{2!}+...$$

When $A=[\theta {]}_{\mathbf{x}}\in so(3)$, its powers satisfy

• $[\theta {]}_{\mathbf{x}}^3=-||\theta |{|}^2[\theta {]}_{\mathbf{x}}$
• $[\theta {]}_{\mathbf{x}}^4=-||\theta |{|}^2[\theta {]}_{\mathbf{x}}^2$

Starting from

$$exp([\theta {]}_{\mathbf{x}})=\frac{[\theta {]}_{\mathbf{x}}^n}{n!}=I+[\theta {]}_{\mathbf{x}}+\frac{[\theta {]}_{\mathbf{x}}^2}{2!}+...$$

use

• $[\theta {]}_{\mathbf{x}}^3=-{\varphi }^2[\theta {]}_{\mathbf{x}}$
• $[\theta {]}_{\mathbf{x}}^4=-{\varphi }^2[\theta {]}_{\mathbf{x}}^2$
• $\varphi =||\theta ||$

Odd powers are proportional to $[\theta {]}_{\times }$:

$$[\theta {]}_{\times }-\frac{{\varphi }^2}{3!}[\theta {]}_{\times }+\frac{{\varphi }^4}{5!}[\theta {]}_{\times }-\cdots =\frac{\sin \varphi }{\varphi }[\theta {]}_{\times }$$

Even powers are proportional to $[\theta {]}_{\times }^2$:

$$\frac{[\theta {]}_{\times }^2}{2!}-\frac{{\varphi }^2}{4!}[\theta {]}_{\times }^2+\frac{{\varphi }^4}{6!}[\theta {]}_{\times }^2-\cdots =\frac{1-\cos \varphi }{{\varphi }^2}[\theta {]}_{\times }^2$$

Using these identities, the series can be summed in closed form:

$$\exp ([\theta {]}_{\times })=I+\frac{\sin \varphi }{\varphi }[\theta {]}_{\times }+\frac{1-\cos \varphi }{{\varphi }^2}[\theta {]}_{\times }^2,\varphi =||\theta ||$$

q.e.d.

---

Question: Using the Taylor expansions for $\sin \varphi$ and $\cos \varphi$, derive the second-order small-angle approximation

$$exp([\theta {]}_{\times })\sim I+[\theta {]}_{\times }+\frac{1}{2}[\theta {]}_{\times }^2$$

Solution:

The second-order Taylor expansions for $\sin$ and $\cos$ are:

• $\sin \varphi =\varphi -\frac{{\varphi }^3}{3!}+...\sim \varphi$ AND after we neglect the higher-order terms we remain with $\varphi$
• $\cos \varphi =1-\frac{{\varphi }^2}{2!}+...$ AND after we neglect the higher-order terms we remain with $1-\frac{{\varphi }^2}{2!}$

Which translates into:

• $\frac{\sin \varphi }{\varphi }=\frac{\varphi }{\varphi }=1$
• $\frac{1-\cos \varphi }{{\varphi }^2}=\frac{1-1+\frac{{\varphi }^2}{2!}}{{\varphi }^2}=\frac{1}{2}$

Therefore, we derive the second-order small-angle approximation:

$$\exp ([\theta {]}_{\times })=I+\frac{\sin \varphi }{\varphi }[\theta {]}_{\times }+\frac{1-\cos \varphi }{{\varphi }^2}[\theta {]}_{\times }^2\sim I+[\theta {]}_{\times }+\frac{1}{2}[\theta {]}_{\times }^2$$

q.e.d.

---

Question: Let $R_B^W(t_k)=I$, ${\hat{\omega }}^B=\left[\begin{array}{c}0\\ 0\\ 2\pi \end{array}\right]$ rad/s, $\Delta t=0.01$ s.

Compute(numerically):

• $\theta$,
• $[\theta {]}_{\times }$ and $[\theta {]}_{\times }^2$
• $\Delta R\sim I+[\theta {]}_{\times }+\frac{1}{2}[\theta {]}_{\times }^2$
• $R_B^W(t_{k+1})=\Delta R$

Solution:

• $\theta ={\hat{\omega }}^B\Delta t=\left[\begin{array}{c}0\\ 0\\ 2\pi \cdot 0.01\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0.06283\end{array}\right]$
• $[\theta {]}_{\mathbf{x}}=\left(\begin{array}{ccc}0& -{\theta }_z& {\theta }_y\\ {\theta }_z& 0& -{\theta }_x\\ -{\theta }_y& {\theta }_x& 0\end{array}\right)=[\theta {]}_{\mathbf{x}}=\left(\begin{array}{ccc}0& -0.06283& 0\\ 0.06283& 0& 0\\ 0& 0& 0\end{array}\right)$
• $[\theta {]}_{\times }^2=\left(\begin{array}{ccc}0& -0.06283& 0\\ 0.06283& 0& 0\\ 0& 0& 0\end{array}\right)\left(\begin{array}{ccc}0& -0.06283& 0\\ 0.06283& 0& 0\\ 0& 0& 0\end{array}\right)=\left(\begin{array}{ccc}-0.00395& 0& 0\\ 0& -0.00395& 0\\ 0& 0& 0\end{array}\right)$
• $\Delta R=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)+\left(\begin{array}{ccc}0& -0.06283& 0\\ 0.06283& 0& 0\\ 0& 0& 0\end{array}\right)+\left(\begin{array}{ccc}-0.001975& 0& 0\\ 0& -0.001975& 0\\ 0& 0& 0\end{array}\right)=\left(\begin{array}{ccc}0.998025& -0.06283& 0\\ 0.06283& 0.998025& 0\\ 0& 0& 1\end{array}\right)$
• $R_B^W(t_{k+1})=\Delta R$. We can, of course, approximate $0.9980\sim 1$ and so we end up with a rotation around the z-axis of about $0.06283[rad]\sim 3.6[\mathrm{deg}]$ which is a very small angle and the second-order approximation makes sense in this case.