An IMU provides angular rate (gyroscope) and specific force (accelerometer). For dead reckoning we integrate: \( \boldsymbol{\omega}(t) \) to update orientation, rotate measured specific force into a navigation frame, add gravity, then integrate acceleration to obtain \( \mathbf{v}(t) \) and \( \mathbf{p}(t) \). For ground robots, we usually enforce a planar motion hypothesis (flat ground), which can dramatically reduce spurious vertical drift.

flowchart TD
  A["IMU samples (gyro, accel)"] --> B["Calibrate: scale, bias removal"]
  B --> C["Attitude propagation (quaternion)"]
  C --> D["Rotate specific force to nav frame"]
  D --> E["Add gravity to get acceleration"]
  E --> F["Integrate: velocity then position"]
  F --> G["Ground constraints: z=0, roll=pitch approx 0"]
  G --> H["Optional yaw correction using wheel heading"]
  H --> I["Dead-reckoned pose output (x,y,yaw)"]
        

Throughout, we work deterministically: biases may be treated as constants (or slowly varying), but we do not yet introduce stochastic state estimation (Chapter 6 onward).

Let \( \{B\} \) be the IMU/body frame and \( \{N\} \) a navigation frame (for ground robots often ENU with z up). The dead-reckoning state (in 3D form) is: \( \mathbf{p}(t)\in\mathbb{R}^3 \), \( \mathbf{v}(t)\in\mathbb{R}^3 \), and attitude \( \mathbf{R}_{NB}(t)\in SO(3) \) (maps body vectors to navigation vectors). We also use a unit quaternion \( \mathbf{q}_{NB}(t) \) for numerical robustness.

The kinematic relations are:

\[ \dot{\mathbf{p}}(t) = \mathbf{v}(t), \qquad \dot{\mathbf{v}}(t) = \mathbf{a}(t), \qquad \dot{\mathbf{R}}_{NB}(t) = \mathbf{R}_{NB}(t)\,[\boldsymbol{\omega}(t)]_{\times}. \]

Here \( [\boldsymbol{\omega}]_{\times} \) is the skew-symmetric matrix such that \( [\boldsymbol{\omega}]_{\times}\mathbf{x} = \boldsymbol{\omega}\times\mathbf{x} \).

Proof sketch (why \( \dot{\mathbf{R}}_{NB} = \mathbf{R}_{NB}[\boldsymbol{\omega}]_{\times} \)):

Let \( \mathbf{b}_i(t) \) be body-frame basis vectors expressed in \( \{N\} \). Stacking them as columns gives \( \mathbf{R}_{NB}(t)=[\mathbf{b}_1\,\mathbf{b}_2\,\mathbf{b}_3] \). Rigid-body kinematics yields \( \dot{\mathbf{b}}_i = \boldsymbol{\omega}\times\mathbf{b}_i \), hence column-wise \( \dot{\mathbf{R}}_{NB} = [\boldsymbol{\omega}\times\mathbf{b}_1\,\,\boldsymbol{\omega}\times\mathbf{b}_2\,\,\boldsymbol{\omega}\times\mathbf{b}_3] \), which equals \( \mathbf{R}_{NB}[\boldsymbol{\omega}]_{\times} \).

The key practical point is that the exact solution for rotation over a small interval uses an exponential map, not naive component-wise integration.

The gyroscope measures angular velocity corrupted by bias and noise: \( \boldsymbol{\omega}_m(t) \). The accelerometer measures specific force, not raw acceleration: \( \mathbf{f}_m(t) \).

\[ \boldsymbol{\omega}_m(t) = \boldsymbol{\omega}(t) + \mathbf{b}_g(t) + \mathbf{n}_g(t), \qquad \mathbf{f}_m(t) = \mathbf{f}(t) + \mathbf{b}_a(t) + \mathbf{n}_a(t). \]

The specific force relates to navigation-frame acceleration by:

\[ \mathbf{f}(t) = \mathbf{R}_{NB}(t)^{\top}\big(\mathbf{a}(t) - \mathbf{g}\big), \qquad \mathbf{a}(t) = \mathbf{R}_{NB}(t)\,\mathbf{f}(t) + \mathbf{g}. \]

Proof (specific force identity):

