Mechanization

Lecture from the RPCN course. Motivation: How does a stabilizer work?

Mechanization is the process of converting the output of an IMU into position, velocity and attitude information. The outputs include rotation rates about three body axes ${\omega }_{ib}^b$ measured by the gyroscopes triad and three specific forces $f^b$ along the body axes measured by the accelerometer triad, all of which are with respect to the inertial frame. Mechanization is a recursive process that starts with a specified set of initial values and iterates on the output.

Properties of angular velocities:

• We usually write ${\omega }_{mk}^p$ which means the angular velocity of frame m, relative to frame k, expressed in frame p coordinates.
• For example, if we have a robotic arm with the fixed base-frame (k), the moving end-effector (m), and we want to express it in p frame coordinates: The angular velocity describes how fast m is rotating relative to k, but you can write those rotation rates using any coordinate system p.
• ${\omega }_{mk}^p={\omega }_{mh}^p+{\omega }_{hk}^p$ (chain rule)
  • m rotates relative to h
  • h rotates relative to k
  • Therefore, m rotates relative to k
  • All expressed in p-frame coordinates

I covered in Coordinate Systems that $f^b=a^b-g^b$ is the specific force of the IMU measured in body frame and that ${\Omega }_{ib}^b=\left(\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right)$ is the skew-symmetric matrix which represents the cross product as a matrix multiplication.

INS Mechanization in an Inertial Frame of Reference

In the figure above we can see the mechanization of an INS(Inertial Navigation System) in the inertial frame. We want to integrate $p$, $v$, and $\theta$ from the IMU measured accelerations $a$ and angular velocities $\omega$.

We know that

$$f^i=a^i-{\overline{\mathbf{g}}}^i$$

By letting $a^i={\ddot{r}}^i$, this can be rewritten as:

$${\ddot{r}}^i=f^i+{\overline{\mathbf{g}}}^i$$

And for ease of solution, the set of three second-order differential equations can be transformed to a set of first-order differential equations as follows:

$${\dot{r}}^i={\mathbf{v}}^i$$ $${\dot{\mathbf{v}}}^i=f^i+{\overline{\mathbf{g}}}^i$$

The measurements are usually made in the body frame, w.r.t. inertial frame. We can use the rotation matrix $R_b^i$ to extract

$$f^i=R_b^i{f}^b$$

Since the gravitational vector is usually expressed in either the e-frame or the l-frame, it can be transformed to the i-frame through a rotation matrix $R_e^i$ or $R_l^i$. We’ll consider e-frame

$${\overline{\mathbf{g}}}^i=R_e^i{\overline{\mathbf{g}}}^e$$

Substituting in the first-order differential equations, we get:

$${\dot{\mathbf{v}}}^i=R_b^i{f}^b+R_e^i{\overline{\mathbf{g}}}^e$$

As discussed in Rotation Matrix Time Derivative, the rate of change of a transformation matrix is

$${\dot{R}}_b^i=R_b^i{\Omega }_{ib}^b$$

The mechanization equations for the i-frame can therefore be summarized as:

$$\left[\begin{array}{c}{\dot{r}}^i\\ {\dot{v}}^i\\ {\dot{R}}_b^i\end{array}\right]=\left[\begin{array}{c}{v}^i\\ R_b^i{f}^b+R_e^i{\overline{\mathbf{g}}}^e\\ R_b^i{\Omega }_{ib}^b\end{array}\right]$$

These all use the measurement from the IMU.

---

INS Mechanization in ECEF Frame

A note on the difference in which frame the angular velocity is expressed in

• Related to Rotation Matrix Time Derivative
• If $\omega$ is expressed in the inertial/fixed frame: $\dot{R}=\Omega \cdot R$
• If $\omega$ is expressed in the rotating/body frame: $\dot{R}=R\cdot \Omega$

Mathematical Proof (From the book):

A position vector in the $r^e$ in the e-frame can be transformed into the i-frame $r^i$ by using the Rotation Matrix as follows:

$$r^i=R_e^i{r}^e$$

After differentiating twice and rearranging the terms (see here) we get:

$${\ddot{\mathbf{r}}}^i=R_e^i\left({\ddot{\mathbf{r}}}^e+2{\Omega }_{ie}^e{\dot{\mathbf{r}}}^e+{\dot{\Omega }}_{ie}^e{\mathbf{r}}^e+{\Omega }_{ie}^e{\Omega }_{ie}^e{\mathbf{r}}^e\right)$$

But we know that ${\ddot{r}}^i=a^i$, therefore ${\ddot{r}}^i=f^i+g^i$, so:

$$R_e^i\left({\ddot{\mathbf{r}}}^e+2{\Omega }_{ie}^e{\dot{\mathbf{r}}}^e+{\dot{\Omega }}_{ie}^e{\mathbf{r}}^e+{\Omega }_{ie}^e{\Omega }_{ie}^e{\mathbf{r}}^e\right)={\mathbf{f}}^i+{\mathbf{g}}^i$$ $$R_e^i\left({\ddot{\mathbf{r}}}^e+2{\Omega }_{ie}^e{\dot{\mathbf{r}}}^e+{\dot{\Omega }}_{ie}^e{\mathbf{r}}^e+{\Omega }_{ie}^e{\Omega }_{ie}^e{\mathbf{r}}^e\right)=R_b^i{\mathbf{f}}^b+R_e^i{\overline{\mathbf{g}}}^e$$

This can be simplified by letting $R_b^i=R_e^i{R}_b^e$ and ${\dot{\Omega }}_{ie}^e{r}^e=0$, because the Earth’s rotation rate ${\omega }_{ie}$ is approximately constant.

$$R_e^i\left({\ddot{\mathbf{r}}}^e+2{\Omega }_{ie}^e{\dot{\mathbf{r}}}^e+{\Omega }_{ie}^e{\Omega }_{ie}^e{\mathbf{r}}^e\right)=R_e^i{R}_b^e{\mathbf{f}}^b+R_e^i{\overline{\mathbf{g}}}^e$$ $${\ddot{\mathbf{r}}}^e=R_b^e{\mathbf{f}}^b-2{\Omega }_{ie}^e{\dot{\mathbf{r}}}^e+{\overline{\mathbf{g}}}^e-{\Omega }_{ie}^e{\Omega }_{ie}^e{\mathbf{r}}^e$$

and because the gravity vector is defined as ${\mathbf{g}}^e={\overline{\mathbf{g}}}^e-{\Omega }_{ie}^e{\Omega }_{ie}^e{\mathbf{r}}^e$ (true gravitational acceleration - centrifugal acceleration due to Earth’s rotation), this can be further reduced to:

$${\ddot{\mathbf{r}}}^e=R_b^e{\mathbf{f}}^b-2{\Omega }_{ie}^e{\dot{\mathbf{r}}}^e+{\mathbf{g}}^e$$

which is equivalent to

$${\dot{r}}^e=v^e,{\dot{v}}^e=R_b^e{\mathbf{f}}^b-2{\Omega }_{ie}^e{v}^e+{\mathbf{g}}^e$$

q.e.d.

The rate of change of the rotation matrix $R_b^e$ can be given as

$${\dot{R}}_b^e=R_b^e{\Omega }_{eb}^b$$

To extract ${\Omega }_{eb}^b$, we can do the following chain rule:

$${\Omega }_{ib}^b={\Omega }_{ie}^b+{\Omega }_{eb}^b$$ $${\Omega }_{eb}^b=-{\Omega }_{ie}^b+{\Omega }_{ib}^b$$

Substituting, we get:

$${\dot{R}}_b^e=R_b^e\left(\begin{array}{c}-{\Omega }_{ie}^b+{\Omega }_{ib}^b\end{array}\right)$$

So the e-frame mechanization equations can be summarized as:

$$\left[\begin{array}{c}{\dot{\mathbf{r}}}^e\\ {\dot{\mathbf{v}}}^e\\ {\dot{R}}_b^e\end{array}\right]=\left[\begin{array}{c}{\mathbf{v}}^e\\ R_b^e{\mathbf{f}}^b-2{\Omega }_{ie}^e{\mathbf{v}}^e+{\mathbf{g}}^e\\ R_b^e({\Omega }_{ie}^b+{\Omega }_{ib}^b)\end{array}\right]$$

