From Lesson 1, proprioceptive sensors measure the robot’s internal state. In practice, a robot uses a small set of such sensors to estimate joint configuration, body motion, and contact forces. The three families in this lesson map physical variables to digital signals:

• \( \theta \) and \( \dot{\theta} \) via encoders,
• \( a \), \( \omega \) (and sometimes heading) via IMUs,
• \( \mathbf{F} \), \( \boldsymbol{\tau} \) (wrench) via force/torque sensors.

flowchart TD
  P["Physical quantity (angle / accel / force)"] --> T["Transduction element"]
  T --> A["Analog conditioning (bridge, amplifier, filter)"]
  A --> D["ADC / digital edge counting"]
  D --> C["Calibration + scaling"]
  C --> E["State estimate for control"]
        

Mathematically, each sensor provides a measurement \( y_k \) related to the true quantity \( x_k \) by a static map plus error:

\[ y_k = h(x_k) + \eta_k, \]

where \( h(\cdot) \) is the sensor model and \( \eta_k \) represents imperfections (quantization, bias, thermal drift, etc.). We keep the analysis deterministic here; probabilistic noise models come in Chapter 8.

2.1 Incremental and Absolute Encoders

An encoder converts shaft rotation to digital counts. Let \( N \) be counts per revolution (CPR). If the counter reads \( c_k \in \mathbb{Z} \) at sample \( k \), then the raw angle estimate is

\[ \hat{\theta}_k = \frac{2\pi}{N}\, c_k. \]

Incremental encoders measure changes in counts. Absolute encoders provide a unique code word for each discrete angular sector. Both ultimately reduce to quantized angle measurement.

2.2 Resolution and Quantization Error

The angular resolution is \( \Delta\theta = \frac{2\pi}{N} \). True angle \( \theta_k \) is rounded to the nearest bin:

\[ c_k = \operatorname{round}\!\left(\frac{N}{2\pi}\theta_k\right). \]

Define quantization error \( e_k = \hat{\theta}_k - \theta_k \).

Proposition (Quantization bound). For rounding quantizers,

\[ |e_k| \le \frac{\Delta\theta}{2} = \frac{\pi}{N}. \]

Proof. Rounding maps \( \theta_k \) to the nearest bin center, so the maximum distance to that center is half the bin width. Substituting \( \Delta\theta = 2\pi/N \) gives the stated bound. □

2.3 Quadrature Encoding and Direction

Many incremental encoders provide two channels A and B that are phase-shifted by 90 degrees. Let \( s_A(k), s_B(k) \in \{0,1\} \). The direction is determined by the phase order. With edge counting, the effective CPR becomes a multiple of the base CPR (e.g., 4× for both edges).

2.4 Velocity Estimation

A discrete-time velocity estimate uses finite differences:

\[ \hat{\dot{\theta}}_k = \frac{\hat{\theta}_k - \hat{\theta}_{k-1}}{T_s} = \frac{2\pi}{N T_s}\,(c_k - c_{k-1}), \]

where \( T_s \) is the sampling period. If the quantization bound holds at both steps, the induced velocity error satisfies

\[ |\hat{\dot{\theta}}_k - \dot{\theta}_k| \le \frac{|e_k| + |e_{k-1}|}{T_s} \le \frac{\Delta\theta}{T_s}. \]

Thus higher CPR and faster sampling reduce velocity granularity.

An IMU typically includes a 3-axis accelerometer and 3-axis gyroscope (magnetometers are optional). We avoid full 3D orientation math here; instead we analyze one-axis motion, which generalizes component-wise.

3.1 Gyroscope Model

A gyroscope measures angular rate. The standard affine model is

\[ y^\omega_k = \omega_k + b_\omega + n^\omega_k, \]

where \( b_\omega \) is (approximately) constant bias in this lesson, and \( n^\omega_k \) is small disturbance.

Angle is obtained by discrete integration:

\[ \hat{\theta}_k = \hat{\theta}_{k-1} + T_s\, y^\omega_{k-1}. \]

Proposition (Bias-induced drift). If \( n^\omega_k = 0 \) and \( b_\omega \neq 0 \), then the estimation error grows linearly:

\[ \hat{\theta}_k - \theta_k = k T_s\, b_\omega. \]

Proof. True dynamics satisfy \( \theta_k = \theta_{k-1} + T_s\, \omega_{k-1} \). Subtracting from the estimator update:

\[ \hat{\theta}_k - \theta_k = (\hat{\theta}_{k-1} - \theta_{k-1}) + T_s(\omega_{k-1} + b_\omega - \omega_{k-1}) = (\hat{\theta}_{k-1} - \theta_{k-1}) + T_s b_\omega. \]

By iterating from \( k=1 \) with zero initial error, \( \hat{\theta}_k - \theta_k = kT_s b_\omega \). □

3.2 Accelerometer Model

An accelerometer measures specific force (approximately linear acceleration in 1D):

\[ y^a_k = a_k + b_a + n^a_k. \]

Integrating acceleration yields velocity and position:

\[ \hat{v}_k = \hat{v}_{k-1} + T_s\, y^a_{k-1}, \qquad \hat{x}_k = \hat{x}_{k-1} + T_s\, \hat{v}_{k-1}. \]

Bias in acceleration causes quadratic growth in position error (exercise in Problems).

flowchart LR
  G["Gyro rate y_omega"] --> I1["Integrate → angle_hat"]
  A["Accel y_a"] --> I2["Integrate → v_hat → x_hat"]
  I1 --> USE["Used by controller"]
  I2 --> USE
        

In real robots, accelerometer-based tilt cues and gyro-based angle-rate cues are combined to reduce drift; formal fusion is introduced later in Chapter 8.

Force/torque (F/T) sensors measure interaction between robot and environment. A complete 6-axis sensor measures a wrench \( \mathbf{w} = [F_x,F_y,F_z,\tau_x,\tau_y,\tau_z]^T \).

4.1 Strain-Gauge Transduction (Linear Elasticity)

In linear elasticity, a small strain \( \varepsilon \) produces stress \( \sigma = E\varepsilon \) (Hooke’s law), where \( E \) is Young’s modulus. For a beam-like sensing element, force causes strain proportional to applied load:

\[ \varepsilon = \alpha F, \qquad \Rightarrow \qquad \Delta R/R = G_f \varepsilon, \]

where \( G_f \) is the gauge factor and \( \Delta R \) the resistance change.

4.2 Wheatstone Bridge Output

Using a full bridge with excitation voltage \( V_{ex} \), the differential output is approximately linear in strain:

\[ V_{out} \approx \frac{V_{ex}}{4}\, G_f \varepsilon = \frac{V_{ex}}{4}\, G_f \alpha F. \]

Thus a scalar force estimate is \( \hat{F} = k_V V_{out} \) for calibration gain \( k_V \).

4.3 Multi-Axis Wrench Model and Calibration

A 6-axis sensor uses multiple strain gauges. Stack the conditioned voltages into \( \mathbf{v} \in \mathbb{R}^m \). Under small deformation, a linear model holds:

\[ \mathbf{v} = \mathbf{S}\,\mathbf{w} + \mathbf{b} , \]

where \( \mathbf{S} \) is the sensitivity matrix and \( \mathbf{b} \) the offset. Calibration uses known applied wrenches.

Least-squares calibration. Given \( n \) calibration trials with known \( \mathbf{w}^{(i)} \) and measured \( \mathbf{v}^{(i)} \), define

\[ \mathbf{V} = [\mathbf{v}^{(1)}\;\cdots\;\mathbf{v}^{(n)}], \quad \mathbf{W} = [\mathbf{w}^{(1)}\;\cdots\;\mathbf{w}^{(n)}]. \]

Ignoring offsets (or after removing mean), solve \( \mathbf{V} \approx \mathbf{S}\mathbf{W} \). The minimum-error estimate is

\[ \hat{\mathbf{S}} = \mathbf{V}\mathbf{W}^T(\mathbf{W}\mathbf{W}^T)^{-1} = \mathbf{V}\mathbf{W}^\dagger , \]

where \( \mathbf{W}^\dagger \) is the Moore–Penrose pseudoinverse.

Proof sketch. Minimize the Frobenius norm \( J(\mathbf{S})=\|\mathbf{V}-\mathbf{S}\mathbf{W}\|_F^2 \). Taking the derivative and setting to zero:

\[ \frac{\partial J}{\partial \mathbf{S}} = -2(\mathbf{V}-\mathbf{S}\mathbf{W})\mathbf{W}^T = 0 \quad\Rightarrow\quad \mathbf{S}\mathbf{W}\mathbf{W}^T = \mathbf{V}\mathbf{W}^T. \]

If \( \mathbf{W}\mathbf{W}^T \) is invertible, the solution above follows. □

These snippets illustrate core computations (count-to-angle, discrete integration, and wrench reconstruction). Hardware drivers are platform-specific and appear in later chapters.

5.1 Python