Newton’s law is \( m\mathbf{a} = \mathbf{F}_{\text{non-grav}} + m\mathbf{g} \). An accelerometer is effectively a proof-mass device measuring \( \mathbf{F}_{\text{non-grav}}/m = \mathbf{a} - \mathbf{g} \), expressed in the sensor frame. Thus \( \mathbf{f} = \mathbf{R}_{NB}^{\top}(\mathbf{a}-\mathbf{g}) \), yielding the inversion above.

For ground robots we often assume \( \mathbf{b}_g(t) \) and \( \mathbf{b}_a(t) \) are approximately constant over a short run (seconds to minutes), which is sufficient to derive drift laws. A fuller bias random-walk discussion belongs to Lesson 3.

Let samples arrive at times \( t_k \) with step \( \Delta t_k = t_{k+1}-t_k \). Define bias-corrected measurements: \( \tilde{\boldsymbol{\omega}}_k = \boldsymbol{\omega}_{m,k} - \mathbf{b}_{g,k} \) and \( \tilde{\mathbf{f}}_k = \mathbf{f}_{m,k} - \mathbf{b}_{a,k} \).

4.1 Exact attitude update via exponential map

\[ \mathbf{R}_{NB,k+1} = \mathbf{R}_{NB,k}\exp\!\big([\tilde{\boldsymbol{\omega}}_k]_{\times}\Delta t_k\big). \]

Proof (closed-form solution of the rotation ODE):

With constant \( \tilde{\boldsymbol{\omega}}_k \) over \( [t_k,t_{k+1}] \), the ODE \( \dot{\mathbf{R}}=\mathbf{R}\mathbf{\Omega} \) with \( \mathbf{\Omega}=[\tilde{\boldsymbol{\omega}}_k]_{\times} \) has solution \( \mathbf{R}(t)=\mathbf{R}(t_k)\exp(\mathbf{\Omega}(t-t_k)) \) by standard linear matrix ODE theory, giving the update. Since \( \mathbf{\Omega}^{\top}=-\mathbf{\Omega} \), \( \exp(\mathbf{\Omega}\Delta t)\in SO(3) \), so orthonormality is preserved (up to numerical rounding).

In quaternion form, letting \( \Delta\boldsymbol{\theta}_k = \tilde{\boldsymbol{\omega}}_k\Delta t_k \), define the incremental quaternion:

\[ \delta\mathbf{q}_k = \begin{bmatrix} \cos\big(\tfrac{1}{2}\|\Delta\boldsymbol{\theta}_k\|\big) \\ \mathbf{u}_k \sin\big(\tfrac{1}{2}\|\Delta\boldsymbol{\theta}_k\|\big) \end{bmatrix}, \quad \mathbf{u}_k = \begin{cases} \Delta\boldsymbol{\theta}_k / \|\Delta\boldsymbol{\theta}_k\|, & \|\Delta\boldsymbol{\theta}_k\| \neq 0 \\ \mathbf{0}, & \|\Delta\boldsymbol{\theta}_k\| = 0 \end{cases} \]

Then \( \mathbf{q}_{NB,k+1} = \mathbf{q}_{NB,k}\otimes \delta\mathbf{q}_k \), followed by normalization.

4.2 Velocity and position updates

\[ \mathbf{a}_k = \mathbf{R}_{NB,k}\tilde{\mathbf{f}}_k + \mathbf{g}, \qquad \mathbf{v}_{k+1} = \mathbf{v}_k + \mathbf{a}_k \Delta t_k, \qquad \mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t_k + \tfrac{1}{2}\mathbf{a}_k \Delta t_k^2. \]

The position update above is second-order accurate if \( \mathbf{a}(t) \) is approximately constant within the step. More accurate schemes (midpoint, trapezoidal) reduce discretization error, but cannot eliminate bias-driven drift.

A wheeled ground robot on flat terrain is often modeled as planar: \( z \approx 0 \), \( v_z \approx 0 \), and roll/pitch remain small. Since IMUs are 3D sensors, we must map 3D inertial propagation to a 2D navigation output \( (x,y,\psi) \).

5.1 Roll/pitch from accelerometer under quasi-static assumption

If translational acceleration is negligible compared to gravity, then \( \tilde{\mathbf{f}} \approx \mathbf{R}_{NB}^{\top}(-\mathbf{g}) \), so the accelerometer direction indicates gravity in the body frame. Using a ZYX convention, one common approximation is:

\[ \hat{\phi} = \operatorname{atan2}\!\big(\tilde{f}_y,\tilde{f}_z\big), \qquad \hat{\theta} = \operatorname{atan2}\!\big(-\tilde{f}_x,\sqrt{\tilde{f}_y^2+\tilde{f}_z^2}\big). \]

For ground robots, a practical alternative is to project the quaternion to yaw-only (set roll/pitch to zero), acknowledging that we are enforcing a kinematic constraint rather than estimating full attitude.

5.2 Complementary yaw correction using wheel odometry heading

From Lesson 1, wheel odometry provides a heading estimate \( \psi_{\text{wheel}} \). IMU yaw from integrated gyro, \( \psi_{\text{imu}} \), is locally smooth but drifts with gyro bias. A deterministic complementary correction is:

\[ \dot{\hat{\psi}}(t) = \omega_{z,m}(t) - \hat{b}_{g,z}(t) + \frac{1}{\tau}\big(\psi_{\text{wheel}}(t) - \hat{\psi}(t)\big). \]

Discretizing with step \( \Delta t \) yields the convenient “exponential smoothing” form:

\[ \hat{\psi}_{k} \leftarrow \operatorname{wrap}\Big(\alpha\,\psi_{\text{imu},k} + (1-\alpha)\,\psi_{\text{wheel},k}\Big), \qquad \alpha = \exp\!\big(-\Delta t/\tau\big). \]

This does not require probabilistic modeling: it is a stable linear correction law with a tunable time constant \( \tau \). It is also easy to implement in embedded code.

The central limitation of pure IMU dead reckoning is that biases integrate into growing errors. The following derivations assume constant biases to make the growth rates explicit.

6.1 Accelerometer bias causes quadratic position drift

Consider 1D motion with measured acceleration \( a_m(t)=a(t)+b_a \) and we integrate assuming bias is zero. The velocity error satisfies: \( \dot{e}_v(t)=b_a \) with \( e_v(0)=0 \), hence:

\[ e_v(t) = b_a t, \qquad e_p(t) = \int_0^t e_v(s)\,ds = \tfrac{1}{2}b_a t^2. \]

Thus, even a small constant bias (e.g., \( 0.05\,\text{m/s}^2 \)) produces meter-level drift over tens of seconds.

6.2 Gyro yaw bias causes quadratic cross-track drift

Suppose a robot drives at constant forward speed \( v \) along the x-axis, while yaw is estimated with bias \( b_{g,z} \). Then yaw error is: \( e_{\psi}(t)=b_{g,z}t \). The estimated forward direction is rotated by \( e_{\psi} \), producing lateral velocity error approximately \( v\sin(e_{\psi}(t)) \). For small angles, \( \sin(e_{\psi}) \approx e_{\psi} \), so:

\[ e_y(t) \approx \int_0^t v\,e_{\psi}(s)\,ds = v\int_0^t b_{g,z}s\,ds = \tfrac{1}{2} v b_{g,z} t^2. \]

This is why a small yaw-rate bias (e.g., \( 0.01\,\text{rad/s} \)) can create large lateral drift unless corrected (e.g., by wheel heading or other absolute references).

A robust integration loop for a ground robot typically includes: (i) timestamp validation, (ii) bias subtraction, (iii) exact rotation update (quaternion exponential), (iv) gravity handling, (v) planar constraints, and (vi) optional yaw correction.

flowchart TD
  S0["Start step k"] --> S1["Read dt, gyro_m, accel_m"]
  S1 --> S2["omega = gyro_m - bg"]
  S2 --> S3["q = normalize(q * expq(0.5*omega*dt))"]
  S3 --> S4["(optional) project q to yaw-only"]
  S4 --> S5["f = accel_m - ba"]
  S5 --> S6["a_nav = R(q)*f + g"]
  S6 --> S7["v = v + a_nav*dt"]
  S7 --> S8["p = p + v*dt + 0.5*a_nav*dt^2"]
  S8 --> S9["(optional) z=0, vz=0"]
  S9 --> S10["(optional) yaw = alpha*yaw_imu + (1-alpha)*yaw_wheel"]
  S10 --> S11["Output (x,y,yaw)"]
        

Numerical notes: always normalize the quaternion; avoid assuming constant \( \Delta t \); and ensure units are consistent (rad/s, m/s²). If you later observe significant high-rate rotations, coning effects may matter, but many ground robots operate in a regime where the midpoint/exponential update suffices.