The term $-2{\Omega }_{ie}^e$ in the velocity derivative comes from the rotation of the Earth.

---

INS Mechanization in Local-Level (Navigation) Frame

In many applications the mechanization equations are desired in the local frame for the following reasons:

• The navigation equations in the l-frame furnish a navigation solution that is intuitive to the user on or near the Earth’s surface.
• Since the axes of the l-frame are aligned to the local east, north and up directions, the attitude angles (pitch, roll and azimuth) can be obtained directly at the output of the mechanization equations when solved in the local-level frame.
• The computational errors in the navigation parameters on the horizontal (E-N) plane are bound by the Schuler effect

The position vector $r^l$ of a moving platform expressed in the geodetic (curvilinear) coordinates in the ECEF frames as:

$$r^l={\left[\begin{array}{c}\phi ,\lambda ,h\end{array}\right]}^T$$

The rate of change of its position is expressed in terms of the velocity in the east, north and up directions. The rate of change of the platform’s latitude, longitude and altitude are

$$\dot{\phi }=\frac{v_n}{R_M+h}$$ $$\dot{\lambda }=\frac{v_e}{(R_N+h)\cos \phi }$$ $$\dot{h}=v_u$$

• $v_e$ is the component of the velocity in the east direction
• $v_n$ is the component of the velocity in the north direction
• $v_u$ is the component of the velocity in the up direction
• $R_M$ is the meridian radius of the ellipsoid
• $R_N$ is the normal radius of the ellipsoid

Thus, we can write

$$\left[\begin{array}{c}\dot{\phi }\\ \dot{\lambda }\\ \dot{h}\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{1}{(R_N+h)\cos \phi }& 0& 0\\ 0& 0& 1\end{array}\right]{\left[\begin{array}{c}{v}_e\\ v_n\\ v_u\end{array}\right]}^l$$ $${\dot{\mathbf{r}}}^l=D^{-1}{\mathbf{v}}^l$$

in which $D^{-1}$ transforms the velocity vector from rectangular coordinates into curvilinear coordinates in the ECEF frame.

Calculation of the velocity derivative:

The Earth-referenced velocity vector ${\dot{r}}^e$ can be transformed into the local-level frame by using the rotation matrix $R_e^l$

$$v^l=R_e^l{\dot{r}}^e$$

where $v^l={\left[\begin{array}{c}{v}_e,v_n,v_u\end{array}\right]}^T$. The time derivative is therefore:

$${\dot{\mathbf{v}}}^l={\dot{R}}_e^l{\dot{\mathbf{r}}}^e+R_e^l{\ddot{\mathbf{r}}}^e$$

We substitute ${\dot{R}}_e^l$ for $R_e^l{\Omega }_{le}^e$, where ${\Omega }_{le}^e$ is the skew-symmetric matrix corresponding to ${\omega }_{el}^l$

$${\dot{\mathbf{v}}}^l=R_e^l\left({\Omega }_{le}^e{\dot{\mathbf{r}}}^e+{\ddot{\mathbf{r}}}^e\right)$$

and since ${\Omega }_{le}^e=-{\Omega }_{el}^e$ and ${\dot{r}}^e=v^e$:

$${\dot{\mathbf{v}}}^l=R_e^l\left({\ddot{\mathbf{r}}}^e-{\Omega }_{el}^e{\mathbf{v}}^e\right)$$

We can transform the position vector r from the ECEF frame into the inertial frame by $r^i=R_e^i{r}^e$. Taking the time derivative and using the relationship ${\dot{R}}_e^i=R_e^i{\Omega }_{ie}^e$ gives:

$${\dot{\mathbf{r}}}^i={\dot{R}}_e^i{\mathbf{r}}^e+R_e^i{\dot{\mathbf{r}}}^e=R_e^i{\Omega }_{ie}^e{\mathbf{r}}^e+R_e^i{\dot{\mathbf{r}}}^e=R_e^i\left({\Omega }_{ie}^e{\mathbf{r}}^e+{\dot{\mathbf{r}}}^e\right)$$

where ${\Omega }_{ie}^e$ is the skew-symmetric matrix corresponding to ${\omega }_{ie}^e$.