import numpy as np

# ---------- Encoder: counts -> angle, velocity ----------
def encoder_angle_velocity(counts, N, Ts):
    counts = np.asarray(counts, dtype=float)
    theta = (2*np.pi / N) * counts
    dtheta = np.diff(theta, prepend=theta[0]) / Ts
    return theta, dtheta

# Example
N = 2048
Ts = 0.001
counts = np.cumsum([0, 1, 1, 2, 2, 1, 0, -1])  # toy tick stream
theta_hat, omega_hat = encoder_angle_velocity(counts, N, Ts)

# ---------- IMU gyro integration (1D) ----------
def integrate_gyro(omega_meas, Ts, theta0=0.0):
    omega_meas = np.asarray(omega_meas, dtype=float)
    theta = np.zeros_like(omega_meas)
    theta[0] = theta0
    for k in range(1, len(omega_meas)):
        theta[k] = theta[k-1] + Ts * omega_meas[k-1]
    return theta

omega_meas = 0.2 + 0.01*np.random.randn(1000)  # rad/s with small noise
theta_from_gyro = integrate_gyro(omega_meas, Ts)

# ---------- F/T sensor: voltages -> wrench ----------
def wrench_from_voltages(v, S_hat, b_hat=None):
    v = np.asarray(v, dtype=float)
    if b_hat is not None:
        v = v - b_hat
    # Least-squares wrench estimate
    w_hat, *_ = np.linalg.lstsq(S_hat, v, rcond=None)
    return w_hat

m = 8  # number of gauge channels (example)
S_hat = np.random.randn(m, 6)
v_meas = np.random.randn(m)
w_hat = wrench_from_voltages(v_meas, S_hat)
      

5.2 C++ (Eigen)

#include <vector>
#include <Eigen/Dense>
#include <cmath>

// Encoder: counts -> angle, velocity
void encoderAngleVelocity(const std::vector<int>& counts,
                          int N, double Ts,
                          std::vector<double>& theta,
                          std::vector<double>& omega) {
    int n = (int)counts.size();
    theta.resize(n);
    omega.resize(n);
    double scale = 2.0 * M_PI / (double)N;

    for (int k = 0; k < n; ++k) {
        theta[k] = scale * (double)counts[k];
        if (k == 0) omega[k] = 0.0;
        else omega[k] = (theta[k] - theta[k-1]) / Ts;
    }
}

// IMU gyro integration (1D)
std::vector<double> integrateGyro(const std::vector<double>& omegaMeas,
                                 double Ts, double theta0 = 0.0) {
    int n = (int)omegaMeas.size();
    std::vector<double> theta(n, 0.0);
    theta[0] = theta0;
    for (int k = 1; k < n; ++k) {
        theta[k] = theta[k-1] + Ts * omegaMeas[k-1];
    }
    return theta;
}

// F/T: voltages -> wrench via least squares
Eigen::VectorXd wrenchFromVoltages(const Eigen::VectorXd& v,
                                   const Eigen::MatrixXd& S_hat) {
    // Solve min ||S_hat w - v||
    return S_hat.colPivHouseholderQr().solve(v);
}
      

5.3 Java

import java.util.Arrays;

public class SensorMath {

    // Encoder counts -> angle (rad)
    public static double[] countsToAngle(int[] counts, int N) {
        double scale = 2.0 * Math.PI / (double) N;
        double[] theta = new double[counts.length];
        for (int k = 0; k < counts.length; k++) {
            theta[k] = scale * counts[k];
        }
        return theta;
    }

    // Gyro integration (1D)
    public static double[] integrateGyro(double[] omegaMeas, double Ts, double theta0) {
        double[] theta = new double[omegaMeas.length];
        theta[0] = theta0;
        for (int k = 1; k < omegaMeas.length; k++) {
            theta[k] = theta[k-1] + Ts * omegaMeas[k-1];
        }
        return theta;
    }

    // Wrench reconstruction using normal equations
    // (for small systems; in practice use a linear algebra library)
    public static double[] wrenchLS(double[][] S, double[] v) {
        int m = S.length;
        int p = S[0].length; // p=6
        double[][] A = new double[p][p];
        double[] b = new double[p];

        for (int i = 0; i < m; i++) {
            for (int r = 0; r < p; r++) {
                b[r] += S[i][r] * v[i];
                for (int c = 0; c < p; c++) {
                    A[r][c] += S[i][r] * S[i][c];
                }
            }
        }

        // Solve A w = b by naive Gaussian elimination
        double[] w = Arrays.copyOf(b, p);
        for (int k = 0; k < p; k++) {
            double pivot = A[k][k];
            for (int j = k; j < p; j++) A[k][j] /= pivot;
            w[k] /= pivot;
            for (int i = k+1; i < p; i++) {
                double f = A[i][k];
                for (int j = k; j < p; j++) A[i][j] -= f * A[k][j];
                w[i] -= f * w[k];
            }
        }
        for (int k = p-1; k >= 0; k--) {
            for (int i = 0; i < k; i++) {
                w[i] -= A[i][k] * w[k];
            }
        }
        return w;
    }
}
      