The following script implements quaternion strapdown integration, planar constraints, and complementary yaw fusion with wheel odometry. It also simulates a constant-speed turn to demonstrate drift.

File: Chapter5_Lesson2.py

"""
Chapter 5 — Odometry and Dead Reckoning
Lesson 2: IMU Integration for Ground Robots

Filename: Chapter5_Lesson2.py
"""

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import Tuple, Optional
import numpy as np

def q_normalize(q: np.ndarray) -> np.ndarray:
    n = np.linalg.norm(q)
    if n <= 0.0:
        return np.array([1.0, 0.0, 0.0, 0.0], dtype=float)
    return q / n

def q_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    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
    ], dtype=float)

def q_from_delta_theta(dtheta: np.ndarray) -> np.ndarray:
    angle = float(np.linalg.norm(dtheta))
    if angle < 1e-12:
        half = 0.5
        return q_normalize(np.array([1.0, half*dtheta[0], half*dtheta[1], half*dtheta[2]], dtype=float))
    axis = dtheta / angle
    half = 0.5 * angle
    s = math.sin(half)
    return np.array([math.cos(half), axis[0]*s, axis[1]*s, axis[2]*s], dtype=float)

def q_to_R(q: np.ndarray) -> np.ndarray:
    w, x, y, z = q
    return np.array([
        [1 - 2*(y*y + z*z),     2*(x*y - w*z),     2*(x*z + w*y)],
        [    2*(x*y + w*z), 1 - 2*(x*x + z*z),     2*(y*z - w*x)],
        [    2*(x*z - w*y),     2*(y*z + w*x), 1 - 2*(x*x + y*y)]
    ], dtype=float)

def yaw_from_q(q: np.ndarray) -> float:
    w, x, y, z = q
    return math.atan2(2.0*(w*z + x*y), 1.0 - 2.0*(y*y + z*z))

def q_from_yaw(yaw: float) -> np.ndarray:
    half = 0.5 * yaw
    return np.array([math.cos(half), 0.0, 0.0, math.sin(half)], dtype=float)

def wrap_pi(a: float) -> float:
    a = (a + math.pi) % (2*math.pi) - math.pi
    return a

@dataclass
class IMUParams:
    g: float = 9.80665
    b_g: np.ndarray = np.zeros(3)
    b_a: np.ndarray = np.zeros(3)
    enforce_planar: bool = True
    fuse_yaw_with_wheel: bool = True
    tau_yaw: float = 2.0