After a lot of mathematics and simplifications, we get to the final local-frame mechanization equations:

$$\left[\begin{array}{c}{\dot{\mathbf{r}}}^l\\ {\dot{\mathbf{v}}}^l\\ {\dot{R}}_b^l\end{array}\right]=\left[\begin{array}{c}{D}^{-1}{\mathbf{v}}^l\\ R_b^l{\mathbf{a}}^b-(2{\Omega }_{el}^l+{\Omega }_{ie}^l){\mathbf{v}}^l+{\mathbf{g}}^l\\ R_b^l({\Omega }_{ib}^b-{\Omega }_{il}^b)\end{array}\right]$$

with

$${\omega }_{ie}^l=\left[\begin{array}{c}0\\ {\omega }^e\cos \phi \\ {\omega }^e\sin \phi \end{array}\right]\to {\Omega }_{ie}^l=\left[\begin{array}{ccc}0& -{\omega }^e\sin \phi & {\omega }^e\cos \phi \\ {\omega }^e\sin \phi & 0& 0\\ -{\omega }^e\cos \phi & 0& 0\end{array}\right]$$ $${\omega }_{el}^l=\left[\begin{array}{c}-\frac{v_n}{R_M+h}\\ \frac{v_e}{R_N+h}\\ \frac{v_e\tan \phi }{R_N+h}\end{array}\right]\to {\Omega }_{el}^l=\left[\begin{array}{ccc}0& -\frac{v_e\tan \phi }{R_N+h}& \frac{v_e}{R_N+h}\\ \frac{v_e\tan \phi }{R_N+h}& 0& \frac{v_n}{R_M+h}\\ -\frac{v_e}{R_N+h}& -\frac{v_n}{R_M+h}& 0\end{array}\right]$$

Dead Reckoning: the process of calculating one's current position by using a previously determined position, or fix, by using estimations of speed and course over elapsed time

This sounds like drift errors.

Mechanization (Attitude)

• Attitude is independent of position and velocity.
• Position and velocity depend on attitude.
• IMU output: angular rate of body frame wr.t. inertial frame ${\omega }_{ib}^b$
• Desired output: angular increment of body frame w.r.t. local-level frame ${\theta }_{lb}^b$
• It’s a first order differential equation
  • ${\dot{\theta }}_{lb}^b=\frac{d{\theta }_{lb}^b}{dt}$
  • ${\theta }_{lb}^b={\omega }_{lb}^b△t$

Mathematical Foundation:

${\omega }_{ib}^b={\omega }_{il}^b+{\omega }_{lb}^b$

$${\omega }_{lb}^b={\omega }_{li}^b+{\omega }_{ib}^b={\omega }_{ib}^b-{\omega }_{il}^b$$ $${\omega }_{il}^b=R_l^b{\omega }_{il}^l$$ $${\omega }_{il}^l={\omega }_{ie}^l+{\omega }_{el}^l$$

where ${\omega }_{ie}^l$ is the Rotation of the Earth and ${\omega }_{el}^l$ is the ellipsoid model for the geographical coordinate system.

We know that ${\omega }_{ie}^l=R_e^l{\omega }_{ie}^e=\left[\begin{array}{ccc}-\sin \lambda & \cos \lambda & 0\\ -\sin \varphi \cos \lambda & -\sin \varphi \sin \lambda & \cos \varphi \\ \cos \varphi \cos \lambda & \cos \varphi \sin \lambda & \sin \varphi \end{array}\right]\left[\begin{array}{c}0\\ 0\\ {\omega }_e\end{array}\right]=\left[\begin{array}{c}0\\ {\omega }_e\cos \varphi \\ {\omega }_e\sin \varphi \end{array}\right]$ and that ${\omega }_{el}^l=\left[\begin{array}{c}-\dot{\varphi }\\ \dot{\lambda }\cos \varphi \\ \dot{\lambda }\sin \varphi \end{array}\right]=\left[\begin{array}{c}-\frac{v_n}{M+h}\\ \frac{v_e}{N+h}\\ \frac{v_e\tan \varphi }{N+h}\end{array}\right]$ where N and M are the radius of curvature of the prime vertical and meridian ($R_M,R_N$).