5.4 Matlab / Simulink

% ---------- Encoder: counts -> angle, velocity ----------
function [theta, omega] = encoder_angle_velocity(counts, N, Ts)
    theta = (2*pi/N) * counts(:);
    omega = [0; diff(theta)/Ts];
end

% Example
N = 1024; Ts = 1e-3;
counts = cumsum([0 1 1 0 -1 -1 0 1]);
[theta_hat, omega_hat] = encoder_angle_velocity(counts, N, Ts);

% ---------- IMU gyro integration (1D) ----------
function theta = integrate_gyro(omega_meas, Ts, theta0)
    omega_meas = omega_meas(:);
    theta = zeros(size(omega_meas));
    theta(1) = theta0;
    for k = 2:length(omega_meas)
        theta(k) = theta(k-1) + Ts * omega_meas(k-1);
    end
end

omega_meas = 0.3 + 0.01*randn(1000,1);
theta_from_gyro = integrate_gyro(omega_meas, Ts, 0);

% ---------- F/T reconstruction ----------
function w_hat = wrench_from_voltages(v, S_hat)
    % least squares: w_hat = argmin ||S_hat w - v||
    w_hat = S_hat \ v;
end
      

In Simulink, the same computations map directly to blocks: edge-counting for encoders, discrete-time integrators for IMU signals, and a “Matrix Solve” block for wrench reconstruction.

Problem 1 (Encoder resolution and error): A rotary encoder has \( N=5000 \) CPR. (a) Compute \( \Delta\theta \). (b) Give the worst-case angle error bound.

Solution:

\[ \Delta\theta = \frac{2\pi}{N} = \frac{2\pi}{5000} \approx 1.257\times 10^{-3}\ \text{rad}. \]

\[ |e_k| \le \frac{\Delta\theta}{2} \approx 6.283\times 10^{-4}\ \text{rad}. \]

Problem 2 (Velocity granularity): The encoder from Problem 1 is sampled at \( T_s=2\ \text{ms} \). Using the bound in Section 2.4, compute an upper bound for the induced velocity error.

Solution:

\[ |\hat{\dot{\theta}}_k - \dot{\theta}_k| \le \frac{\Delta\theta}{T_s} = \frac{1.257\times 10^{-3}}{2\times 10^{-3}} \approx 0.629\ \text{rad/s}. \]

This is a worst-case bound; typical errors are smaller but show why high CPR is valuable for slow motions.

Problem 3 (Gyro bias drift): A 1D gyro has bias \( b_\omega = 0.02\ \text{rad/s} \). With \( T_s = 0.01\ \text{s} \), estimate the angle error after 60 seconds.

Solution: There are \( k=60/0.01=6000 \) steps. From Section 3.1,

\[ \hat{\theta}_k - \theta_k = kT_s b_\omega = 6000 \cdot 0.01 \cdot 0.02 = 1.2\ \text{rad}. \]

Even small biases create large errors over time, motivating later fusion methods.

Problem 4 (F/T calibration): Suppose a 6-axis sensor has \( m=6 \) channels and you perform \( n=10 \) trials. (a) Write the least-squares estimate of \( \mathbf{S} \). (b) State a condition ensuring uniqueness.

Solution:

\[ \hat{\mathbf{S}} = \mathbf{V}\mathbf{W}^T(\mathbf{W}\mathbf{W}^T)^{-1}. \]

Uniqueness holds when \( \mathbf{W}\mathbf{W}^T \) is invertible, i.e., the applied wrenches span \( \mathbb{R}^6 \) (rank 6).

We introduced encoders, IMUs, and force/torque sensors as foundational proprioceptive devices. Encoders provide quantized joint angle and velocity with explicit resolution bounds. IMUs measure angular rate and acceleration; discretized integration reveals how bias causes drift. Force/torque sensors map elastic strain to a wrench through linear calibration, solvable by least squares. These models will be used later when we discuss perception pipelines and sensor quality.