def strapdown_integrate(
    t: np.ndarray,
    gyro: np.ndarray,
    accel: np.ndarray,
    params: IMUParams,
    wheel_yaw: Optional[np.ndarray] = None,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    N = t.shape[0]
    p = np.zeros(3, dtype=float)
    v = np.zeros(3, dtype=float)
    q = np.array([1.0, 0.0, 0.0, 0.0], dtype=float)
    p_hist = np.zeros((N,3), dtype=float)
    v_hist = np.zeros((N,3), dtype=float)
    yaw_hist = np.zeros(N, dtype=float)
    g_nav = np.array([0.0, 0.0, -params.g], dtype=float)

    for k in range(N):
        dt = float(t[1] - t[0]) if k == 0 else float(t[k] - t[k-1])

        omega = gyro[k] - params.b_g
        dq = q_from_delta_theta(omega * dt)
        q = q_normalize(q_mul(q, dq))

        if params.enforce_planar:
            q = q_from_yaw(yaw_from_q(q))

        f_b = accel[k] - params.b_a
        a_nav = q_to_R(q) @ f_b + g_nav

        v = v + a_nav * dt
        p = p + v * dt + 0.5 * a_nav * (dt**2)

        if params.enforce_planar:
            p[2] = 0.0
            v[2] = 0.0

        yaw = yaw_from_q(q)
        if params.fuse_yaw_with_wheel and wheel_yaw is not None:
            alpha = math.exp(-dt / max(params.tau_yaw, 1e-6))
            yaw = wrap_pi(alpha * yaw + (1.0 - alpha) * float(wheel_yaw[k]))
            q = q_from_yaw(yaw)

        p_hist[k] = p
        v_hist[k] = v
        yaw_hist[k] = yaw

    return p_hist, v_hist, yaw_hist

def simulate_ground_robot(N: int = 2000, dt: float = 0.01) -> None:
    t = np.arange(N) * dt
    v0 = 1.2
    w0 = 0.20
    yaw_true = w0 * t
    x_true = (v0 / w0) * np.sin(yaw_true)
    y_true = (v0 / w0) * (1.0 - np.cos(yaw_true))

    ax_nav = v0 * w0 * np.cos(yaw_true)
    ay_nav = v0 * w0 * np.sin(yaw_true)
    a_nav = np.stack([ax_nav, ay_nav, np.zeros_like(t)], axis=1)

    def Rz(yaw: np.ndarray) -> np.ndarray:
        c = np.cos(yaw); s = np.sin(yaw)
        R = np.zeros((yaw.size, 3, 3))
        R[:,0,0] = c;  R[:,0,1] = -s
        R[:,1,0] = s;  R[:,1,1] =  c
        R[:,2,2] = 1.0
        return R

    R_nb = Rz(yaw_true)
    g_nav = np.array([0.0, 0.0, -9.80665])
    f_b = np.einsum('nij,nj->ni', np.transpose(R_nb, (0,2,1)), (a_nav - g_nav))

    gyro_true = np.zeros((N,3))
    gyro_true[:,2] = w0

    rng = np.random.default_rng(1)
    b_g = np.array([0.0, 0.0, 0.01])
    b_a = np.array([0.05, -0.02, 0.00])

    gyro_meas = gyro_true + b_g + rng.normal(0.0, 0.002, size=(N,3))
    accel_meas = f_b + b_a + rng.normal(0.0, 0.05, size=(N,3))

    wheel_yaw = yaw_true + rng.normal(0.0, 0.01, size=(N,))
    wheel_yaw = wheel_yaw + 0.002 * t

    p2, v2, yaw2 = strapdown_integrate(
        t, gyro_meas, accel_meas,
        IMUParams(b_g=b_g*0.0, b_a=b_a*0.0, enforce_planar=True, fuse_yaw_with_wheel=True, tau_yaw=2.0),
        wheel_yaw=wheel_yaw
    )

    p_true = np.stack([x_true, y_true, np.zeros_like(t)], axis=1)
    e2 = p2[-1,:2] - p_true[-1,:2]
    print("Final position error (IMU + yaw fusion) [m]:", e2)
    print("Final yaw error (fused) [deg]:", np.degrees(wrap_pi(yaw2[-1] - yaw_true[-1])))

if __name__ == "__main__":
    simulate_ground_robot()
      

Libraries to know (Python ecosystem): \( \) NumPy for vectorization, SciPy rotations (optional), and (later) ROS message types such as sensor_msgs/Imu for real data ingestion.

The C++ version is designed for embedded-style clarity: a minimal quaternion, vector, and strapdown loop. In production, you would typically use Eigen for linear algebra; here we keep dependencies at zero.

File: Chapter5_Lesson2.cpp

/*
Filename: Chapter5_Lesson2.cpp
*/

#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

struct Vec3 {
  double x{0}, y{0}, z{0};
  Vec3() = default;
  Vec3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
  Vec3 operator+(const Vec3& o) const { return Vec3{x + o.x, y + o.y, z + o.z}; }
  Vec3 operator-(const Vec3& o) const { return Vec3{x - o.x, y - o.y, z - o.z}; }
  Vec3 operator*(double s) const { return Vec3{x * s, y * s, z * s}; }
};

struct Mat3 {
  double m[3][3] = { {0,0,0},{0,0,0},{0,0,0} };
  Vec3 mul(const Vec3& v) const {
    return Vec3{
      m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z,
      m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z,
      m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z
    };
  }
};

static inline double dot(const Vec3& a, const Vec3& b) { return a.x*b.x + a.y*b.y + a.z*b.z; }
static inline double norm(const Vec3& a) { return std::sqrt(dot(a,a)); }

struct Quat { // w, x, y, z
  double w{1}, x{0}, y{0}, z{0};

  void normalize() {
    double n = std::sqrt(w*w + x*x + y*y + z*z);
    if (n <= 0) { w=1; x=y=z=0; return; }
    w/=n; x/=n; y/=n; z/=n;
  }

  static Quat mul(const Quat& a, const Quat& b) {
    return Quat{
      a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z,
      a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y,
      a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x,
      a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w
    };
  }

  static Quat fromDeltaTheta(const Vec3& dtheta) {
    double angle = norm(dtheta);
    if (angle < 1e-12) {
      return Quat{1.0, 0.5*dtheta.x, 0.5*dtheta.y, 0.5*dtheta.z};
    }
    Vec3 axis{dtheta.x/angle, dtheta.y/angle, dtheta.z/angle};
    double half = 0.5*angle;
    double s = std::sin(half);
    return Quat{std::cos(half), axis.x*s, axis.y*s, axis.z*s};
  }

  Mat3 toR() const {
    Mat3 R;
    const double ww=w*w, xx=x*x, yy=y*y, zz=z*z;
    R.m[0][0] = 1 - 2*(yy + zz);
    R.m[0][1] = 2*(x*y - w*z);
    R.m[0][2] = 2*(x*z + w*y);
    R.m[1][0] = 2*(x*y + w*z);
    R.m[1][1] = 1 - 2*(xx + zz);
    R.m[1][2] = 2*(y*z - w*x);
    R.m[2][0] = 2*(x*z - w*y);
    R.m[2][1] = 2*(y*z + w*x);
    R.m[2][2] = 1 - 2*(xx + yy);
    return R;
  }

  double yaw() const {
    return std::atan2(2.0*(w*z + x*y), 1.0 - 2.0*(y*y + z*z));
  }

  static Quat fromYaw(double yaw) {
    double half = 0.5*yaw;
    return Quat{std::cos(half), 0.0, 0.0, std::sin(half)};
  }
};

      

Libraries to know (C++ ecosystem): Eigen (linear algebra), and later ROS 2 (rclcpp, sensor_msgs::msg::Imu) for real-time ingestion; but the underlying math should remain transparent.

Java remains common in Android robotics stacks and some research prototypes. The code below mirrors the C++ structure. For matrix-heavy work, EJML is a common choice, but is not required here.

File: Chapter5_Lesson2.java

/*
Filename: Chapter5_Lesson2.java
*/

public class Chapter5_Lesson2 {

  static class Vec3 {
    double x, y, z;
    Vec3(double x, double y, double z) { this.x=x; this.y=y; this.z=z; }
    Vec3 add(Vec3 o) { return new Vec3(x+o.x, y+o.y, z+o.z); }
    Vec3 mul(double s) { return new Vec3(x*s, y*s, z*s); }
    double norm() { return Math.sqrt(x*x + y*y + z*z); }
  }

  static class Mat3 {
    double[][] m = new double[3][3];
    Vec3 mul(Vec3 v) {
      return new Vec3(
        m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z,
        m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z,
        m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z
      );
    }
  }

  static class Quat {
    double w, x, y, z;
    Quat(double w, double x, double y, double z) { this.w=w; this.x=x; this.y=y; this.z=z; }
    void normalize() {
      double n = Math.sqrt(w*w + x*x + y*y + z*z);
      if (n <= 0) { w=1; x=y=z=0; return; }
      w/=n; x/=n; y/=n; z/=n;
    }
    static Quat mul(Quat a, Quat b) {
      return new Quat(
        a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z,
        a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y,
        a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x,
        a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w
      );
    }
    static Quat fromDeltaTheta(Vec3 dtheta) {
      double angle = dtheta.norm();
      if (angle < 1e-12) return new Quat(1.0, 0.5*dtheta.x, 0.5*dtheta.y, 0.5*dtheta.z);
      double ax = dtheta.x/angle, ay = dtheta.y/angle, az = dtheta.z/angle;
      double half = 0.5*angle;
      double s = Math.sin(half);
      return new Quat(Math.cos(half), ax*s, ay*s, az*s);
    }
    Mat3 toR() {
      Mat3 R = new Mat3();
      double ww=w*w, xx=x*x, yy=y*y, zz=z*z;
      R.m[0][0] = 1 - 2*(yy + zz);
      R.m[0][1] = 2*(x*y - w*z);
      R.m[0][2] = 2*(x*z + w*y);
      R.m[1][0] = 2*(x*y + w*z);
      R.m[1][1] = 1 - 2*(xx + zz);
      R.m[1][2] = 2*(y*z - w*x);
      R.m[2][0] = 2*(x*z - w*y);
      R.m[2][1] = 2*(y*z + w*x);
      R.m[2][2] = 1 - 2*(xx + yy);
      return R;
    }
    double yaw() {
      return Math.atan2(2.0*(w*z + x*y), 1.0 - 2.0*(y*y + z*z));
    }
    static Quat fromYaw(double yaw) {
      double half = 0.5*yaw;
      return new Quat(Math.cos(half), 0.0, 0.0, Math.sin(half));
    }
  }
}
      

MATLAB is widely used for prototyping; Simulink is common for real-time deployment workflows. The script below (i) runs a synthetic integration demo, and (ii) programmatically constructs a minimal Simulink model with a MATLAB Function block that performs strapdown propagation at fixed step size.

File: Chapter5_Lesson2.m

% Filename: Chapter5_Lesson2.m

clear; clc;

N = 2000; dt = 0.01;
t = (0:N-1)' * dt;

v0 = 1.2; w0 = 0.20; g = 9.80665;
yaw_true = w0*t;

ax_nav = v0*w0*cos(yaw_true);
ay_nav = v0*w0*sin(yaw_true);
a_nav = [ax_nav, ay_nav, zeros(N,1)];

f_b = zeros(N,3);
for k=1:N
    c = cos(yaw_true(k)); s = sin(yaw_true(k));
    R_bn = [ c, s, 0;
            -s, c, 0;
             0, 0, 1];
    f_b(k,:) = (R_bn * (a_nav(k,:)' - [0;0;-g]))';
end

gyro = [zeros(N,2), w0*ones(N,1)];
accel = f_b;

rng(1);
b_g = [0,0,0.01];
b_a = [0.05,-0.02,0];

gyro_m  = gyro  + b_g + 0.002*randn(N,3);
accel_m = accel + b_a + 0.05*randn(N,3);

wheel_yaw = yaw_true + 0.01*randn(N,1) + 0.002*t;

params.g = g;
params.enforce_planar = true;
params.fuse_yaw_with_wheel = true;
params.tau_yaw = 2.0;

[p_est, v_est, yaw_est] = strapdown_integrate_matlab(t, gyro_m, accel_m, wheel_yaw, params);

function [p_hist, v_hist, yaw_hist] = strapdown_integrate_matlab(t, gyro, accel, wheel_yaw, params)
N = size(t,1);
p = zeros(1,3); v = zeros(1,3); q = [1 0 0 0];
g_nav = [0 0 -params.g];

p_hist = zeros(N,3); v_hist = zeros(N,3); yaw_hist = zeros(N,1);
for k=1:N
    if k==1, dt = t(2)-t(1); else, dt = t(k)-t(k-1); end
    dq = q_from_delta_theta(gyro(k,:)*dt);
    q = q_mul(q, dq); q = q./norm(q);
    yaw = yaw_from_q(q);
    if params.enforce_planar
        q = q_from_yaw(yaw);
    end
    R = q_to_R(q);
    a_nav = (R*accel(k,:)')' + g_nav;
    v = v + a_nav*dt;
    p = p + v*dt + 0.5*a_nav*dt^2;
    if params.enforce_planar
        p(3)=0; v(3)=0;
    end
    if params.fuse_yaw_with_wheel
        alpha = exp(-dt/max(params.tau_yaw,1e-6));
        yaw = wrapToPi(alpha*yaw + (1-alpha)*wheel_yaw(k));
        q = q_from_yaw(yaw);
    end
    p_hist(k,:) = p; v_hist(k,:) = v; yaw_hist(k) = yaw;
end
end

function dq = q_from_delta_theta(dtheta)
angle = norm(dtheta);
if angle < 1e-12
    dq = [1, 0.5*dtheta];
else
    axis = dtheta/angle;
    half = 0.5*angle;
    dq = [cos(half), axis*sin(half)];
end
end

function q = q_mul(a,b)
q = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)-a(4)*b(4), ...
     a(1)*b(2)+a(2)*b(1)+a(3)*b(4)-a(4)*b(3), ...
     a(1)*b(3)-a(2)*b(4)+a(3)*b(1)+a(4)*b(2), ...
     a(1)*b(4)+a(2)*b(3)-a(3)*b(2)+a(4)*b(1)];
end

function R = q_to_R(q)
w=q(1); x=q(2); y=q(3); z=q(4);
R = [1-2*(y^2+z^2), 2*(x*y-w*z), 2*(x*z+w*y); ...
     2*(x*y+w*z), 1-2*(x^2+z^2), 2*(y*z-w*x); ...
     2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x^2+y^2)];
end

function yaw = yaw_from_q(q)
w=q(1); x=q(2); y=q(3); z=q(4);
yaw = atan2(2*(w*z + x*y), 1-2*(y^2+z^2));
end

function q = q_from_yaw(yaw)
half = 0.5*yaw;
q = [cos(half), 0, 0, sin(half)];
end
      

Mathematica is useful for symbolic drift derivations and rapid validation of growth rates under bias assumptions.

File: Chapter5_Lesson2.nb

(* Filename: Chapter5_Lesson2.nb *)

(* 1D accelerometer bias drift *)
ba = b_a;
v[t_] := Integrate[ba, {τ, 0, t}];
p[t_] := Integrate[v[σ], {σ, 0, t}];
{Simplify[v[t]], Simplify[p[t]]}

(* yaw gyro bias and cross-track drift for constant speed v *)
bg = b_g; v0 = v;
eyaw[t_] := bg t;
yerr[t_] := Integrate[v0*Sin[eyaw[τ]], {τ, 0, t}];
Series[yerr[t], {t, 0, 3}]
      

Problem 1 (Rotation Update Preserves Orthonormality): Let \( \mathbf{R}_{k+1}=\mathbf{R}_k\exp([\boldsymbol{\omega}]_{\times}\Delta t) \). Prove that if \( \mathbf{R}_k\in SO(3) \) then \( \mathbf{R}_{k+1}\in SO(3) \).

Solution:

Since \( [\boldsymbol{\omega}]_{\times}^{\top}=-[\boldsymbol{\omega}]_{\times} \), it is skew-symmetric. For any skew-symmetric matrix \( \mathbf{\Omega} \), \( \exp(\mathbf{\Omega}) \) is orthogonal: \( \exp(\mathbf{\Omega})^{\top}\exp(\mathbf{\Omega})=\exp(\mathbf{\Omega}^{\top})\exp(\mathbf{\Omega})=\exp(-\mathbf{\Omega})\exp(\mathbf{\Omega})=\mathbf{I} \). Also \( \det(\exp(\mathbf{\Omega}))=\exp(\operatorname{tr}(\mathbf{\Omega}))=\exp(0)=1 \). Therefore \( \exp(\mathbf{\Omega})\in SO(3) \). As the product of two elements in \( SO(3) \) remains in \( SO(3) \), \( \mathbf{R}_{k+1}\in SO(3) \).

Problem 2 (Accelerometer Bias Drift): In 1D, assume the true acceleration is zero but the accelerometer has constant bias \( b_a \). If you integrate naively, derive \( e_v(t) \) and \( e_p(t) \).

Solution:

\[ a_m(t)=b_a \;\Rightarrow\; e_v(t)=\int_0^t b_a\,ds=b_a t, \qquad e_p(t)=\int_0^t e_v(s)\,ds=\tfrac{1}{2}b_a t^2. \]

Problem 3 (Yaw Bias to Cross-Track Drift): A robot drives forward at constant speed \( v \) in the x-direction. The yaw estimate has error \( e_{\psi}(t)=b_{g,z}t \). Using the small-angle approximation, derive the lateral drift \( e_y(t) \).

Solution:

\[ \dot{e}_y(t)\approx v\sin(e_{\psi}(t)) \approx v e_{\psi}(t)=v b_{g,z} t \;\Rightarrow\; e_y(t)\approx \int_0^t v b_{g,z} s\,ds = \tfrac{1}{2} v b_{g,z} t^2. \]

Problem 4 (Complementary Yaw Fusion Time Constant): Consider \( \dot{\hat{\psi}} = \psi_{\text{imu}}^{\cdot} + \frac{1}{\tau}(\psi_{\text{wheel}}-\hat{\psi}) \). Assume \( \psi_{\text{wheel}} \) is constant and \( \psi_{\text{imu}}^{\cdot}=0 \). Show that the yaw error decays exponentially with time constant \( \tau \).

Solution:

Let \( e(t)=\hat{\psi}(t)-\psi_{\text{wheel}} \). Then \( \dot{e}(t)= -\frac{1}{\tau}e(t) \) which has solution \( e(t)=e(0)\exp(-t/\tau) \). Thus the correction term enforces exponential convergence.

We formulated IMU strapdown integration from first principles: a specific-force accelerometer model, exact rotation updates via the exponential map, and discrete propagation of velocity and position. For ground robots, planar constraints and deterministic complementary yaw correction with wheel odometry provide practical drift mitigation without invoking probabilistic estimation. The derived drift laws show why biases dominate long-term accuracy, motivating the drift analysis in Lesson 3 and filtering in Chapter 6.