In the end, we can extract the desired output ${\theta }_{lb}^b$ based on ${\omega }_{ib}^b$ (IMU’s output w.r.t. the inertial frame — measured) and $({\omega }_{ie}^l+{\omega }_{el}^l)$ (Earth’s rotation w.r.t. the inertial frame — known)

$${\theta }_{lb}^b=\left({\omega }_{ib}^b-R_l^b({\omega }_{ie}^l+{\omega }_{el}^l)\right)\Delta t$$

Mechanization of Velocity in Navigation Frame

The change of velocity in local-level frame where $v_k^l=[v_e,v_n,v_u{]}^T$ and ${\mathbf{g}}^l=[0,0,-\mathbf{g}{]}^T$:

$$\Delta v_{k+1}^l=R_b^l{\mathbf{a}}^b\Delta t-(2{\Omega }_{ie}^l+{\Omega }_{el}^l)v_k^l\Delta t+{\mathbf{g}}^l\Delta t$$

Mechanization (Position)

$${\phi }_{k+1}={\phi }_k+\frac{1}{2}\frac{v_{n,k}+v_{n,k+1}}{N+h}\Delta t$$ $${\lambda }_{k+1}={\lambda }_k+\frac{1}{2}\frac{v_{e,k}+v_{e,k+1}}{(N+h)\cos \phi }\Delta t$$ $$h_{k+1}=h_k+\frac{1}{2}(v_{u,k}+v_{u,k+1})\Delta t$$

What happens at the North pole?

• The equation diverges as $\cos \phi$ goes to zero! That’s why we create a Wander Frame where the z-vector is pointing elsewhere than at the North pole.

Data Smoothing

• INS rely on accelerometer and gyroscope data for estimating position, velocity, and orientation
• Data Smoothing reduces noise and thus improves the reliability of navigation solutions

Sliding Window Averaging:

1. Applies a moving average over recent measurements
2. Reduces short-term fluctuations while preserving trends
3. Velocity is calculated for example by using modified Euler formula ${\mathbf{v}}_{k+1}^l={\mathbf{v}}_k^l+\frac{1}{2}(\Delta {\mathbf{v}}_k^l+\Delta {\mathbf{v}}_{k+1}^l)$

Kalman Filtering:

1. Optimal estimation combining sensor data and system dynamics
2. Correct for drift and noise by incorporating external measurements like GPS

Complementary Filtering:

1. Combined high-freq data (gyroscopes) and low-freq data (accelerometers)
2. Simple and computationally efficient
3. Can be used e.g. for gyro bias estimation

What if one directly uses the IMU measurements to integrate attitude? That is, by assuming that Earth's rotation is negligible with the following assumption:

$${\theta }_{lb}^b=({\omega }_{ib}^b-R_l^b({\omega }_{ie}^l+{\omega }_{el}^l)){\Delta }_t$$ $${\theta }_{lb}^b={\omega }_{lb}^b{\Delta }_t\sim {\omega }_{ib}^b{\Delta }_t$$

Answer: There will be an accumulating drift on the angle! The magnitude of this drift follows from Earth’s rotation. When integration time is less than 1s, the effect could be negligible but if integration is continued even with these small steps, the error accumulates with respect to time

Per short: That exact amount we consider 0 ($R_l^b({\omega }_{ie}^l+{\omega }_{el}^l)$) will be the error accumulating in our data.

Introduction to Rodrigues’ Formula

Lie Algebra

Covered in Lie Algebra.

As a general idea, we represent a spatial rotation SO(3) by an axis-angle pair. This way, we represent every rotation as a single operation instead of 3 in the case of Euler Angles.

$$\theta =\varphi u,||u||=1,\varphi \in R$$

where u is the rotation axis and $\varphi$ is the rotation magnitude.

The associated element of the lie algebra $so(3)$ is the skew-symmetric matrix denoted as $[\theta {]}_{\mathbf{x}}$

$$[\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 exponential map provides the correspondence

$$R=exp([\theta {]}_{\mathbf{x}})\in SO(3)$$

linking the axis-angle (lie algebra) representation with a rotation matrix

Closed form exponential of a skew-symmetric matrix

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 ||$$

This expression is known as Rodrigues’ Rotation Formula.