1 Analysis

1.1 Fast Fourier Transform (FFT)

1.1.1 Overview & Motivation

Many problems in signal processing, communications, and control require knowing the frequency content of a signal. The Discrete Fourier Transform (DFT) answers this question exactly, but its direct computation costs \(O(N^2)\) multiplications — prohibitive for real-time embedded systems processing thousands of samples.

The Fast Fourier Transform is a family of algorithms that compute the DFT in \(O(N \log N)\) operations by exploiting symmetry and periodicity of the complex exponential. This library implements the Radix-2 Cooley-Tukey variant, which recursively splits an \(N\)-point DFT into two \(N/2\)-point DFTs, requiring \(N\) to be a power of two.

The core insight is that the DFT matrix has a recursive block structure: half of its entries are shared between the even-indexed and odd-indexed sub-problems, so most of the work can be reused rather than recomputed.

1.1.2 Mathematical Theory

1.1.2.1 Prerequisites

The Discrete Fourier Transform of a length-\(N\) sequence \(x[n]\) is defined as:

\[X[k] = \sum_{n=0}^{N-1} x[n] \, e^{-j2\pi kn/N}, \quad k = 0, 1, \ldots, N-1\]

The inverse DFT recovers the original sequence:

\[x[n] = \frac{1}{N} \sum_{k=0}^{N-1} X[k] \, e^{j2\pi kn/N}\]

We define the twiddle factor \(W_N = e^{-j2\pi/N}\), so \(X[k] = \sum_{n} x[n] \, W_N^{kn}\).

1.1.2.2 Cooley-Tukey Decomposition

Split the sum into even-indexed and odd-indexed terms:

\[X[k] = \underbrace{\sum_{m=0}^{N/2-1} x[2m] \, W_{N/2}^{km}}_{E[k]} \;+\; W_N^k \underbrace{\sum_{m=0}^{N/2-1} x[2m+1] \, W_{N/2}^{km}}_{O[k]}\]

Because \(E[k]\) and \(O[k]\) are periodic with period \(N/2\):

\[X[k] = E[k] + W_N^k \, O[k]\] \[X[k + N/2] = E[k] - W_N^k \, O[k]\]

This pair of equations is the butterfly operation: two outputs from two inputs and one twiddle-factor multiplication.

1.1.2.3 Bit-Reversal Permutation

The recursive decomposition reorders input by bit-reversing the sample indices. For in-place computation the input array is pre-permuted so that butterfly stages proceed sequentially from the smallest sub-problems to the full \(N\)-point result.

1.1.3 Complexity Analysis

Why \(O(N \log N)\): There are \(\log_2 N\) butterfly stages, each performing \(N/2\) butterfly operations (one complex multiply + two complex adds). Total: \(\frac{N}{2} \log_2 N\) complex multiplications.

Twiddle factors are pre-computed and stored in a lookup table of size \(N/2\), so no trigonometric evaluations occur at runtime.

1.1.4 Step-by-Step Walkthrough

Input: \(x = [1, 2, 3, 4]\), \(N = 4\)

Step 1 — Bit-reversal permutation

Reordered: \([1, 3, 2, 4]\)

Step 2 — Stage 1 (size-2 butterflies), twiddle \(W_2^0 = 1\)

• Butterfly\((1, 3)\): outputs \((1+3, \; 1-3) = (4, \; -2)\)
• Butterfly\((2, 4)\): outputs \((2+4, \; 2-4) = (6, \; -2)\)

Result: \([4, -2, 6, -2]\)

Step 3 — Stage 2 (size-4 butterfly), twiddles \(W_4^0 = 1\), \(W_4^1 = -j\)

• \(X[0] = 4 + 1 \cdot 6 = 10\)
• \(X[1] = -2 + (-j)(-2) = -2 + 2j\)
• \(X[2] = 4 - 1 \cdot 6 = -2\)
• \(X[3] = -2 - (-j)(-2) = -2 - 2j\)

Output: \(X = [10,\; -2+2j,\; -2,\; -2-2j]\)

Verification: \(X[0] = 1+2+3+4 = 10\) ✓

1.1.5 Pitfalls & Edge Cases

• Power-of-2 constraint. Non-power-of-2 inputs require zero-padding, which introduces interpolation artifacts in the frequency domain.
• Fixed-point overflow. Each butterfly stage can double the magnitude. For Q15/Q31 types, scale the input by \(1/N\) before the transform or apply per-stage scaling.
• Spectral leakage. A finite-length DFT implicitly applies a rectangular window. Use a window function before the transform to reduce sidelobes.
• Aliasing. The Nyquist-Shannon theorem requires \(f_s \geq 2 f_{\max}\). Violating this folds high frequencies onto low frequencies irreversibly.
• Frequency resolution is \(\Delta f = f_s / N\). Increasing \(N\) improves resolution at the cost of more memory and computation.

1.1.6 Variants & Generalizations

1.1.7 Applications

• Spectral analysis — Identifying frequency components, harmonics, and noise floors in sensor data.
• Convolution and filtering — Multiplication in the frequency domain is equivalent to time-domain convolution; enables efficient FIR filtering.
• Power Spectral Density — Used internally by Welch’s method for spectral estimation.
• Communication systems — OFDM modulation/demodulation is built directly on the FFT/IFFT pair.
• Vibration and fault diagnosis — Examining spectral peaks to detect mechanical anomalies.
• Data compression — Foundation for transforms (DCT, MDCT) used in JPEG, MP3, etc.

1.1.8 Connections to Other Algorithms

graph LR
    FFT --> PSD["Power Spectral Density"]
    FFT --> DCT["Discrete Cosine Transform"]
    WIN["Window Functions"] --> FFT
    FFT -.-> DK["Durand-Kerner (polynomial eval)"]

1.2 Real-Input FFT (RFFT)

1.2.1 Overview & Motivation

Most sensor data is real-valued: audio samples, vibration measurements, ADC readings. A general complex FFT applied to such data wastes half its work because the imaginary input channel is identically zero. The real-input FFT exploits this structure to deliver roughly twice the throughput and half the memory of a complex FFT at the same transform length. On bandwidth- and RAM-limited embedded targets this difference is significant.

1.2.2 Mathematical Theory

1.2.2.1 Prerequisites

The RFFT is a specialisation of the length-\(N\) Discrete Fourier Transform

\[X[k] = \sum_{n=0}^{N-1} x[n]\, W_N^{nk}, \quad W_N = e^{-j2\pi/N}\]

for the case where every \(x[n]\) is real-valued. Because \(x[n] \in \mathbb{R}\), the spectrum satisfies Hermitian symmetry: \(X[N-k] = X^*[k]\). Consequently only the \(N/2 + 1\) bins \(k = 0, \dots, N/2\) carry independent information.

1.2.2.2 Decimation-in-time split

Split \(x\) into its even- and odd-indexed subsequences:

\[x_e[n] = x[2n], \qquad x_o[n] = x[2n+1], \qquad n = 0, \dots, N/2 - 1.\]

Their length-\((N/2)\) DFTs are \(E[k]\) and \(O[k]\). The full transform decomposes as

\[X[k] = E[k] + W_N^k \, O[k], \qquad k = 0, \dots, N/2 - 1,\]

\[X[N/2 + k] = E[k] - W_N^k \, O[k], \qquad k = 0, \dots, N/2 - 1.\]

The twiddle factor \(W_N^k = e^{-j2\pi k/N}\) is precomputed for \(k = 1, \dots, N/2 - 1\). The boundary cases are purely real:

\[X[0] = E[0] + O[0], \qquad X[N/2] = E[0] - O[0].\]

1.2.2.3 Inverse transform

Given the \(N/2 + 1\) unique bins, the even and odd sub-spectra are recovered using the inverse of the split relations. For \(k = 1, \dots, N/2 - 1\):

\[E[k] = \tfrac{1}{2}(X[k] + X^*[N/2 - k]),\]

\[O[k] = W_N^{-k} \cdot \tfrac{1}{2}(X[k] - X^*[N/2 - k]).\]

Two independent length-\((N/2)\) inverse DFTs then reconstruct \(x_e\) and \(x_o\), which interleave back to \(x\).

1.2.3 Complexity Analysis

The constant factor is approximately half that of a complex \(N\)-point FFT because both sub-DFTs exploit real-input symmetry internally (they receive real-only vectors).

1.2.4 Step-by-Step Walkthrough

Input: \(x = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\) (unit impulse, \(N = 16\)).

1. Split: \(x_e = [1, 0, 0, 0, 0, 0, 0, 0]\), \(x_o = [0, 0, 0, 0, 0, 0, 0, 0]\).
2. Sub-DFTs: \(E[k] = 1\) for all \(k\); \(O[k] = 0\) for all \(k\).
3. Combine: \(X[k] = E[k] + W_{16}^k \cdot 0 = 1\) for \(k = 0, \dots, 8\).
4. Result: \(|X[k]| = 1\) — flat magnitude, the hallmark of a unit impulse.

1.2.5 Pitfalls & Edge Cases

• N must be an even power of two. The underlying radix-2 engine imposes this; a static_assert enforces it at compile time.
• DC and Nyquist bins are exactly real. Floating-point errors in the sub-DFTs may introduce tiny imaginary residuals; downstream code should not rely on them being precisely zero.
• Round-trip accumulation. Two forward and two inverse sub-DFTs compound rounding errors. The round-trip error is \(O(\varepsilon \log N)\) where \(\varepsilon\) is the floating-point machine epsilon.
• Spectrum aliasing at Nyquist. The bin at \(k = N/2\) is real and unique only when \(N\) is even, which is always the case here.

1.2.6 Variants & Generalizations

• Split-radix RFFT: replaces the two half-length DFTs with a mixed radix-2/4 decomposition, reducing the arithmetic count by roughly 30% versus the radix-2 split used here.
• In-place RFFT: packs the \(N/2 + 1\) complex output into the same \(N\)-sample real buffer, halving memory further.
• Zero-padded RFFT: appending zeros to \(x\) before the transform gives finer frequency resolution at the cost of more compute.

1.2.7 Applications

• Spectrum analysis of real sensor signals (vibration, audio, pressure).
• Cross-correlation and convolution of real sequences via ConvolutionCorrelation.
• Power spectral density estimation via PowerDensitySpectrum.
• Feature extraction in embedded machine learning pipelines.

1.2.8 Connections to Other Algorithms

• FastFourierTransform / FastFourierTransformRadix2Impl: the injected complex engine that does the heavy butterfly work; RFFT adds only a linear-time split pass on top.
• PowerDensitySpectrum: consumes the output of the RFFT to estimate spectral power.
• ConvolutionCorrelation: uses the FFT for \(O(N \log N)\) convolution; RFFT halves the cost when both operands are real.
• HilbertTransform: also builds on the FFT engine with a frequency-domain manipulation pass, similar in structure to the RFFT split step.

1.3 Power Spectral Density (Welch’s Method)

1.3.1 Overview & Motivation

Given a noisy signal, a natural question is: how is the signal’s power distributed across frequencies? A single FFT of the entire record gives a very noisy estimate — the variance of the periodogram does not decrease as the record grows. Welch’s method trades frequency resolution for statistical reliability by dividing the signal into overlapping, windowed segments, computing a periodogram for each, and averaging them. The result is a smooth, consistent estimate of the Power Spectral Density (PSD).

This is the standard approach in vibration analysis, audio engineering, communication system design, and any domain where reliable spectral estimates matter more than the finest possible frequency resolution.

1.3.2 Mathematical Theory

1.3.2.1 Power Spectral Density Definition

For a wide-sense stationary process \(x(t)\), the PSD is the Fourier transform of the autocorrelation function (Wiener-Khinchin theorem):

\[S_{xx}(f) = \int_{-\infty}^{\infty} R_{xx}(\tau) \, e^{-j2\pi f\tau} \, d\tau\]

In practice we estimate \(S_{xx}\) from a finite record.

1.3.2.2 The Periodogram

The periodogram of a length-\(N\) segment \(x[n]\) is:

\[\hat{P}(f_k) = \frac{1}{N} \left| \sum_{n=0}^{N-1} x[n] \, e^{-j2\pi kn/N} \right|^2 = \frac{1}{N} |X[k]|^2\]

This is an unbiased but inconsistent estimator — its variance does not shrink as \(N \to \infty\).

1.3.2.3 Welch’s Method

1. Segment the signal of length \(L\) into \(K\) overlapping segments of length \(N\) with step size \(S = N - \text{overlap}\):

\[x_i[n] = x[n + iS], \quad i = 0, 1, \ldots, K-1\]

2. Window each segment:

\[w_i[n] = w[n] \cdot x_i[n]\]

3. Compute the periodogram of each windowed segment:

\[P_i[k] = \frac{1}{N} |W_i[k]|^2\]

4. Average across all segments:

\[\hat{S}_{xx}[k] = \frac{1}{K} \sum_{i=0}^{K-1} P_i[k]\]

The averaging reduces variance by a factor of approximately \(K\) (slightly less due to overlap correlation).

1.3.2.4 Frequency Resolution

\[\Delta f = \frac{f_s}{N}\]

Increasing segment size \(N\) improves resolution but reduces the number of averages \(K\) for a fixed-length record.

1.3.3 Complexity Analysis

Where \(K = \lfloor (L - N) / S \rfloor + 1\) and \(S = N \cdot (1 - \text{overlap fraction})\).

Why: Each segment requires an \(O(N \log N)\) FFT, and there are \(K\) segments. The averaging and magnitude-squared operations are \(O(N)\) per segment.

1.3.4 Step-by-Step Walkthrough

Input: \(x = [1, 3, 5, 7, 6, 4, 2, 0]\), segment size \(N=4\), 50 % overlap, rectangular window.

Step 1 — Segmentation (step \(S = 2\)):

Step 2 — Window (rectangular → no change)

Step 3 — FFT and periodogram for \(x_0 = [1, 3, 5, 7]\):

• \(X_0 = [16, \; -4+4j, \; -4, \; -4-4j]\)
• \(P_0 = \frac{1}{4}[256, \; 32, \; 16, \; 32] = [64, \; 8, \; 4, \; 8]\)

(Repeat for \(x_1\) and \(x_2\).)

Step 4 — Average the three periodograms element-wise.

Output: One-sided PSD estimate at frequencies \(0, \frac{f_s}{4}, \frac{f_s}{2}\).

1.3.5 Pitfalls & Edge Cases

• Too few segments. If the signal is short relative to the segment size, \(K\) is small and the variance reduction is limited. Reduce \(N\) or increase overlap.
• Overlap > 75 % gives diminishing returns — the segments become highly correlated and additional averages barely reduce variance.
• Window choice affects both the main-lobe width and sidelobe level. A Hanning window is a safe default; use Blackman for better dynamic range at the cost of resolution.
• Segment size must be even (and a power of 2 for the FFT). Odd or non-power-of-2 sizes are rejected at compile time.
• DC and Nyquist bins appear only once in the one-sided spectrum; all other bins are doubled. Omitting this correction distorts the total power.

1.3.6 Variants & Generalizations

1.3.7 Applications

• Vibration analysis — Identifying resonant frequencies and tracking their amplitude over time.
• Noise characterization — Determining white, pink, or brown noise profiles in electronic circuits or sensors.
• Communication systems — Measuring occupied bandwidth and interference levels.
• Audio engineering — Equalizer design, room acoustics analysis.
• Biomedical signal processing — EEG, EMG spectral analysis for clinical diagnostics.

1.3.8 Connections to Other Algorithms

graph LR
    SIG["Input Signal"] --> WIN["Window Functions"]
    WIN --> FFT["Fast Fourier Transform"]
    FFT --> PSD["Power Spectral Density"]
    PSD --> YW["Yule-Walker (parametric alternative)"]

1.4 Discrete Cosine Transform (DCT)

1.4.1 Overview & Motivation

The Discrete Cosine Transform projects a real-valued signal onto a set of cosine basis functions at different frequencies. Unlike the DFT, the DCT produces purely real coefficients and exhibits superior energy compaction — most of the signal’s energy concentrates in a few low-frequency coefficients. This property makes it the backbone of lossy compression standards (JPEG, MPEG, AAC).

This library implements DCT-II (the most common variant) by leveraging the FFT: the input is rearranged, transformed via a real-valued FFT, and post-processed with twiddle factors. This approach inherits the FFT’s \(O(N \log N)\) efficiency rather than requiring a dedicated \(O(N^2)\) computation.

1.4.2 Mathematical Theory

1.4.2.1 DCT-II Definition

For a length-\(N\) sequence \(x[n]\), the DCT-II is defined as:

\[X[k] = \sum_{n=0}^{N-1} x[n] \cos\!\left(\frac{\pi}{N}\left(n + \tfrac{1}{2}\right) k\right), \quad k = 0, 1, \ldots, N-1\]

The inverse (DCT-III, also called IDCT) recovers \(x[n]\):

\[x[n] = \frac{1}{N}\left[\frac{X[0]}{2} + \sum_{k=1}^{N-1} X[k] \cos\!\left(\frac{\pi}{N}\left(n + \tfrac{1}{2}\right) k\right)\right]\]

1.4.2.2 FFT-Based Computation

The DCT can be computed via the DFT of a reordered sequence:

1. Form a new sequence \(y[n]\) by interleaving even-indexed and reverse odd-indexed samples of \(x\).
2. Compute the \(N\)-point FFT: \(Y[k] = \text{FFT}\{y\}\).
3. Apply twiddle factors: \(X[k] = 2 \cdot \text{Re}\!\left(W[k] \cdot Y[k]\right)\), where \(W[k] = e^{-j\pi k / 2N}\).

1.4.2.3 Why Cosine Basis?

Cosine functions are symmetric (even functions). The DCT implicitly mirrors the signal rather than making it periodic, avoiding the boundary discontinuities that cause spectral leakage in the DFT. This is why DCT coefficients decay faster and energy compaction is better.

1.4.3 Complexity Analysis

The reordering step and twiddle multiplication are both linear, so the overall cost is determined by the FFT.

1.4.4 Step-by-Step Walkthrough

Input: \(x = [1, 2, 3, 4]\), \(N = 4\)

Step 1 — Reorder for FFT

Even-indexed positions filled left-to-right, odd-indexed filled right-to-left:

\[y = [x[0],\; x[2],\; x[3],\; x[1]] = [1, 3, 4, 2]\]

Step 2 — Compute FFT

\[Y = \text{FFT}([1, 3, 4, 2]) = [10,\; -3+j,\; -2,\; -3-j]\]

Step 3 — Apply twiddle factors \(W[k] = e^{-j\pi k/8}\)

Output: \(X \approx [20, -4.44, -2.83, -0.47]\)

Notice how most of the energy is in \(X[0]\) (the DC component) — energy compaction in action.

1.4.5 Pitfalls & Edge Cases

• Power-of-2 length required — inherited from the underlying FFT constraint.
• Normalization convention. Different references use different scaling (some include \(\sqrt{2/N}\)). Verify which convention the consumer expects.
• Fixed-point overflow. The reordering and FFT steps must preserve range; apply the 0.9999 scaling factor used throughout this library.
• Inverse accuracy. Rounding errors accumulate in the forward-then-inverse round-trip, especially for Q15 types.
• Real input only. Complex inputs are not supported by the reordering trick.

1.4.6 Variants & Generalizations

1.4.7 Applications

• Image compression — JPEG applies DCT to 8×8 pixel blocks; quantizing high-frequency coefficients achieves compression.
• Audio compression — MDCT (a DCT variant) is the core transform in MP3, AAC, and Opus codecs.
• Feature extraction — Mel-frequency cepstral coefficients (MFCCs) used in speech recognition are derived from a DCT.
• Data reduction — Truncating small DCT coefficients provides a compact signal representation for embedded storage.
• Numerical methods — DCT is used in fast solvers for certain partial differential equations (Poisson’s equation on regular grids).

1.4.8 Connections to Other Algorithms

graph LR
    DCT["Discrete Cosine Transform"]
    FFT["Fast Fourier Transform"]
    WIN["Window Functions"]
    FFT --> DCT
    WIN -.-> DCT

1.5 Signal Detectors

1.5.1 Overview & Motivation

Real-time signal monitoring requires lightweight, stateful primitives that track envelope, periodicity, and energy without buffering entire waveforms. Three composable detectors cover the most common needs: holding the peak magnitude for VU-style metering, counting sign reversals to estimate frequency, and smoothing instantaneous power to approximate RMS level. All three operate in O(1) time and O(1) memory per sample, making them suitable for tight interrupt service routines.

1.5.2 Mathematical Theory

1.5.2.1 Peak Hold

Let \(m_n = |x_n|\). With a decay coefficient \(d \in (0, 1]\), the held peak evolves as:

\[p_n = \max(m_n,\; d \cdot p_{n-1})\]

When \(d = 1\) the peak is infinite-hold; when \(d < 1\) it releases exponentially toward zero between new maxima.

1.5.2.2 Zero-Crossing Detection

A zero crossing occurs at sample \(n\) when the sign of \(x_n\) differs from the sign of \(x_{n-1}\) and \(|x_n|\) exceeds a hysteresis threshold \(h \geq 0\):

\[\text{crossed}_n = \bigl(\mathrm{sgn}(x_n) \neq \mathrm{sgn}(x_{n-1})\bigr) \land \bigl(|x_n| > h\bigr)\]

The instantaneous fundamental frequency of a periodic signal can be estimated from the cumulative count \(C\) over a window of \(N\) samples at sampling period \(T_s\):

\[f_0 \approx \frac{C}{2 \, N \, T_s}\]

1.5.2.3 RMS Envelope

A one-pole IIR filter is applied to \(x^2\), yielding an exponentially weighted mean-square:

\[E_n = E_{n-1} + \alpha\,(x_n^2 - E_{n-1}), \quad \alpha \in (0, 1)\]

The RMS envelope is:

\[r_n = \sqrt{E_n}\]

This is structurally identical to an exponential moving average applied to the squared signal.

1.5.3 Complexity Analysis

No buffers, no windows, no dynamic allocation.

1.5.4 Step-by-Step Walkthrough

1.5.4.1 Peak Hold (decay = 0.5), input = [0.8, 0, 0, 0]

1.5.4.2 Zero-Crossing Counter (h = 0), input = [1, -1, 1, -1]

1.5.4.3 RMS Envelope (\(\alpha = 0.25\)), DC input \(x = 0.4\)

\(E_0 = 0\); after \(n\) steps, \(E_n = 0.16\,(1-(1-0.25)^n)\). At \(n = 20\): \(E_{20} \approx 0.1599\), \(r_{20} \approx 0.3999\).

1.5.5 Pitfalls & Edge Cases

• Decay = 0 collapses the peak hold to the instantaneous absolute value; decay must be in \((0, 1]\) for meaningful hold behavior.
• Zero-crossing sign comparison must treat \(x = 0\) consistently; a sample at exactly zero with \(h = 0\) can cause spurious double-counts if not handled. The hysteresis guard mitigates this.
• RMS Envelope with very small \(\alpha\) responds slowly to changes in signal level; with \(\alpha\) near 1 it approaches the instantaneous absolute value squared.
• The RMS detector returns 0 at initialization (\(E_0 = 0\)) and converges to the true RMS from below for positive-power signals.

1.5.6 Variants & Generalizations

• The peak hold decay parameter maps directly to a time constant: \(\tau = -T_s / \ln(d)\), bridging the gap between digital and analog VU meter release times.
• A bilateral zero-crossing counter can distinguish positive-going from negative-going edges, enabling half-wave frequency estimation.
• The RMS envelope is the special case of an exponential moving average applied to \(x^2\); replacing the squaring with \(|x|\) yields a mean-absolute-value envelope.

1.5.7 Applications

• VU meters and compressor side-chains use peak hold with a configurable release time.
• Zero-crossing rate is a low-cost voiced/unvoiced classifier in speech processing.
• RMS envelope tracks AC signal levels in power monitoring, audio dynamic processing, and vibration analysis.
• All three detectors compose naturally into a signal statistics block for real-time diagnostics.

1.5.8 Connections to Other Algorithms

• RMS Envelope is structurally equivalent to the Exponential Moving Average (EMA) filter applied to \(x^2\), sharing the same one-pole recurrence.
• Zero-crossing detection is a degenerate form of edge detection in discrete sequences, related to sign-change detection in root-finding algorithms.

1.6 Decibels & Magnitude-Response Helpers

1.6.1 Overview & Motivation

Audio, filter, and control-system specifications express signal levels and filter performance in decibels (dB) because the human auditory system and most engineering metrics scale logarithmically with amplitude ratio. Specifying a stop-band attenuation of 60 dB or a pass-band ripple of 0.1 dB is natural and compact; the equivalent linear ratios (1 000 : 1 and 1.01161 : 1) are not. A small set of conversion primitives — ToDecibels, FromDecibels, and two derived helpers for attenuation and ripple — centralises this conversion and eliminates scattered, error-prone inline 20·log10 expressions throughout the rest of the library.

1.6.2 Mathematical Theory

1.6.2.1 Magnitude Decibel Conversion

For a positive amplitude ratio \(r > 0\), the equivalent level in decibels is:

\[L_{\mathrm{dB}} = 20 \log_{10}(r)\]

The factor 20 (rather than 10) reflects the voltage/pressure convention: power is proportional to the square of amplitude, so a doubling of amplitude (\(r = 2\)) gives a 6 dB increase, matching the \(10 \log_{10}(4) = 6.02\) dB power equivalent.

1.6.2.2 Inverse Conversion

\[r = 10^{L_{\mathrm{dB}}/20}\]

This inverse is exact for all finite \(L_{\mathrm{dB}}\); no guard is needed on the output side.

1.6.2.3 Zero and Negative Input Guard

\(\log_{10}(0) = -\infty\); negative ratios are physically meaningless. Both cases are mapped to a finite floor value \(L_{\min}\) chosen well below any engineering specification of interest:

\[L_{\mathrm{dB}} = \max\!\left(20\log_{10}(r),\; L_{\min}\right), \quad r > 0\] \[L_{\mathrm{dB}} = L_{\min}, \quad r \leq 0\]

A floor of \(-160\,\mathrm{dB}\) corresponds to an amplitude ratio below \(10^{-8}\), safely beyond the dynamic range of any practical floating-point computation in 32-bit single precision.

1.6.2.4 Derived Helpers

Stop-band attenuation between a pass-band ratio \(r_p\) and a stop-band ratio \(r_s\):

\[A = L_{\mathrm{dB}}(r_p) - L_{\mathrm{dB}}(r_s)\]

Pass-band ripple between the maximum and minimum in-band ratios \(r_{\max}\) and \(r_{\min}\):

\[\Delta = L_{\mathrm{dB}}(r_{\max}) - L_{\mathrm{dB}}(r_{\min})\]

Both are simple differences in decibel space, exploiting the logarithm identity \(\log(a/b) = \log a - \log b\).

1.6.3 Complexity Analysis

No state, no buffers. All operations are pure functions.

1.6.4 Step-by-Step Walkthrough

Converting a ratio of 10 to decibels:

1. Input \(r = 10\); guard passes (\(r > 0\)).
2. Compute \(20 \cdot \log_{10}(10) = 20 \cdot 1 = 20\).
3. Apply floor: \(\max(20, -160) = 20\).
4. Output: \(20\,\mathrm{dB}\).

Round-trip for \(r = 0.5\):

1. ToDecibels(0.5) = \(20 \cdot \log_{10}(0.5) \approx -6.0206\,\mathrm{dB}\).
2. FromDecibels(-6.0206) = \(10^{-6.0206/20} \approx 0.5\).

1.6.5 Pitfalls & Edge Cases

• Passing \(r = 0\) produces \(-\infty\) from log10; the floor guard prevents propagation into downstream computations.
• Negative ratios indicate a programming error (signed sample values must not be passed directly as ratios without taking absolute value first); they are silently floored rather than raising an exception, consistent with the no-exception policy.
• With fast-math enabled, the compiler may fuse or reorder floating-point operations. The log10 result is still monotone and the floor remains correct because it uses std::max, which is not reordered away.
• FromDecibels has no floor; at very large positive dB values the result overflows to +inf in float — this is expected behaviour for out-of-range inputs.

1.6.6 Variants & Generalizations

• Power decibels use \(10 \log_{10}(\cdot)\) (factor 10 rather than 20). The amplitude convention used here (\(\times 20\)) is correct for voltage, pressure, and filter transfer-function magnitude.
• Field-quantity vs. power-quantity disambiguation: IEEE 60268 / IEC 61672 mandate \(20 \log_{10}\) for sound pressure level; the same convention applies to filter magnitude response.
• The floor can be parameterised if a stricter or looser sentinel is required; the default of \(-160\,\mathrm{dB}\) is conservative for 32-bit float.

1.6.7 Applications

• Filter specification: stop-band attenuation and pass-band ripple in dB are the primary acceptance criteria for IIR/FIR designs.
• Frequency response plots: FrequencyResponse::Calculate() already returns magnitude in dB using \(20 \log_{10}\); these helpers provide the same conversion for ad-hoc analysis.
• Controller gain margin is expressed in dB; converting from a linear ratio with ToDecibels avoids duplication.
• Audio dynamic processing (compressor thresholds, limiter ceilings) and acoustic measurement both use dB natively.

1.6.8 Connections to Other Algorithms

• control_analysis::FrequencyResponse internally applies \(20 \log_{10}(\|H\|)\) on its magnitude output vector; these helpers are the scalar equivalent exposed for library consumers.
• Pass-band ripple computed by RippleDb feeds directly into filter-design acceptance testing alongside the step/transient-response metrics in the evaluation primitives family.

1.7 Goertzel Algorithm

1.7.1 Overview & Motivation

When a system needs to detect or measure a single known frequency — a DTMF touch-tone, a pilot sync tone, mains-hum, or a narrowband alarm — computing a full FFT wastes every cycle spent on bins that will never be read. The Goertzel algorithm computes exactly one DFT bin using a second-order recurrence that processes samples one at a time, requiring no input buffer and no twiddle-factor table. On the smallest microcontrollers with a few hundred bytes of RAM, this distinction is decisive.

1.7.2 Mathematical Theory

1.7.2.1 Core Definitions

Let \(x[n]\), \(n = 0, \ldots, N-1\), be a block of \(N\) real samples. The DFT coefficient at bin \(k\) is:

\[X[k] = \sum_{n=0}^{N-1} x[n]\, e^{-j2\pi kn/N}\]

Define the resonator coefficient:

\[c = 2\cos\!\left(\frac{2\pi k}{N}\right)\]

1.7.2.2 Derivation

The DFT sum can be reorganised as the output of a single-pole IIR filter evaluated at \(n = N\). Introduce the auxiliary sequence:

\[s[n] = x[n] + c\,s[n-1] - s[n-2], \quad s[-1] = s[-2] = 0\]

After \(N\) steps the DFT bin follows from the final two state values:

\[X[k] = s[N-1] - s[N-2]\,e^{-j2\pi k/N}\]

Separating real and imaginary parts:

\[\mathrm{Re}\{X[k]\} = s[N-1] - s[N-2]\cos\!\left(\tfrac{2\pi k}{N}\right)\]

\[\mathrm{Im}\{X[k]\} = s[N-2]\sin\!\left(\tfrac{2\pi k}{N}\right)\]

The magnitude can be obtained without the final trigonometric products using the identity:

\[|X[k]|^2 = s[N-1]^2 + s[N-2]^2 - c\,s[N-1]\,s[N-2]\]

so \(|X[k]| = \sqrt{s[N-1]^2 + s[N-2]^2 - c\,s[N-1]\,s[N-2]}\), saving two multiplies on the hot path.

1.7.2.3 Frequency-to-Bin Mapping

A physical frequency \(f_t\) sampled at \(f_s\) maps to bin:

\[k = \mathrm{round}\!\left(\frac{f_t}{f_s}\,N\right)\]

Frequency resolution is \(\Delta f = f_s / N\).

1.7.3 Complexity Analysis

For \(B\) bins of interest the total cost is \(O(BN)\). An FFT computing all \(N\) bins costs \(O(N\log N)\), so the Goertzel approach is cheaper when \(B \ll \log_2 N\) — typically \(B \le 4\) for a 256-point transform.

1.7.4 Step-by-Step Walkthrough

Consider \(N = 4\), \(k = 1\), \(x = [1, 0, -1, 0]\).

\(c = 2\cos(\pi/2) = 0\)

\(\cos(2\pi/4) = 0\), \(\sin(2\pi/4) = 1\)

\(\mathrm{Re}\{X[1]\} = 1 - (-1)\cdot 0 = 1\)

\(\mathrm{Im}\{X[1]\} = (-1)\cdot 1 = -1\)

Direct DFT check: \(X[1] = 1 - j\). Matches.

1.7.5 Pitfalls & Edge Cases

The recurrence pole sits exactly on the unit circle, making the filter marginally stable. Error accumulates with each block if Reset is not called between blocks. The algorithm is designed for finite, bounded blocks of exactly \(N\) samples.

Choosing \(N\) such that the target tone lands near an integer bin minimises spectral leakage. A mismatch of half a bin can reduce the apparent magnitude by several decibels. Window functions do not apply here because the Goertzel filter already selects one bin; applying a window changes the effective DFT kernel.

The DC bin (\(k = 0\)) and the Nyquist bin (\(k = N/2\)) are purely real; their imaginary output is zero by symmetry.

1.7.6 Variants & Generalizations

The magnitude-squared form \(s_1^2 + s_2^2 - c\,s_1 s_2\) avoids the square root when only power detection (threshold comparison) is needed, saving a transcendental call on platforms without hardware sqrt.

When several tones must be tracked simultaneously, one independent Goertzel instance per tone runs in \(O(BN)\) total with \(O(B)\) state — still far cheaper than an FFT for small \(B\).

A sliding Goertzel variant updates one sample at a time by subtracting the oldest sample, giving a continuous output at the cost of maintaining an input ring buffer; this trades \(O(1)\) memory for \(O(N)\) memory and is outside the scope of the no-heap constraint here.

1.7.7 Applications

• DTMF decoding (telephony, industrial keypads)
• Sync-tone and pilot-tone detection in radio links
• Mains-frequency monitoring (50/60 Hz hum)
• Single-bin power spectral estimation in embedded monitors
• Lock-in detection for narrow-band sensor readout

1.7.8 Connections to Other Algorithms

The Goertzel algorithm computes exactly the same result as one bin of the DFT computed by the Cooley-Tukey FFT (FastFourierTransform), but limits the computation to that single bin. When all \(N\) bins are required simultaneously, the FFT is always faster. PowerDensitySpectrum builds on the FFT for broadband spectral estimation. SignalDetectors provides simpler, non-frequency-selective detectors (peak hold, RMS) that cost even less per sample.

1.8 Hilbert Transform / Analytic Signal

1.8.1 Overview & Motivation

Every real-valued signal contains both positive and negative frequency components that carry redundant information. The Hilbert transform discards the negative half and pairs the original signal with a version shifted by exactly −90° at every frequency, producing a complex analytic signal whose magnitude tracks instantaneous amplitude and whose angle tracks instantaneous phase.

This is the standard technique for AM/DSB demodulation, vibration envelope detection, bearing-fault analysis, single-sideband modulation, and instantaneous-frequency measurement — all tasks where the underlying amplitude or phase varies slowly relative to a carrier and must be tracked in real time.

1.8.2 Mathematical Theory

1.8.2.1 Hilbert Transform

For a real signal \(x(t)\) the Hilbert transform is the convolution

\[\mathcal{H}\{x\}(t) = \frac{1}{\pi} \, \text{p.v.} \int_{-\infty}^{\infty} \frac{x(\tau)}{t - \tau} \, d\tau\]

which is equivalently a multiplication of each frequency component by \(-j \operatorname{sgn}(f)\), i.e. a \(-90°\) phase rotation for positive frequencies and \(+90°\) for negative.

1.8.2.2 Analytic Signal

The analytic signal is

\[z(t) = x(t) + j\,\mathcal{H}\{x\}(t)\]

Its one-sided spectrum satisfies \(Z(f) = 0\) for \(f < 0\), \(Z(0) = X(0)\), and \(Z(f) = 2X(f)\) for \(f > 0\).

1.8.2.3 Instantaneous Attributes

1.8.2.4 FFT Method (Marple)

Given the \(N\)-point DFT \(X[k]\) of a real sequence, the analytic signal is recovered by:

\[H[k] = \begin{cases} X[0] & k = 0 \\ 2X[k] & 1 \le k < N/2 \\ X[N/2] & k = N/2 \\ 0 & N/2 < k < N \end{cases}\]

followed by the inverse DFT of \(H\).

1.8.2.5 FIR Approximation

A Type III/IV antisymmetric FIR filter with impulse response

\[h[k] = \frac{2}{\pi k} w[k], \quad k \text{ odd}; \quad h[k] = 0, \quad k \text{ even}\]

(with a Hamming window \(w[k]\)) approximates the ideal \(-90°\) phase shift across its passband. The delayed original signal (center-tap copy) is paired with the filtered output to form the approximate analytic signal.

1.8.3 Complexity Analysis

\(P\) = number of FIR taps.

1.8.4 Step-by-Step Walkthrough

FFT method on \(N = 8\), input \(x = \cos(2\pi n / 8)\):

1. Forward DFT yields \(X[1] = 4\), \(X[7] = 4\), all others zero.
2. Apply one-sided weighting: \(H[1] = 8\), \(H[7] = 0\).
3. Inverse DFT of \(H\) produces the imaginary part \(\sin(2\pi n / 8)\).
4. Analytic signal: \(z[n] = \cos(2\pi n/8) + j\sin(2\pi n/8)\), amplitude \(\equiv 1\).

1.8.5 Pitfalls & Edge Cases

• DC and Nyquist bins — their imaginary parts must remain zero; the one-sided formula preserves this.
• Phase unwrapping — before computing instantaneous frequency, the phase difference must be mapped to \((-\pi, \pi]\) to suppress \(2\pi\) jumps.
• Block-edge artefacts — the FFT method treats the block as periodic; trim the first and last few samples when asserting accuracy.
• FIR group delay — the FIR output is delayed by \((P-1)/2\) samples relative to the input; align before comparing to the FFT method.
• FIR bandwidth — the FIR Hilbert approximation degrades near DC and Nyquist; use only over the filter’s flat passband.

1.8.6 Variants & Generalizations

• Block FFT method — exact, no ripple, requires a complete block; suitable for offline or buffered processing.
• FIR streaming method — causal, constant memory, approximate; length and window choice trade accuracy against latency.
• Quadrature-oscillator method — for narrowband signals, a simple IIR resonator can approximate the 90° shift with even lower cost.

1.8.7 Applications

• AM/DSB envelope demodulation and AGC.
• Vibration and bearing-fault analysis (envelope of resonance band).
• Single-sideband (SSB) radio modulation/demodulation.
• Instantaneous frequency tracking in FM receivers and Doppler radar.
• Phase and frequency estimation in PLLs.

1.8.8 Connections to Other Algorithms

• Uses FastFourierTransform / FastFourierTransformRadix2Impl as the block back-end.
• RealFastFourierTransform is an alternative back-end for purely real inputs.
• ConvolutionCorrelation provides the FIR convolution primitive used by the streaming path.
• SignalDetectors (RMS envelope, peak hold) offers cheaper non-coherent envelope estimation.

1.9 Convolution & Correlation

1.9.1 Overview & Motivation

Two signals interacting in time — one being filtered, delayed, or matched against another — are described by convolution and correlation. Convolution is the time-domain action of any linear time-invariant (LTI) filter: given an input signal and a system’s impulse response, their convolution is the output. Correlation measures the similarity between two signals as a function of relative shift (lag), making it the foundation of matched filtering, pulse detection, and time-delay estimation.

Both operations are indispensable in embedded DSP: they underlie FIR filtering, pitch detection, radar/sonar processing, and auto-regressive modeling. Bounded, no-heap implementations make these algorithms practical on microcontrollers.

1.9.2 Mathematical Theory

1.9.2.1 Linear Convolution

For finite sequences \(x\) of length \(M\) and \(h\) of length \(K\), the linear convolution \(y = x * h\) is a sequence of length \(M + K - 1\) defined by:

\[y[n] = \sum_{k=0}^{M-1} x[k] \cdot h[n - k], \quad n = 0, \ldots, M+K-2\]

where \(h[j] = 0\) for \(j < 0\) or \(j \geq K\).

1.9.2.2 Circular Convolution

For two sequences of equal length \(N\), circular (periodic) convolution wraps indices modulo \(N\):

\[y[n] = \sum_{k=0}^{N-1} x[k] \cdot h[(n - k) \bmod N], \quad n = 0, \ldots, N-1\]

Circular convolution equals linear convolution only when both operands are zero-padded to at least \(M + K - 1\) samples. Without padding, time-aliasing occurs.

1.9.2.3 Cross-Correlation

Cross-correlation of \(x\) (length \(M\)) and \(y\) (length \(K\)) is defined as:

\[r_{xy}[\ell] = \sum_{n} x[n] \cdot y[n - \ell]\]

This equals linear convolution of \(x\) with the time-reversed \(y\):

\[r_{xy} = x * \overline{y}\]

1.9.2.4 Auto-Correlation

Auto-correlation is cross-correlation of a signal with itself:

\[r_{xx}[\ell] = \sum_{n} x[n] \cdot x[n - \ell]\]

It is always symmetric about the zero-lag index, and its maximum occurs at zero lag. Dividing by \(r_{xx}[0]\) yields a normalized coefficient in \([-1, 1]\).

1.9.2.5 Convolution Theorem (Fast Path)

For long signals, the convolution theorem enables an \(O(L \log L)\) computation:

\[x * h = \mathcal{F}^{-1}\!\left(\mathcal{F}(x) \cdot \mathcal{F}(h)\right)\]

where \(\mathcal{F}\) denotes the DFT and \(L\) is the next power of two \(\geq M + K - 1\). The pointwise complex product replaces the \(O(MK)\) direct sum.

1.9.3 Complexity Analysis

The fast path is beneficial when \(MK > L \log_2 L\), roughly when both operands exceed 32–64 samples.

1.9.4 Step-by-Step Walkthrough

Linear convolution of \(x = [1, 2, 3]\) and \(h = [1, 1]\) (output length \(= 4\)):

Result: \([1, 3, 5, 3]\).

Auto-correlation of \([1, 1, 1, 1]\) produces the triangular sequence \([1, 2, 3, 4, 3, 2, 1]\), with the peak at the zero-lag center index (index 3 in 0-based notation).

1.9.5 Pitfalls & Edge Cases

• Time-aliasing: Circular convolution of length \(N\) equals linear convolution only when both operands are zero-padded to \(M + K - 1 \leq N\). Failing to pad causes energy from the tail to wrap around and corrupt the beginning of the output.
• Lag estimation: ArgMaxLag returns the index of the maximum in the cross-correlation output. For cross-correlation of two length-\(M\) sequences the zero-lag corresponds to index \(M - 1\); a delay of \(d\) samples appears at index \(M - 1 + d\).
• Normalization: The raw auto-correlation at zero lag equals the signal energy. Dividing the entire sequence by \(r[0]\) gives a coefficient bounded in \([-1, 1]\).
• Float precision: Accumulating many products in single precision can lose significance for very long sequences. The MAC loop uses the native float accumulator; for sequences longer than a few hundred samples, extended precision may be warranted.

1.9.6 Variants & Generalizations

• Normalized cross-correlation: Divide \(r_{xy}[\ell]\) by \(\sqrt{r_{xx}[0] \cdot r_{yy}[0]}\) for an amplitude-invariant similarity measure.
• Overlap-add / overlap-save: Partition long signals into short blocks and convolve each with \(h\) via the FFT fast path, then reassemble. Reduces latency for streaming applications.
• Partial correlation: Compute cross-correlation over a sliding window rather than the full signal length — useful for non-stationary signals.

1.9.7 Applications

• FIR filtering: Direct-form FIR is linear convolution of the input with the filter’s impulse response.
• Matched filtering: Cross-correlate the received signal with a known template to detect it in noise.
• Time-delay estimation (TDOA): The lag of the cross-correlation peak between two sensors gives the inter-sensor arrival time difference.
• Pitch detection: Auto-correlation of a speech frame reveals the fundamental period as the first large secondary peak.
• AR modeling: Yule-Walker equations use auto-correlation values to fit auto-regressive parameters.

1.9.8 Connections to Other Algorithms

• FastFourierTransform: The fast convolution path is a direct application of the FFT-based convolution theorem.
• GoertzelAlgorithm: A single-frequency DFT bin — cross-correlating with a complex exponential at one frequency is equivalent.
• YuleWalker: Constructs the AR system matrix from auto-correlation lags computed by AutoCorrelation.
• PowerSpectralDensity: The power spectrum is the DFT of the auto-correlation sequence (Wiener–Khinchin theorem).

2 Control Analysis

2.1 Frequency Response

2.1.1 Overview & Motivation

The frequency response \(H(e^{j\omega})\) characterizes how a discrete-time linear system (filter) modifies the amplitude and phase of each frequency component in a signal. Computing and plotting the frequency response (Bode plot) is essential for verifying filter designs, understanding system behavior, and tuning parameters. This implementation evaluates the transfer function at logarithmically spaced frequencies, producing magnitude (dB) and phase (degrees) curves.

2.1.2 Mathematical Theory

2.1.2.1 Transfer Function Evaluation

Given a filter with feedforward coefficients \(\{b_i\}_{i=0}^{P-1}\) and feedback coefficients \(\{a_i\}_{i=0}^{Q-1}\), the frequency response at angular frequency \(\omega\) is:

\[H(e^{j\omega}) = \frac{\sum_{i=0}^{P-1} b_i \cdot e^{-j\omega i}}{\sum_{i=0}^{Q-1} a_i \cdot e^{-j\omega i}}\]

For FIR filters, the denominator reduces to \(a_0 = 1\).

2.1.2.2 Magnitude and Phase

From the complex-valued response:

\[|H(e^{j\omega})| = \sqrt{\text{Re}(H)^2 + \text{Im}(H)^2}\]

\[\text{Magnitude (dB)} = 20 \log_{10} |H(e^{j\omega})|\]

\[\text{Phase (degrees)} = \frac{180}{\pi} \cdot \arg(H(e^{j\omega}))\]

2.1.2.3 Frequency Grid

Frequencies are evaluated on a logarithmic grid from \(f_s / N\) to \(f_s / 2\) (Nyquist), where \(f_s\) is the sampling frequency and \(N\) is the number of evaluation points. The angular frequency at each point is:

\[\omega_k = \frac{2\pi f_k}{f_s}\]

2.1.3 Complexity Analysis

2.1.4 Step-by-Step Walkthrough

Given a simple first-order low-pass FIR filter with \(b = [0.5, 0.5]\), \(a = [1.0]\), and \(f_s = 100\) Hz, evaluated at \(f = 10\) Hz:

1. Compute \(\omega = 2\pi \cdot 10 / 100 = 0.2\pi\)
2. Numerator: \(0.5 \cdot e^{0} + 0.5 \cdot e^{-j0.2\pi} = 0.5 + 0.5(\cos(0.2\pi) - j\sin(0.2\pi))\)
   • \(= 0.5 + 0.5(0.809 - j0.588) = 0.905 - j0.294\)
3. Denominator: \(1.0\)
4. \(|H| = \sqrt{0.905^2 + 0.294^2} = 0.952\)
5. Magnitude: \(20\log_{10}(0.952) = -0.43\) dB
6. Phase: \(\arctan(-0.294/0.905) = -18°\)

2.1.5 Pitfalls & Edge Cases

• Zero denominator: If the denominator evaluates to zero at a frequency, the implementation sets it to 1.0 to avoid division by zero. This produces a meaningful (if approximate) result near resonances.
• QNumber conversion: When coefficients are in Q15/Q31 format, they are converted to float via math::ToFloat() before evaluation. This ensures sufficient dynamic range for the complex arithmetic.
• Logarithmic spacing: Very low frequencies near DC may have sparse coverage. The lowest evaluated frequency is \(f_s / N\).
• Phase wrapping: The std::arg function returns phase in \((-\pi, \pi]\). Phase unwrapping is not performed.

2.1.6 Variants & Generalizations

• Linear frequency spacing: Alternative to logarithmic spacing for uniform frequency resolution.
• Zero-pole form: Evaluate directly from pole-zero locations rather than polynomial coefficients.
• Chirp z-transform: Generalization that evaluates the z-transform along arbitrary paths in the z-plane.

2.1.7 Applications

• Verifying filter coefficient designs match the desired specification
• Bode plot generation for stability analysis
• Comparing filter topologies (FIR vs. IIR, different orders)
• System identification validation

2.1.8 Connections to Other Algorithms

• FIR / IIR Filters: The frequency response evaluates the transfer functions implemented by Fir and Iir filters.
• FFT: The DFT of a zero-padded impulse response produces the frequency response at uniformly spaced frequencies. This class generalizes to arbitrary frequency points.
• Root Locus: Both frequency response and root locus analyze the same transfer function from complementary perspectives (frequency domain vs. pole-zero domain).
• Power Density Spectrum: PDS estimates the spectral content of a signal; frequency response characterizes the system that shapes it.

2.2 Root Locus

2.2.1 Overview & Motivation

When designing a feedback control system, a fundamental question is: how do the closed-loop poles move as I change the loop gain? The root locus answers this by plotting the trajectories of the closed-loop poles in the complex plane as a scalar gain \(K\) varies from \(K_{\min}\) to \(K_{\max}\).

This visualization immediately reveals whether increasing gain will drive the system unstable (poles crossing into the right half-plane), where oscillatory modes appear (complex pole pairs), and which gain ranges produce acceptable damping. It is one of the oldest and most intuitive tools in classical control design.

2.2.2 Mathematical Theory

2.2.2.1 Closed-Loop Characteristic Equation

Given an open-loop transfer function:

\[G(s) = K \cdot \frac{N(s)}{D(s)}\]

the closed-loop characteristic equation (for unity feedback) is:

\[D(s) + K \cdot N(s) = 0\]

The root locus is the set of all roots of this equation as \(K\) varies over \([K_{\min}, K_{\max}]\).

2.2.2.2 Key Properties

• Starting points (\(K \to 0\)): Roots begin at the open-loop poles (roots of \(D(s)\)).
• Ending points (\(K \to \infty\)): Roots converge to the open-loop zeros (roots of \(N(s)\)) or diverge to infinity along asymptotes.
• Number of branches: Equal to the order of \(D(s)\).
• Symmetry: For real-coefficient polynomials, complex roots always appear in conjugate pairs.

2.2.2.3 Gain Sweep

The gain is swept logarithmically to provide uniform resolution across decades:

\[K_i = 10^{\,\log_{10}(K_{\min}) \;+\; \frac{i}{N-1}\left(\log_{10}(K_{\max}) - \log_{10}(K_{\min})\right)}\]

where \(N\) is the number of gain steps and \(i \in [0, N-1]\).

At each gain step, the roots of \(D(s) + K_i \cdot N(s) = 0\) are found using the Durand-Kerner polynomial root-finder.

2.2.3 Complexity Analysis

Why: At each of the \(N_g\) gain steps, a degree-\(n\) polynomial is solved via Durand-Kerner, which performs \(k\) iterations each costing \(O(n^2)\) (evaluating the polynomial and computing the denominator product for all \(n\) roots).

2.2.4 Step-by-Step Walkthrough

System: \(G(s) = K \cdot \frac{s + 1}{s(s + 2)}\), sweep \(K\) from 0.01 to 10.

Step 1 — Identify poles and zeros

• Open-loop poles: \(s = 0\), \(s = -2\) (roots of \(D(s) = s^2 + 2s\))
• Open-loop zero: \(s = -1\) (root of \(N(s) = s + 1\))

Step 2 — Form characteristic polynomial at \(K = 1\)

\[D(s) + K \cdot N(s) = s^2 + 2s + 1 \cdot (s + 1) = s^2 + 3s + 1\]

Step 3 — Solve using Durand-Kerner:

\[s = \frac{-3 \pm \sqrt{9 - 4}}{2} = \frac{-3 \pm \sqrt{5}}{2} \approx -0.382,\; -2.618\]

Both poles are real and negative → system is stable at \(K = 1\).

Step 4 — Repeat for each gain step and plot all root positions in the complex plane.

Im(s)
  |
  |     × zero (-1)
--●-----×------●--> Re(s)
  0    -1     -2
 pole         pole

As \(K\) increases: the two poles approach each other on the real axis, meet between 0 and −2, then split into a complex conjugate pair. One branch eventually converges to the zero at \(s = -1\); the other diverges to \(-\infty\).

2.2.5 Pitfalls & Edge Cases

• Strictly positive gains required — the logarithmic sweep cannot handle \(K \leq 0\). For negative feedback analysis, negate the numerator.
• Proper transfer function assumed — \(\deg(D) \geq \deg(N)\). Improper transfer functions (more zeros than poles) are not supported.
• Branch tracking ambiguity — Durand-Kerner returns roots sorted by real part, not by branch identity. For smooth visualization, apply nearest-neighbor matching between consecutive gain steps.
• Clustered or repeated roots cause slower convergence in Durand-Kerner; increase the maximum iteration count if the locus appears jagged.

2.2.6 Variants & Generalizations

2.2.7 Applications

• Gain selection — Choosing the operating gain that meets damping and bandwidth specifications.
• Compensator design — Adding poles/zeros (lead/lag networks) and observing how the locus reshapes.
• Stability analysis — Determining the gain margin (gain at which the locus crosses the imaginary axis).
• Educational tool — Developing intuition about how feedback affects system dynamics.

2.2.8 Connections to Other Algorithms

graph LR
    RL["Root Locus"] --> DK["Durand-Kerner"]
    RL --> LQR["LQR (pole placement alternative)"]
    DK --> RL

2.3 Controllability and Observability

2.3.1 Overview & Motivation

Controllability and observability are fundamental structural properties of a linear dynamical system. They answer two essential design questions: can the inputs drive the state to any desired configuration, and can the initial state be inferred from the outputs alone?

A system lacking controllability has modes that no actuator can excite; a system lacking observability has modes that no sensor can detect. Both deficiencies make feedback design either impossible or degenerate. Checking these properties before attempting pole placement, LQR synthesis, or observer design prevents numerical failure and guides sensor-actuator placement.

2.3.2 Mathematical Theory

2.3.2.1 Prerequisites

Consider a discrete-time linear time-invariant system

\[x_{k+1} = A x_k + B u_k, \quad y_k = C x_k + D u_k\]

with state dimension \(n\), input dimension \(m\), and output dimension \(p\).

2.3.2.2 Controllability Matrix

The \(n \times nm\) controllability matrix is

\[\mathcal{C} = \begin{bmatrix} B & AB & A^2 B & \cdots & A^{n-1}B \end{bmatrix}\]

The pair \((A, B)\) is controllable if and only if \(\mathrm{rank}(\mathcal{C}) = n\).

2.3.2.3 Observability Matrix

The \(np \times n\) observability matrix is

\[\mathcal{O} = \begin{bmatrix} C \\ CA \\ CA^2 \\ \vdots \\ CA^{n-1} \end{bmatrix}\]

The pair \((A, C)\) is observable if and only if \(\mathrm{rank}(\mathcal{O}) = n\).

2.3.2.4 Duality

Controllability and observability are dual: \((A, B)\) is controllable if and only if \((A^\top, B^\top)\) is observable. Equivalently, \(\mathcal{C}(A, B) = \mathcal{O}(A^\top, C^\top)^\top\).

2.3.2.5 Gramians

For a Schur-stable matrix \(A\) (spectral radius \(< 1\)), the controllability and observability Gramians are the unique positive semi-definite solutions to the discrete Lyapunov equations

\[A W_c A^\top - W_c + BB^\top = 0, \quad A^\top W_o A - W_o + C^\top C = 0.\]

The Gramians encode more than rank: their eigenvalues quantify how easily each mode is excited or observed. Balanced truncation model reduction uses Gramians to identify modes that are simultaneously hard to reach and hard to see.

2.3.2.6 Rank via Gaussian Elimination

Rank is computed by Gaussian elimination with partial pivoting on a working copy of the matrix. At each step the column with the largest absolute pivot is selected among remaining rows. A pivot is counted only if its magnitude exceeds \(\tau \cdot \|M\|_\infty\) where \(\tau\) is a user-supplied tolerance.

2.3.3 Complexity Analysis

2.3.4 Step-by-Step Walkthrough

Plant: \(A = \begin{bmatrix}0 & 1 \\ -2 & -3\end{bmatrix}\), \(B = \begin{bmatrix}0 \\ 1\end{bmatrix}\), \(C = \begin{bmatrix}1 & 0\end{bmatrix}\).

Controllability matrix:

\(AB = \begin{bmatrix}0 & 1 \\ -2 & -3\end{bmatrix}\begin{bmatrix}0 \\ 1\end{bmatrix} = \begin{bmatrix}1 \\ -3\end{bmatrix}\)

\(\mathcal{C} = \begin{bmatrix}0 & 1 \\ 1 & -3\end{bmatrix}\), \(\det(\mathcal{C}) = -1 \neq 0\), rank \(= 2\). Controllable.

Observability matrix:

\(CA = \begin{bmatrix}1 & 0\end{bmatrix}\begin{bmatrix}0 & 1 \\ -2 & -3\end{bmatrix} = \begin{bmatrix}0 & 1\end{bmatrix}\)

\(\mathcal{O} = \begin{bmatrix}1 & 0 \\ 0 & 1\end{bmatrix}\), rank \(= 2\). Observable.

2.3.5 Pitfalls & Edge Cases

• Numerical rank: The binary controllable/observable answer is sensitive to the tolerance \(\tau\). Near-rank-deficient systems (nearly uncontrollable modes) may flip with finite-precision arithmetic. Gramian eigenvalue magnitudes provide a graded measure.
• Unstable A for Gramians: The fixed-point iteration \(X_{k+1} = AXA^\top + Q\) converges only when \(\rho(A) < 1\) (Schur-stable). If \(A\) has eigenvalues on or outside the unit disk the iteration diverges and a zero matrix is returned.
• Wide controllability matrix: When \(m > 1\) the controllability matrix has more columns than rows; rank can be full even with fewer than \(n\) block columns contributing independent directions.
• Repeated eigenvalues: A matrix with repeated eigenvalues can still be controllable; the Cayley-Hamilton theorem guarantees that \(n\) block columns always suffice, but the matrix may be close to rank-deficient.

2.3.6 Variants & Generalizations

• Continuous-time Gramians: satisfy \(AW + WA^\top + BB^\top = 0\) and require a different solver (e.g., Bartels-Stewart or Hammarling).
• PBH test: The eigenvector-based Popov-Belevitch-Hautus test checks \(\mathrm{rank}[A - \lambda I \;|\; B] = n\) for each eigenvalue \(\lambda\), revealing which modes are uncontrollable.
• Balanced realization: Simultaneously diagonalizes \(W_c\) and \(W_o\) via similarity transformation; the resulting Hankel singular values rank-order modes by joint excitability and observability.
• Stochastic observability: Extends to systems driven by process noise; replaced by reachability Gramians.

2.3.7 Applications

• Pre-synthesis checks before LQR, pole placement, or Luenberger observer design.
• Structural sensor/actuator placement: choose locations that maximize the minimum Gramian eigenvalue.
• Model order reduction: balanced truncation discards modes with small Hankel singular values.
• Fault detection: loss of observability of a mode may indicate sensor failure.

2.3.8 Connections to Other Algorithms

• LQR / DARE: LQR requires controllability of \((A, B)\) and observability of \((A, Q^{1/2})\) for a finite-cost solution.
• Luenberger Observer: observer gain existence requires observability of \((A, C)\).
• Lyapunov solver (item 31): provides an exact algebraic solver for the Gramian equations, superseding the fixed-point iteration used here.
• Balanced truncation: uses both Gramians to compute Hankel singular values for model reduction.

2.4 Continuous-to-Discrete Conversion (c2d)

2.4.1 Overview & Motivation

Physical plants and classical controllers are designed in continuous time using differential equations and the Laplace transform. Microcontrollers, however, execute discrete update loops at a fixed sampling period \(T_s\). Continuous-to-discrete conversion bridges the two worlds: it takes a continuous-time state-space model \((A, B, C, D)\) and produces the equivalent discrete-time model \((A_d, B_d, C_d, D_d)\) that a sampled-data controller can execute directly.

Choosing the right discretization method is critical. A poor choice can push stable poles outside the unit disk, introduce spurious resonances, or distort the DC gain — all of which degrade closed-loop performance or cause instability on hardware.

2.4.2 Mathematical Theory

2.4.2.1 State-Space Representation

A continuous-time LTI system obeys

\[\dot{x}(t) = A\,x(t) + B\,u(t), \qquad y(t) = C\,x(t) + D\,u(t)\]

where \(x \in \mathbb{R}^n\) is the state, \(u \in \mathbb{R}^m\) the input, and \(y \in \mathbb{R}^p\) the output. The goal is to find \((A_d, B_d, C_d, D_d)\) such that

\[x_{k+1} = A_d\,x_k + B_d\,u_k, \qquad y_k = C_d\,x_k + D_d\,u_k\]

matches the continuous solution at sample instants \(t_k = k\,T_s\).

2.4.2.2 Zero-Order Hold (Van Loan’s Trick)

Assuming the input is held constant between samples (true for a DAC or PWM), the exact solution is

\[A_d = e^{A T_s}, \qquad B_d = \left(\int_0^{T_s} e^{A\tau}\,d\tau\right) B\]

Van Loan (1978) observed that both quantities emerge from a single matrix exponential of the augmented \((n+m) \times (n+m)\) block matrix:

\[M = \begin{bmatrix} A & B \\ 0 & 0 \end{bmatrix} T_s, \qquad e^M = \begin{bmatrix} A_d & B_d \\ 0 & I \end{bmatrix}\]

The upper-left \(n\times n\) block is \(A_d\); the upper-right \(n\times m\) block is \(B_d\). \(C\) and \(D\) are unchanged under ZOH because the output equation is instantaneous.

2.4.2.3 Tustin / Bilinear Transformation

The Tustin method replaces the Laplace variable via

\[s \;\longleftarrow\; \frac{2}{T_s}\,\frac{z-1}{z+1}\]

Setting \(\alpha = 2/T_s\) and \(P = (\alpha I - A)^{-1}\):

\[A_d = P(\alpha I + A), \quad B_d = 2P\,B, \quad C_d = \alpha\,C\,P, \quad D_d = D + C\,P\,B\]

Tustin maps the entire open left-half plane into the open unit disk, so it is stability-preserving by construction. It is the preferred method for discretizing controllers and filters when a specific corner frequency must be matched (after frequency prewarping).

2.4.2.4 Forward Euler

The cheapest approximation — first-order rectangle rule:

\[A_d = I + A\,T_s, \quad B_d = B\,T_s, \quad C_d = C, \quad D_d = D\]

Conditionally stable: poles may leave the unit disk when \(T_s\) is too large relative to the fastest mode.

2.4.2.5 Backward Euler

Implicit first-order rule, equivalent to the \(s \leftarrow (z-1)/(z\,T_s)\) substitution. Setting \(P = (I - A\,T_s)^{-1}\):

\[A_d = P, \quad B_d = P\,B\,T_s, \quad C_d = C\,P, \quad D_d = D + C\,P\,B\,T_s\]

Unconditionally stable (maps the left-half plane into the unit disk) but introduces phase lag relative to the true system.

2.4.3 Complexity Analysis

All working memory is allocated on the stack; no heap is used.

2.4.4 Step-by-Step Walkthrough

Consider a scalar integrator \(\dot{x} = u\), so \(A=0\), \(B=1\), \(C=1\), \(D=0\), with \(T_s = 0.1\).

ZOH:

\[M = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \times 0.1 = \begin{bmatrix} 0 & 0.1 \\ 0 & 0 \end{bmatrix}\]

\[e^M = I + M = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}\]

So \(A_d = 1\), \(B_d = 0.1\) — the integrator accumulates \(u \cdot T_s\) each step.

Tustin (\(\alpha = 20\)):

\[P = (20 - 0)^{-1} = 0.05\]

\[A_d = 0.05 \times 20 = 1, \quad B_d = 2 \times 0.05 = 0.1\]

Both methods agree exactly for a pure integrator, as the bilinear transform maps the pole at \(s=0\) to \(z=1\) without distortion.

2.4.5 Pitfalls & Edge Cases

• Singular matrix — Tustin and Backward Euler require solving \((\alpha I - A)\) and \((I - A\,T_s)\), respectively. If \(A\) has an eigenvalue exactly at \(\alpha\) (Tustin) or \(1/T_s\) (Backward Euler), the system matrix is singular. The LU factorisation will flag degeneracy; the caller should validate the sample rate.
• Large \(T_s\) with Forward Euler — For a mode with time constant \(\tau\), Forward Euler is stable only when \(T_s < 2\tau\). Exceeding this causes poles to flip outside the unit circle and the discrete simulation diverges.
• Frequency warping (Tustin) — The Tustin transform compresses the entire imaginary axis into the unit circle, warping frequencies above \(\pi/T_s\). To match a specific analogue frequency \(\omega_0\), prewarp: use \(\alpha = \omega_0 / \tan(\omega_0 T_s / 2)\) instead of \(2/T_s\).
• Numerical conditioning — For systems with eigenvalues spanning many decades, the augmented matrix exponential (ZOH) may suffer cancellation. Scaling the system before discretisation improves accuracy.

2.4.6 Variants & Generalizations

• Frequency prewarping — Modify \(\alpha = \omega_c / \tan(\omega_c T_s / 2)\) before Tustin to preserve the gain at a critical frequency \(\omega_c\) exactly.
• Generalised hold functions — Higher-order holds (first-order hold, triangular hold) modify the ZOH integral; each requires its own augmented block.
• Sampled-data \(H_\infty\) — Discretisation under a minimax criterion rather than exact ZOH; accounts for inter-sample behaviour explicitly.

2.4.7 Applications

• Embedded control — Convert a continuous PID or LQR design to a discrete update law running at a fixed ISR rate.
• Digital filter design — Convert an analogue prototype (Butterworth, Chebyshev) to a discrete IIR filter using Tustin with prewarping.
• Simulation — ZOH discretisation yields exact step-response simulation for piecewise- constant inputs, enabling hardware-in-the-loop testing without numerical ODE integration.

2.4.8 Connections to Other Algorithms

• MatrixExponential — The ZOH method is entirely built on expm of the Van Loan augmented block; the exponential accuracy directly sets the ZOH accuracy.
• LinearTimeInvariant — Both input and output are LinearTimeInvariant structs; the discrete model drops straight into the state-update loop.
• LU decomposition / Gaussian elimination — Tustin and Backward Euler need one \(n \times n\) factorisation each; an inline partial-pivoting LU is used to keep the module self-contained.

2.5 Transfer-Function ↔︎ State-Space Conversion

2.5.1 Overview & Motivation

Classical control design and modern state-space methods speak different mathematical languages. Frequency-domain tools — Bode plots, root locus, PID/lead-lag synthesis — operate on transfer functions, ratios of polynomials in the Laplace variable \(s\). Modern methods — LQR, observers, Kalman filters — require the state-space quadruple \((A, B, C, D)\). This converter is the bridge. A compensator designed in the frequency domain can be dropped into a state-space runtime without hand-derivation, and a state-space plant model can be lifted into the frequency domain for classical analysis.

2.5.2 Mathematical Theory

2.5.2.1 Transfer Function

A SISO rational transfer function is

\[H(s) = \frac{b_0 s^n + b_1 s^{n-1} + \cdots + b_n}{a_0 s^n + a_1 s^{n-1} + \cdots + a_n}\]

Normalising by \(a_0\) produces the monic denominator \(s^n + \hat{a}_1 s^{n-1} + \cdots + \hat{a}_n\). When \(\deg(\text{num}) = \deg(\text{den})\), a polynomial division peels off the direct feed-through scalar \(D = b_0/a_0\), leaving a strictly proper remainder.

2.5.2.2 Controllable Canonical Form

The monic denominator maps directly to the companion (controllable canonical) matrix

\[A = \begin{bmatrix} 0 & 1 & 0 & \cdots & 0 \\ 0 & 0 & 1 & \cdots & 0 \\ \vdots & & & \ddots & \vdots \\ -\hat{a}_n & -\hat{a}_{n-1} & \cdots & & -\hat{a}_1 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ \vdots \\ 0 \\ 1 \end{bmatrix}\]

The output row \(C = \begin{bmatrix} \hat{b}_n' & \cdots & \hat{b}_1' \end{bmatrix}\) is formed from the strictly-proper remainder coefficients \(\hat{b}_i'\) after the feed-through split.

2.5.2.3 Observable Canonical Form

The observable canonical form is the algebraic dual of the controllable form:

\[A_\text{ocf} = A_\text{ccf}^\top, \quad B_\text{ocf} = C_\text{ccf}^\top, \quad C_\text{ocf} = B_\text{ccf}^\top, \quad D_\text{ocf} = D_\text{ccf}\]

Both realisations represent the same input-output map and share identical transfer functions.

2.5.2.4 State-Space to Transfer Function: Faddeev–Le Verrier

Given \((A, B, C, D)\), the transfer function is recovered via

\[H(s) = C\,(sI - A)^{-1} B + D = \frac{C\,\text{adj}(sI - A)\,B + D\,\det(sI - A)}{\det(sI - A)}\]

The Faddeev–Le Verrier algorithm computes the characteristic polynomial \(\det(sI - A) = s^n + c_1 s^{n-1} + \cdots + c_n\) and the adjugate action \(\text{adj}(sI - A)B\) in a single recursion of \(n\) steps:

\[M_0 = I, \quad c_k = -\frac{1}{k}\mathrm{tr}(A M_{k-1}), \quad M_k = A M_{k-1} + c_k I\]

The numerator coefficient for degree \(n - k\) is \(C M_k B\).

2.5.3 Complexity Analysis

Memory is \(O(n^2)\) for \(A\) and \(O(n)\) for coefficient arrays; all allocations are std::array on the stack.

2.5.4 Step-by-Step Walkthrough

\(H(s) = \dfrac{s + 2}{s^2 + 3s + 2}\), so \(n = 2\), denominator \([1, 3, 2]\), numerator \([0, 1, 2]\).

Monic normalisation: already monic; \(\hat{a}_1 = 3\), \(\hat{a}_2 = 2\).

Feed-through split: \(b_0 = 0\), so \(D = 0\) and \(\hat{b}' = [0, 1, 2]\).

Companion matrix:

\[A = \begin{bmatrix} 0 & 1 \\ -2 & -3 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}, \quad C = \begin{bmatrix} 2 & 1 \end{bmatrix}\]

Observable form:

\[A_\text{ocf} = \begin{bmatrix} 0 & -2 \\ 1 & -3 \end{bmatrix}, \quad B_\text{ocf} = \begin{bmatrix} 2 \\ 1 \end{bmatrix}, \quad C_\text{ocf} = \begin{bmatrix} 0 & 1 \end{bmatrix}\]

Round-trip verification via Le Verrier (\(n = 2\)):

• \(M_0 = I\); \(c_1 = -\tfrac{1}{1}\mathrm{tr}(AM_0) = -\mathrm{tr}(A) = 3\); \(M_1 = A + 3I = \begin{bmatrix}3 & 1 \\ -2 & 0\end{bmatrix}\).
• \(c_2 = -\tfrac{1}{2}\mathrm{tr}(AM_1) = -\tfrac{1}{2}\mathrm{tr}\!\begin{bmatrix}-2 & 0 \\ -6 & -2\end{bmatrix} = 2\).
• Numerator coefficients: \(CM_1B = [2,1]\begin{bmatrix}1\\0\end{bmatrix} = 2\); \(CM_2B = CM_0B = 0\).

Result: \(H(s) = \dfrac{s + 2}{s^2 + 3s + 2}\). Identical to the original.

2.5.5 Pitfalls & Edge Cases

• Near-zero leading denominator coefficient: the normalisation step divides by \(a_0\); a guard is required to avoid division by zero.
• Non-minimal realisation: if numerator and denominator share a common factor (pole-zero cancellation), the realisation is valid but is not minimal — it is either uncontrollable or unobservable. The conversion does not detect or cancel common factors.
• Le Verrier conditioning: the recursion accumulates floating-point errors in \(O(n^4)\) operations. For large \(n\) the recovered transfer function coefficients degrade. Keep system orders to the small values typical on an MCU.
• Proper vs strictly proper: when \(\deg(\text{num}) < \deg(\text{den})\) the feed-through term \(D = 0\) and the split step is a no-op. No special branch is required; the leading numerator coefficient being zero makes \(D = 0\) automatically.

2.5.6 Variants & Generalizations

• Phase-variable form: an alternative state numbering; identical to controllable canonical form up to index reversal.
• Modal canonical form: diagonalises \(A\); numerically sensitive but decouples modes for analysis.
• Balanced realisation: minimises the condition number jointly; requires Gramian computation and similarity transformation.
• Continuous-time vs discrete-time: the companion structure is identical; only the interpretation of \(s\) vs \(z\) changes.

2.5.7 Applications

• Bridging frequency-domain compensator design to the LinearTimeInvariant runtime for embedded execution.
• Computing frequency response or root locus from a state-space plant model.
• Initialising Kalman filter or LQR designs from a transfer-function spec.
• Verifying a state-space model by recovering and inspecting its transfer function.

2.5.8 Connections to Other Algorithms

• LinearTimeInvariant: the target realisation type; canonical forms drop directly into its Step/Output interface.
• ControllabilityObservability: a canonical realisation is minimal if and only if it is both controllable and observable; use these checks after conversion.
• FrequencyResponse / RootLocus: classical analyses that accept transfer-function coefficients; feed the recovered transfer function from ToTransferFunction directly.
• DurandKerner: factors the denominator polynomial into its roots (poles); combine with this converter for pole-zero plots.

3 Controllers

3.1 Bang-Bang / Hysteresis (Relay) Controller

3.1.1 Overview & Motivation

The bang-bang controller is the simplest closed-loop regulator: its output switches between two discrete levels depending on whether the controlled variable is above or below a threshold. Without hysteresis, a plain threshold comparator chatters — switching at high frequency whenever noise nudges the signal across the boundary.

Adding a dead-band (Schmitt trigger) eliminates chatter by giving the relay memory: once the output goes High it stays High until the measurement falls all the way to a lower threshold, and vice versa. This makes the relay a practical actuator-safe control primitive for thermostats, fridge compressors, tank level switches, and power-stage on/off regulation.

3.1.2 Mathematical Theory

3.1.2.1 State Transition

The relay holds a binary state \(s \in \{\text{Low}, \text{High}\}\) with the following transition rules:

\[ s[k] = \begin{cases} \text{High} & \text{if } s[k-1] = \text{Low} \text{ and } x[k] \geq \theta_H \\ \text{Low} & \text{if } s[k-1] = \text{High} \text{ and } x[k] \leq \theta_L \\ s[k-1] & \text{otherwise} \end{cases} \]

where \(\theta_L < \theta_H\) are the lower and upper switching thresholds (the hysteresis band).

3.1.2.2 Output Map

\[ u[k] = \begin{cases} u_H & \text{if } s[k] = \text{High} \\ u_L & \text{if } s[k] = \text{Low} \end{cases} \]

The output levels \(u_L\) and \(u_H\) are arbitrary; common choices are \(\{0, 1\}\) or \(\{-1, +1\}\).

3.1.2.3 Hysteresis Band Width

The band width \(\Delta = \theta_H - \theta_L\) is the key design parameter. It bounds the switching frequency \(f_s\) given a signal slope \(\dot{x}\):

\[ f_s \leq \frac{|\dot{x}|}{2\Delta} \]

A wider band reduces \(f_s\) (protecting relays and power stages) at the cost of a larger steady-state limit cycle amplitude.

3.1.3 Complexity Analysis

No arithmetic on the signal path — only comparisons — so the relay introduces no numerical error and is exactly representable in any floating-point format.

3.1.4 Step-by-Step Walkthrough

Setup: band \([\theta_L, \theta_H] = [-0.2, 0.2]\), outputs \(u_L = 0\), \(u_H = 1\), initial state Low.

3.1.5 Pitfalls & Edge Cases

• Inverted band. \(\theta_H \leq \theta_L\) latches the output in an undefined state; reject this at construction time via a precondition assertion.
• Exactly on threshold. Transitions are inclusive: \(x = \theta_H\) triggers Low→High and \(x = \theta_L\) triggers High→Low. This avoids a dead-zone at the switching points.
• Zero-width band. \(\theta_H = \theta_L\) collapses the relay to a pure comparator (no hysteresis). The logic is still correct but offers no chatter suppression.
• Noise sizing. The band width must exceed the peak-to-peak noise amplitude; otherwise noise alone drives state transitions at the sampling rate.
• Actuator minimum on-time. Size \(\Delta\) so that the minimum on-time (derived from \(\Delta / |\dot{x}|_\text{max}\)) is above the actuator’s rated minimum switching period.

3.1.6 Variants & Generalizations

3.1.7 Applications

• Thermostats and HVAC — heating/cooling switched on/off around a temperature set-point.
• Tank and vessel level control — pump on/off between high- and low-level floats.
• Power-stage converters — hysteretic current-mode control in DC-DC converters and class-D amplifiers.
• Motor drive enable/disable — protecting power stages with a current-band relay.
• Åström–Hägglund auto-tuning — the relay feedback experiment that drives limit-cycle oscillation for PID parameter identification.

3.1.8 Connections to Other Algorithms

3.2 PID Controller

3.2.1 Overview & Motivation

The Proportional-Integral-Derivative (PID) controller is the workhorse of industrial feedback control. It continuously computes an error \(e(t)\) — the difference between a desired setpoint and a measured process variable — and applies a correction based on three terms: one proportional to the current error, one to its accumulated history, and one to its rate of change.

Despite its simplicity, PID handles a remarkably wide range of plants — from temperature regulation to motor speed control — because each term addresses a different aspect of the transient response:

• P reacts immediately to the current error.
• I eliminates steady-state offset by integrating past errors.
• D anticipates future error by acting on the rate of change.

This library implements a discrete recursive form suitable for fixed-sample-rate embedded loops, where the output is computed incrementally rather than from scratch at every step.

3.2.2 Mathematical Theory

3.2.2.1 Continuous-Time PID

\[u(t) = K_p \, e(t) + K_i \int_0^t e(\tau)\,d\tau + K_d \frac{de(t)}{dt}\]

3.2.2.2 Discrete Recursive Form

Discretizing with sampling period \(T_s\) and applying the difference approximation:

\[u[n] = u[n-1] + a_0 \, e[n] + a_1 \, e[n-1] + a_2 \, e[n-2]\]

where:

\[a_0 = K_p + K_i + K_d\] \[a_1 = -(K_p + 2K_d)\] \[a_2 = K_d\]

This incremental (velocity) form has two key advantages:

1. No integral accumulator — the integral action is implicit in the recursion, reducing overflow risk.
2. Bumpless mode switching — toggling between auto/manual mode does not cause output jumps because the output is updated incrementally.

3.2.2.3 Output Saturation

The output is clamped to \([u_{\min}, u_{\max}]\) after each computation, which also provides implicit anti-windup: the integral term cannot drive the output beyond the saturation limits.

3.2.3 Complexity Analysis

The algorithm uses a fixed-size recursive buffer storing \(e[n-1]\), \(e[n-2]\), and \(u[n-1]\). No loops, no dynamic allocation, fully deterministic execution time.

3.2.4 Step-by-Step Walkthrough

Parameters: \(K_p = 2\), \(K_i = 0.5\), \(K_d = 0.1\), limits \([-10, 10]\), setpoint \(= 5\).

Coefficients: - \(a_0 = 2 + 0.5 + 0.1 = 2.6\) - \(a_1 = -(2 + 0.2) = -2.2\) - \(a_2 = 0.1\)

The output converges toward the steady-state value needed to maintain the setpoint.

3.2.5 Pitfalls & Edge Cases

• Derivative kick. A sudden change in setpoint causes a spike in \(e[n] - e[n-1]\). Mitigate by differentiating the process variable instead of the error, or by filtering the derivative term.
• Integral windup. Although the recursive form provides implicit anti-windup through output clamping, very aggressive \(K_i\) values can still cause sluggish recovery from saturation. Monitor the output rail time.
• Sample rate dependency. The gains \(K_i\) and \(K_d\) are implicitly scaled by \(T_s\). Changing the control loop rate without retuning will alter the effective integral and derivative contributions.
• Fixed-point saturation. For Q15/Q31 types, the intermediate products \(a_0 \cdot e[n]\) etc. can overflow. Ensure gains and error ranges are scaled to stay within the representable range.
• Setpoint jumps. Call Reset() after large setpoint changes to clear stale error history.

3.2.6 Variants & Generalizations

3.2.7 Applications

• Temperature control — Maintaining oven, room, or process temperatures via heater/cooler actuation.
• Motor speed/position control — Servo drives, robotics joints, CNC machines.
• Flow and pressure regulation — Industrial process control (chemical plants, water treatment).
• Voltage/current regulation — Power supply output regulation, battery charging.
• Attitude control — Drone stabilization, satellite pointing.

3.2.8 Connections to Other Algorithms

graph LR
    PID["PID Controller"]
    LQR["LQR Controller"]
    KF["Kalman Filter"]
    PID -.->|"alternative"| LQR
    KF -->|"state estimate"| LQR
    KF -->|"filtered measurement"| PID

3.3 LQR Controller (Linear Quadratic Regulator)

3.3.1 Overview & Motivation

When a state-space model of the plant is available, the Linear Quadratic Regulator provides the mathematically optimal state-feedback gain. Instead of manually tuning three knobs (as with PID), LQR asks a more principled question: given how much I care about state deviation versus control effort, what is the cheapest way to drive the state to zero?

The answer is a constant gain matrix \(K\) such that the control law \(u[k] = -Kx[k]\) minimizes an infinite-horizon quadratic cost. The elegance of LQR is that this problem has a closed-form solution through the Discrete Algebraic Riccati Equation (DARE).

For embedded systems, the gain \(K\) can be computed offline and stored as a constant — the runtime cost is then a single matrix-vector multiplication per control step.

3.3.2 Mathematical Theory

3.3.2.1 Discrete-Time State-Space System

\[x[k+1] = Ax[k] + Bu[k]\]

where \(x \in \mathbb{R}^n\) is the state, \(u \in \mathbb{R}^m\) is the control input, \(A\) is the state transition matrix, and \(B\) is the input matrix.

3.3.2.2 Cost Function

The LQR minimizes the infinite-horizon quadratic cost:

\[J = \sum_{k=0}^{\infty} \left( x[k]^T Q \, x[k] + u[k]^T R \, u[k] \right)\]

where: - \(Q \succeq 0\) (positive semi-definite) penalizes state deviation. - \(R \succ 0\) (positive definite) penalizes control effort.

3.3.2.3 Optimal Gain via DARE

The solution requires finding the matrix \(P\) that satisfies the Discrete Algebraic Riccati Equation:

\[P = A^T P A - A^T P B \left(R + B^T P B\right)^{-1} B^T P A + Q\]

The optimal gain is then:

\[K = \left(R + B^T P B\right)^{-1} B^T P A\]

and the optimal control law is \(u[k] = -Kx[k]\).

3.3.2.4 Existence and Uniqueness

A unique stabilizing solution \(P\) exists when: 1. \((A, B)\) is stabilizable (all unstable modes are controllable). 2. \((A, C)\) is detectable (where \(Q = C^T C\)).

3.3.2.5 Bryson’s Rule (Initial Weight Selection)

\[Q_{ii} = \frac{1}{(\text{max acceptable } x_i)^2}, \qquad R_{jj} = \frac{1}{(\text{max acceptable } u_j)^2}\]

3.3.3 Complexity Analysis

The DARE iteration is the expensive part, but it is done once (offline or at initialization). The per-sample cost is just a matrix-vector product.

3.3.4 Step-by-Step Walkthrough

System: Double integrator with \(\Delta t = 0.01\)

\[A = \begin{bmatrix} 1 & 0.01 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0.00005 \\ 0.01 \end{bmatrix}\]

\[Q = \begin{bmatrix} 100 & 0 \\ 0 & 1 \end{bmatrix}, \quad R = [1]\]

Step 1 — Initialize \(P_0 = Q\)

Step 2 — DARE iteration (showing first iteration):

1. \(S = R + B^T P_0 B = 1 + [0.00005\; 0.01] \begin{bmatrix}100 & 0\\0 & 1\end{bmatrix} \begin{bmatrix}0.00005\\0.01\end{bmatrix} = 1.0001\)
2. Solve \(S \cdot K_{\text{part}} = B^T P_0 A\) via Gaussian elimination
3. Update \(P_1 = A^T P_0 A - A^T P_0 B \cdot K_{\text{part}} + Q\)
4. Check convergence: \(\|P_1 - P_0\|\)

Step 3 — Repeat until \(\|P_{k+1} - P_k\| < \varepsilon\) (typically 10–30 iterations).

Step 4 — Extract gain \(K = (R + B^T P B)^{-1} B^T P A\)

Runtime: \(u[k] = -K x[k]\) — a \(1 \times 2\) times \(2 \times 1\) multiply per sample.

3.3.5 Pitfalls & Edge Cases

• Uncontrollable modes. If \((A, B)\) is not stabilizable, the DARE has no stabilizing solution. Verify controllability before calling the solver.
• Ill-conditioned \(R\). If \(R\) is near-singular, the inversion in \(K\) becomes numerically unstable. Ensure \(R\) is strictly positive definite.
• Q/R scaling. Only the ratio of \(Q\) to \(R\) matters. Scaling both by the same factor does not change \(K\).
• Full state required. LQR assumes all states are measured. If only partial measurements are available, combine LQR with a Kalman Filter to form an LQG controller.
• Fixed-point limitations. The DARE involves matrix inversions and multiplications that can overflow Q15/Q31 ranges. Prefer floating-point for the offline solve; use the pre-computed \(K\) constructor for fixed-point runtime.

3.3.6 Variants & Generalizations

3.3.7 Applications

• Cart-pole balancing — Classic underactuated system; LQR linearized around the upright equilibrium.
• Quadrotor attitude control — Regulating roll, pitch, yaw with motor thrust as input.
• Satellite station-keeping — Maintaining orbital position with minimal thruster fuel.
• Industrial process control — Multi-input multi-output (MIMO) systems where PID struggles.
• Autonomous vehicles — Lateral and longitudinal control using a linearized vehicle model.

3.3.8 Connections to Other Algorithms

graph LR
    LQR["LQR Controller"]
    DARE["DARE Solver"]
    GE["Gaussian Elimination"]
    KF["Kalman Filter"]
    PID["PID Controller"]
    DARE --> LQR
    GE --> DARE
    KF -->|"state estimate"| LQR
    PID -.->|"model-free alternative"| LQR

3.4 Integral State Feedback (LQI / Servo)

3.4.1 Overview & Motivation

Plain LQR drives the system state toward the origin but leaves a persistent steady-state offset when a constant reference or disturbance is present. The integral state feedback controller (LQI, or servo LQR) removes this offset by augmenting the plant with integrators on the tracking error. A single LQR solve on the augmented system produces two gains: one that acts on the physical states and one that closes the integral loop, guaranteeing zero steady-state error to constant references with no manual trim.

3.4.2 Mathematical Theory

3.4.2.1 Plant Model

The discrete-time plant is

\[x_{k+1} = A x_k + B u_k, \quad y_k = C x_k\]

with state \(x \in \mathbb{R}^n\), input \(u \in \mathbb{R}^m\), and tracked output \(y \in \mathbb{R}^p\).

3.4.2.2 Augmented Plant

Define the integral-of-error state \(x_i \in \mathbb{R}^p\):

\[x_{i,k+1} = x_{i,k} + (r_k - y_k) T_s\]

Stacking \(\xi = [x^\top \; x_i^\top]^\top\) gives the augmented system

\[\xi_{k+1} = A_a \xi_k + B_a u_k + E_a r_k\]

\[A_a = \begin{bmatrix} A & 0 \\ -C T_s & I \end{bmatrix}, \quad B_a = \begin{bmatrix} B \\ 0 \end{bmatrix}, \quad E_a = \begin{bmatrix} 0 \\ T_s I \end{bmatrix}\]

3.4.2.3 LQR Design on the Augmented Plant

Minimise the infinite-horizon quadratic cost

\[J = \sum_{k=0}^{\infty} \bigl(\xi_k^\top Q \xi_k + u_k^\top R u_k\bigr)\]

by solving the Discrete Algebraic Riccati Equation (DARE) for \(P\):

\[P = A_a^\top P A_a - A_a^\top P B_a (R + B_a^\top P B_a)^{-1} B_a^\top P A_a + Q\]

The optimal gain partitions as \(K_a = [K_x \mid K_i]\) where \(K_x \in \mathbb{R}^{m \times n}\) acts on the physical states and \(K_i \in \mathbb{R}^{m \times p}\) acts on the integral states.

3.4.2.4 Control Law

\[u_k = -K_x x_k - K_i x_{i,k}\]

At equilibrium \(r - y = 0\), so \(x_{i}\) stops changing, and the control law holds \(y = r\) exactly.

3.4.3 Complexity Analysis

Design is a one-time offline computation. The per-sample update cost is dominated by the two gain-state products and the integral accumulation.

3.4.4 Step-by-Step Walkthrough

Consider a scalar plant (\(n=1\), \(m=1\), \(p=1\), \(T_s = 0.01\)):

1. Form \(A_a\) (2×2), \(B_a\) (2×1) from plant matrices and \(T_s\).
2. Choose \(Q\) (2×2 diagonal) and \(R\) (scalar) to weight states and input.
3. Solve DARE → \(P\) (2×2) → \(K_a = [k_x \; k_i]\) (1×2).
4. Per step: accumulate \(x_i \mathrel{+}= (r - y) T_s\), output \(u = -k_x x - k_i x_i\).
5. After ~200 steps the output converges to \(r\) within numerical tolerance.

3.4.5 Pitfalls & Edge Cases

• Integral windup: when the actuator saturates, the integral keeps growing because the control cannot reach the demanded value. Apply a clamp on \(x_i\) or back-calculate (anti-windup) to prevent divergence after the saturation clears.
• Marginally stable plant: a plant with an open-loop integrator combined with the error integrator yields a double-integrator augmented system. The DARE still converges if the augmented pair \((A_a, B_a)\) is stabilisable; verify that condition before deploying.
• Slow integral weighting: under-weighting the integral state in \(Q\) allows steady-state error to persist for many steps before correcting; over-weighting causes overshoot.
• Sample-time mismatch: the discrete integral \(x_i\) accumulates \(T_s\)-scaled errors. Using the wrong \(T_s\) at run time shifts the effective integral gain and breaks zero-error convergence.

3.4.6 Variants & Generalizations

• Continuous-time LQI: replace the discrete integrator with \(\dot{x}_i = r - y\) and solve the continuous ARE.
• Output-feedback LQI (LQGI): combine with a Kalman filter when only the output (not the full state) is measurable — the separation principle still holds.
• Anti-windup: add a saturation block on \(x_i\) with back-calculation to recover from actuator limits without integral drift.
• Multiple outputs: the design extends directly to \(p > 1\) by stacking \(p\) integral states.

3.4.7 Applications

• Motor position/speed servo: eliminate gravity or friction offsets without manual trim.
• Process control: temperature or pressure regulation with constant load disturbances.
• Aerospace attitude control: integral action compensates for persistent aerodynamic moments.
• Robotics: joint torque control with payload-induced constant forces.

3.4.8 Connections to Other Algorithms

• Lqr: the base regulator; LQI delegates the DARE solve to Lqr on the augmented plant.
• DiscreteAlgebraicRiccatiEquation: the inner solver used by Lqr.
• Lqg: pairs Lqr with a Kalman filter; LQI could similarly pair with Lqg for output-feedback servo control.
• Pid: the integral channel of a PID is the scalar, single-output analogue of \(K_i x_i\).
• SaturationRateLimiter: provides output clamping for anti-windup on the LQI integral.

3.5 Linear Quadratic Gaussian (LQG) Controller

3.5.1 Overview & Motivation

The Linear Quadratic Gaussian (LQG) controller combines two classical optimal control results: the Linear Quadratic Regulator (LQR) and the Kalman Filter (KF). It is the optimal output-feedback controller for linear time-invariant (LTI) systems subject to Gaussian process and measurement noise.

While LQR assumes full state availability — a strict requirement rarely satisfied in practice — LQG relaxes this by estimating the state from noisy output measurements. The Kalman Filter provides the minimum-variance estimate, and LQR computes the optimal control law given that estimate. By the Separation Principle, these two subproblems can be solved independently and their solutions combined without loss of optimality.

LQG is widely used in aerospace guidance, robotics, and process control when sensor noise is substantial and the plant model is reasonably accurate.

3.5.2 Mathematical Theory

3.5.2.1 System Model

Consider the discrete-time LTI system:

\[x_{k+1} = A x_k + B u_k + w_k\]

\[y_k = C x_k + v_k\]

where: - \(x_k \in \mathbb{R}^n\) is the state vector - \(u_k \in \mathbb{R}^m\) is the control input - \(y_k \in \mathbb{R}^p\) is the measurement vector - \(w_k \sim \mathcal{N}(0, Q_w)\) is process noise - \(v_k \sim \mathcal{N}(0, R_v)\) is measurement noise

3.5.2.2 LQR Subproblem

The regulator minimises the infinite-horizon quadratic cost:

\[J = \sum_{k=0}^{\infty} \left( x_k^T Q x_k + u_k^T R u_k \right)\]

The optimal state-feedback gain is:

\[K = (R + B^T P B)^{-1} B^T P A\]

where \(P\) is the positive semi-definite solution to the Discrete Algebraic Riccati Equation (DARE):

\[P = Q + A^T P A - A^T P B (R + B^T P B)^{-1} B^T P A\]

The optimal control law is \(u_k = -K \hat{x}_{k|k}\).

3.5.2.3 Kalman Filter Subproblem

The optimal state estimator propagates:

Predict: \[\hat{x}_{k+1|k} = A \hat{x}_{k|k} + B u_k\]

\[P_{k+1|k} = A P_{k|k} A^T + Q_w\]

Update: \[K_f = P_{k|k-1} C^T (C P_{k|k-1} C^T + R_v)^{-1}\]

\[\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_f (y_k - C \hat{x}_{k|k-1})\]

\[P_{k|k} = (I - K_f C) P_{k|k-1} (I - K_f C)^T + K_f R_v K_f^T\]

The steady-state Kalman gain converges to a fixed matrix as \(k \to \infty\).

3.5.2.4 Separation Principle

The fundamental result that makes LQG tractable is:

Theorem (Separation Principle): The optimal output-feedback control law for the LQG problem is the composition of (1) the optimal state estimator (Kalman Filter) and (2) the optimal state-feedback controller (LQR), designed independently.

This means the LQR gain \(K\) and the Kalman gain \(K_f\) do not interact — changing the noise covariances \(Q_w\), \(R_v\) affects only the estimator, and changing the cost weights \(Q\), \(R\) affects only the regulator.

3.5.2.5 Combined Control Loop

At each time step \(k\):

1. Measurement update: \(\hat{x}_{k|k} \leftarrow \text{KF.Update}(y_k)\)
2. Compute control: \(u_k = -K \hat{x}_{k|k}\)
3. Time update: \(\hat{x}_{k+1|k} \leftarrow \text{KF.Predict}(u_k)\)

3.5.3 Complexity Analysis

where \(n\) = state size, \(m\) = input size, \(p\) = measurement size.

3.5.4 Numerical Considerations

• DARE convergence: The LQR DARE requires \((A, B)\) to be stabilisable and \((A, Q^{1/2})\) to be detectable. Ill-conditioned cost matrices or near-unstable plants can slow convergence or yield poor numerical accuracy.
• Kalman Filter initialisation: The choice of initial covariance \(P_0\) affects transient performance. A larger \(P_0\) leads to faster initial adaptation at the cost of more initial aggressiveness.
• Noise covariance tuning: \(Q_w / R_v\) ratio controls the balance between trusting the model (large \(R_v\)) versus measurements (small \(R_v\)). Poor tuning degrades closed-loop performance.
• Stability: The separation principle guarantees stability only when both the LQR gain and the Kalman Filter produce stable designs individually. Verify that the observer eigenvalues are well within the unit disc.
• Fixed-point arithmetic: LQG is not recommended for Q15/Q31 types unless the operating range can be normalised to \([-1, 1]\) for all state, input, and measurement signals. Use float unless the target platform lacks an FPU.

3.5.5 Pitfalls & Edge Cases

• Model uncertainty: LQG is optimal only for the assumed model. Unmodelled dynamics (e.g., flexible modes, time delays) can destabilise the closed loop. LQG/LTR (loop transfer recovery) or robust variants should be considered when model fidelity is limited.
• Non-minimum-phase systems: LQG does not guarantee good robustness margins for non-minimum-phase plants. The loop gain can drop, resulting in poor disturbance rejection.
• Nonlinear systems: LQG is only optimal for linear systems with Gaussian noise. For nonlinear systems, Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF) with an LQR gain around an operating point provides a linearisation-based approximation.

3.5.6 Step-by-Step Walkthrough

Consider a double integrator plant — a mass subject to force input, observed through a noisy position sensor. The discrete-time matrices with sample period \(T_s = 0.1\,\text{s}\) are:

\[A = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 0.1 \end{bmatrix}, \quad C = \begin{bmatrix} 1 & 0 \end{bmatrix}\]

Construction (offline):

1. Choose LQR weights \(Q = 10 I_2\), \(R = 0.1\). Solve DARE for \(P\), compute gain \(K = (R + B^T P B)^{-1} B^T P A\).
2. Choose noise covariances \(Q_w = 0.1 I_2\), \(R_v = 1.0\). Solve dual DARE for steady-state Kalman gain \(K_f\).
3. Verify closed-loop stability: eigenvalues of \((A - BK)\) and \((A - K_f C)\) must lie strictly inside the unit disc.

Online loop (per sample):

Starting from initial position \(x_0 = [1, 0]^T\), the LQG regulator drives the position to zero while the Kalman Filter’s estimated trajectory converges to the true state despite measurement noise.

3.5.7 Variants & Generalizations

• LQG/LTR (Loop Transfer Recovery): A systematic method to recover the robustness margins of the full-state LQR design, which are not preserved by the LQG output-feedback loop. Achieved by scaling \(Q_w \to \rho B B^T\) with \(\rho \to \infty\).
• Output-feedback MPC: Replaces the LQR regulator with an MPC solver while retaining the Kalman Filter estimator. Handles hard state and input constraints at the cost of online optimisation.
• LQG with integral action: Augments the state with an integrator on the tracking error to eliminate steady-state offset for step reference signals. The augmented system \((A_a, B_a, C_a)\) is formed and the DARE solved on the expanded state.
• Reduced-order Kalman Filter: When \(p < n\), a Luenberger observer with Kalman-optimal gain provides a simpler alternative to the full Kalman Filter.
• Extended/Unscented LQG: Replaces the Kalman Filter with EKF or UKF for mildly nonlinear plants while keeping the LQR gain computed around the operating point.

3.5.8 Applications

• Aerospace guidance: Inertial navigation systems use LQG to fuse accelerometer/gyroscope measurements with GPS for optimal state estimation under sensor noise.
• Vibration suppression: Active damping in flexible structures (spacecraft appendages, machine tools) employs LQG to attenuate resonant modes while rejecting measurement noise from strain gauges.
• Process control: Chemical reactors and distillation columns use LQG when only a subset of states are measured (e.g., temperature at one point, not at every tray).
• Robotics: Joint torque control in manipulators with encoder noise and flexible joint dynamics benefits from LQG to smooth out the velocity estimates used by the feedback law.
• Automotive: Active suspension and powertrain control use LQG to balance ride comfort (low state weight on acceleration) against actuator energy (high control weight) while filtering noisy road-profile sensors.

3.5.9 Connections to Other Algorithms

• LQR: LQG reduces to LQR when full state is directly observed (no noise, identity measurement matrix).
• Kalman Filter: The estimation subproblem is exactly the standard Kalman Filter.
• MPC: Model Predictive Control generalises LQR to handle hard constraints on states and inputs; LQG provides the estimator component in output-feedback MPC.
• DARE Solver: Both LQR and the steady-state Kalman Filter depend on the Discrete Algebraic Riccati Equation.
• Separation Principle: Connects to the Certainty Equivalence Principle in stochastic optimal control, which states that the optimal policy under Gaussian uncertainty acts as if the state estimate is the true state.

3.6 Model Predictive Controller (MPC)

3.6.1 Overview & Motivation

Model Predictive Control (MPC) is a receding-horizon optimal control strategy that computes control inputs by solving a finite-horizon optimization problem at each time step. Unlike LQR, which applies a fixed gain computed offline, MPC re-solves the optimization online, enabling it to handle constraints on control inputs directly.

At each sample instant, MPC predicts the future system trajectory over a prediction horizon \(N_p\), optimizes a sequence of control moves over a control horizon \(N_c \leq N_p\), applies only the first control move, and repeats at the next step. This receding-horizon approach provides feedback and robustness to disturbances.

Key advantages over LQR: - Explicit handling of input constraints (actuator limits) - Preview and anticipation of future reference changes - Tunable trade-off between horizon length and computational cost

When to use MPC: - Systems with actuator saturation or operational limits - Multi-input multi-output (MIMO) systems requiring coordinated control - Applications where the computational budget allows online optimization (typically \(N_c \cdot m \leq 20\))

3.6.2 Mathematical Theory

3.6.2.1 Prerequisites

• Discrete-time linear state-space model: \(x_{k+1} = Ax_k + Bu_k\)
• Quadratic cost weighting matrices \(Q \succeq 0\) (state), \(R \succ 0\) (control)
• Optional terminal cost matrix \(P \succeq 0\) (typically from DARE)

3.6.2.2 Core Definitions

Cost function over prediction horizon \(N_p\) with control horizon \(N_c\):

\[J = \sum_{k=0}^{N_p-1} x_k^T Q\, x_k + \sum_{k=0}^{N_c-1} u_k^T R\, u_k + x_{N_p}^T P\, x_{N_p}\]

For \(k \geq N_c\), the control input is held at \(u_{N_c-1}\) or zero (our implementation sets it to zero beyond \(N_c\)).

3.6.2.3 Prediction Matrices

The future state trajectory can be expressed as:

\[\mathbf{X} = \Psi\, x_0 + \Theta\, \mathbf{U}\]

where \(\mathbf{X} = [x_1^T, x_2^T, \ldots, x_{N_p}^T]^T\) and \(\mathbf{U} = [u_0^T, u_1^T, \ldots, u_{N_c-1}^T]^T\).

State propagation matrix \(\Psi\) (\(N_p n \times n\)):

\[\Psi = \begin{bmatrix} A \\ A^2 \\ \vdots \\ A^{N_p} \end{bmatrix}\]

Control-to-state matrix \(\Theta\) (\(N_p n \times N_c m\)):

\[\Theta = \begin{bmatrix} B & 0 & \cdots & 0 \\ AB & B & \cdots & 0 \\ A^2B & AB & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N_p-1}B & A^{N_p-2}B & \cdots & A^{N_p-N_c}B \end{bmatrix}\]

3.6.2.4 QP Formulation

Substituting predictions into the cost yields a quadratic program in \(\mathbf{U}\):

\[J = \mathbf{U}^T H\, \mathbf{U} + 2\, x_0^T F^T \mathbf{U} + \text{const}\]

where:

\[H = \Theta^T \bar{Q}\, \Theta + \bar{R}\] \[F = \Theta^T \bar{Q}\, \Psi\]

\(\bar{Q}\) is block-diagonal with \(Q\) on the first \(N_p - 1\) blocks and \(P\) on the last block. \(\bar{R}\) is block-diagonal with \(R\) repeated \(N_c\) times.

3.6.2.5 Unconstrained Solution

Setting \(\nabla_{\mathbf{U}} J = 0\):

\[\mathbf{U}^* = -H^{-1} F\, x_0\]

Solved via Gaussian elimination (\(H\, \mathbf{U}^* = -F\, x_0\)) rather than explicit matrix inversion.

3.6.2.6 Box Constraints

For control input constraints \(u_{\min} \leq u_k \leq u_{\max}\), a simple projected approach clamps each element of the unconstrained solution. This is a first-order approximation; for tight constraints or state constraints, a full QP solver would be required.

3.6.3 Complexity Analysis

For typical embedded use (\(n=2, m=1, N_c=10\)): the online solve is a \(10 \times 10\) linear system, well within real-time budgets.

3.6.4 Step-by-Step Walkthrough

System: Double integrator with \(dt = 0.1\) s

\[A = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0.005 \\ 0.1 \end{bmatrix}\]

\[Q = \begin{bmatrix} 10 & 0 \\ 0 & 1 \end{bmatrix}, \quad R = [0.1], \quad N_p = N_c = 3\]

Step 1 — Build \(\Psi\):

\[\Psi = \begin{bmatrix} A \\ A^2 \\ A^3 \end{bmatrix} = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \\ 1 & 0.2 \\ 0 & 1 \\ 1 & 0.3 \\ 0 & 1 \end{bmatrix}\]

Step 2 — Build \(\Theta\):

\[\Theta = \begin{bmatrix} 0.005 & 0 & 0 \\ 0.1 & 0 & 0 \\ 0.015 & 0.005 & 0 \\ 0.2 & 0.1 & 0 \\ 0.03 & 0.015 & 0.005 \\ 0.3 & 0.2 & 0.1 \end{bmatrix}\]

Step 3 — Compute \(H\) and \(F\):

\(H = \Theta^T \bar{Q} \Theta + \bar{R}\) produces a \(3 \times 3\) positive definite matrix.

\(F = \Theta^T \bar{Q} \Psi\) produces a \(3 \times 2\) matrix.

Step 4 — Online solve for \(x_0 = [1, 0]^T\):

Solve \(H\, \mathbf{U}^* = -F \cdot [1, 0]^T\) via Gaussian elimination.

Apply first element \(u_0^*\) to the plant.

3.6.5 Pitfalls & Edge Cases

• Horizon too short: If \(N_p\) is too small, the controller is myopic and may produce oscillatory or unstable behavior. Use DARE terminal cost \(P\) to mitigate.
• Ill-conditioned Hessian: Very large \(Q/R\) ratios create poorly conditioned \(H\). Keep \(Q\) and \(R\) within 3–4 orders of magnitude of each other.
• Constraint infeasibility: Box constraints that are too tight for the required control effort will clamp every step, degrading performance. Monitor constraint activity.
• Fixed-point overflow: For Q15/Q31 types, the Hessian elements grow with horizon length. Keep \(N_c \cdot m\) small and scale weights carefully.
• Stack usage: All matrices are stack-allocated. A system with \(n=3, m=2, N_c=10\) creates a \(20 \times 20\) Hessian (1600 bytes for float). Monitor stack budget.
• Terminal cost omission: Without terminal cost \(P\), the controller ignores cost-to-go beyond the horizon, leading to suboptimal or unstable behavior for short horizons.

3.6.6 Variants & Generalizations

3.6.7 Applications

• Autonomous vehicles: Path tracking with steering and acceleration limits
• Process control: Chemical reactor temperature/pressure regulation with valve constraints
• Robotics: Joint torque-constrained trajectory tracking
• Power electronics: Voltage/current regulation in converters with switching constraints
• HVAC systems: Energy-efficient building climate control with comfort bounds
• Quadrotor control: Attitude and position control with thrust limits

3.6.8 Connections to Other Algorithms

graph LR
    DARE["Discrete Algebraic<br/>Riccati Equation"] -->|terminal cost P| MPC["Model Predictive<br/>Controller"]
    GE["Gaussian<br/>Elimination"] -->|solves H·U = -F·x| MPC
    LQR["LQR Controller"] -.->|special case<br/>N→∞, no constraints| MPC
    MPC -->|first control move| Plant["Plant Model"]
    Plant -->|state measurement| MPC

• LQR is the infinite-horizon, unconstrained special case of MPC. As \(N_p \to \infty\) with DARE terminal cost, the MPC gain converges to the LQR gain.
• DARE computes the terminal cost matrix \(P\), ensuring the finite-horizon problem approximates the infinite-horizon solution.
• Gaussian Elimination solves the dense linear system \(H\mathbf{U} = -Fx\) at each step. For constrained problems, iterative QP solvers could replace this.
• Kalman Filter provides state estimates when full state measurement is unavailable (MPC + KF = output-feedback MPC).

3.7 Linear Time-Invariant System Model

3.7.1 Overview & Motivation

A Linear Time-Invariant (LTI) system model is the fundamental mathematical object shared across state-space control, estimation, and signal processing. It captures the dynamics of a physical plant — a motor, a pendulum, a vehicle — in a compact matrix representation that enables the systematic application of optimal control and filtering theory.

Representing an LTI model as a first-class value type makes it possible to pass a single plant description into algorithms such as the Kalman Filter, LQR, LQG, and MPC, eliminating the error-prone practice of supplying the same system matrices (A, B, C, D) separately to each component.

3.7.2 Mathematical Theory

3.7.2.1 Discrete-Time State-Space Representation

A discrete-time LTI system with state \(x_k \in \mathbb{R}^n\), input \(u_k \in \mathbb{R}^m\), and output \(y_k \in \mathbb{R}^p\) is described by two equations:

State equation: \[x_{k+1} = A x_k + B u_k\]

Output equation: \[y_k = C x_k + D u_k\]

where: - \(A \in \mathbb{R}^{n \times n}\) is the state transition matrix - \(B \in \mathbb{R}^{n \times m}\) is the input matrix (sometimes called the control matrix) - \(C \in \mathbb{R}^{p \times n}\) is the output matrix (sometimes called the measurement matrix) - \(D \in \mathbb{R}^{p \times m}\) is the feedthrough matrix (often zero for strictly proper systems)

3.7.2.2 Special Cases

Full-state output (\(C = I\), \(D = 0\)): When all states are directly observable (e.g., a simulation or a state-feedback controller with perfect sensing), the output equation collapses to \(y_k = x_k\). This is the typical case for LQR.

Autonomous system (\(B = 0\), \(u_k = 0\)): A system driven only by initial conditions and process noise. Used in Kalman smoothing and spectral analysis.

3.7.2.3 Discretisation

Continuous-time plants described by \(\dot{x} = A_c x + B_c u\) must be discretised before use in digital controllers:

\[A = e^{A_c T_s}, \quad B = A_c^{-1}(A - I) B_c\]

where \(T_s\) is the sampling period. For small \(T_s\) and stable \(A_c\), the forward-Euler approximation \(A \approx I + A_c T_s\), \(B \approx B_c T_s\) may be used.

3.7.2.4 Stability and Controllability

An LTI model is: - Stable if all eigenvalues of \(A\) lie strictly inside the unit disc (\(|\lambda_i| < 1\) for discrete-time systems). - Controllable if the controllability matrix \(\mathcal{C} = [B \; AB \; A^2B \; \cdots \; A^{n-1}B]\) has full row rank. - Observable if the observability matrix \(\mathcal{O} = [C^T \; (CA)^T \; \cdots \; (CA^{n-1})^T]^T\) has full column rank.

Controllability and observability are prerequisites for LQR and Kalman Filter stability, respectively.

3.7.3 Complexity Analysis

3.7.4 Numerical Considerations

• Condition number of A: Poorly conditioned \(A\) amplifies numerical errors during iterative integration. Monitor the spectral radius of \(A\) to ensure stability is not lost due to finite-precision arithmetic.
• Feedthrough matrix D: For most physical systems \(D = 0\) (strictly proper). A nonzero \(D\) requires care in closed-loop analysis since direct input-to-output coupling may cause algebraic loops.
• Fixed-point use: LTI matrices are suitable for Q15/Q31 when all matrix entries and intermediate products remain within \([-1, 1]\). For real physical systems with gains exceeding unity, scale the representation (e.g., normalise state and input ranges) before using fixed-point arithmetic.
• Identity construction: When building the \(C = I\) case (full-state output), use \(0.9999\) rather than \(1.0\) for Q15 compatibility, since \(1.0\) is not representable in the Q15 format.

3.7.5 Pitfalls & Edge Cases

• Time-varying systems: The LTI model is invalid for systems whose matrices \(A\), \(B\), \(C\), \(D\) depend on time or operating point. Use scheduling or re-linearisation for such plants.
• Nonlinear plants: State-space linearisation is valid only in a neighbourhood of the operating point. Large deviations from the linearisation point invalidate the model.
• Sampling rate: Discrete-time LTI models are sampling-rate-specific. Changing \(T_s\) requires re-discretisation and possibly re-tuning of dependent controllers and filters.

3.7.6 Step-by-Step Walkthrough

Consider a double integrator (position and velocity state, force input, position output) with sample period \(T_s = 0.1\,\text{s}\):

Continuous-time plant: \(\dot{x} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} x + \begin{bmatrix} 0 \\ 1 \end{bmatrix} u\), \(y = \begin{bmatrix} 1 & 0 \end{bmatrix} x\)

Step 1 — Discretise using forward-Euler (\(A \approx I + A_c T_s\), \(B \approx B_c T_s\)):

\[A = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 0.1 \end{bmatrix}, \quad C = \begin{bmatrix} 1 & 0 \end{bmatrix}, \quad D = \begin{bmatrix} 0 \end{bmatrix}\]

Step 2 — Simulate one step from \(x_0 = [1, 0]^T\) with \(u_0 = -2\):

\[x_1 = A x_0 + B u_0 = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0.1 \end{bmatrix}(-2) = \begin{bmatrix} 1 \\ -0.2 \end{bmatrix}\]

Step 3 — Compute output at \(k=0\):

\[y_0 = C x_0 = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} 1 \\ 0 \end{bmatrix} = 1\]

The position measurement is \(1.0\) (exact, since \(D = 0\) and noise is excluded here).

3.7.7 Variants & Generalizations

• Full-state output (\(C = I\), \(D = 0\)): All states are directly available as outputs. Used in LQR with perfect sensing. Constructed via the WithFullStateOutput factory, which sets \(C\) to the identity.
• Autonomous system (\(B = 0\)): No input; the system evolves freely from initial conditions. Used in Kalman smoothing and spectral analysis (e.g., auto-regressive models).
• Strictly proper systems (\(D = 0\)): Most physical plants have zero feedthrough; the \(D\) matrix is omitted from the signal path. The LTI representation preserves \(D\) for generality.
• Continuous-time counterpart: Described by \(\dot{x} = A_c x + B_c u\), \(y = C_c x + D_c u\). Requires discretisation before use in digital controllers.
• Multi-rate LTI: Different sensors and actuators may operate at different sample rates; a lifted LTI representation can handle periodic multi-rate systems.

3.7.8 Applications

• Digital control design: The LTI model is the canonical starting point for controller synthesis (LQR, MPC, pole placement). A linearised plant description is the single shared artefact that all design tools consume.
• Kalman Filter initialisation: The \((A, B, C)\) matrices are passed directly into the Kalman Filter to define the predict and update equations, ensuring consistency between plant and estimator.
• Simulation and validation: Monte Carlo simulations propagate an LTI model forward to verify closed-loop stability margins, noise sensitivity, and disturbance rejection before deployment on hardware.
• System identification: Identified discrete-time state-space models from input-output data (e.g., via subspace identification or prediction-error methods) are naturally expressed as LTI structs.
• Transfer function analysis: The \(z\)-domain transfer function \(H(z) = C(zI - A)^{-1}B + D\) can be extracted from the LTI matrices for frequency-response computation and Bode plotting.

3.7.9 Connections to Other Algorithms

• LQR: Uses the \((A, B)\) pair to solve the DARE and compute the optimal state-feedback gain.
• Kalman Filter: Uses \((A, B, C)\) to predict the next state and relate states to measurements.
• LQG: Composes both — \((A, B)\) for the regulator and \((A, B, C)\) for the estimator.
• MPC: Uses \((A, B)\) to build the prediction model over a finite horizon.
• Kalman Smoother: Uses \((A, C)\) for the forward-backward pass in batch smoothing.
• DARE Solver: Receives \((A, B)\) as inputs to compute the Riccati solution.

3.8 Feedforward / 2-DOF Controller

3.8.1 Overview & Motivation

A single feedback loop must balance two competing demands: fast reference tracking and robust rejection of disturbances. Tightening the feedback gains for faster tracking reduces stability margins; relaxing them for robustness slows the response. The two-degree-of-freedom (2-DOF) structure resolves this conflict by separating the two tasks into independent paths. A feedforward path shapes how the output follows the reference, while the feedback path corrects errors and rejects disturbances. Because the feedforward term is open-loop, it can be tuned for tracking without affecting the closed-loop stability margins at all.

3.8.2 Mathematical Theory

3.8.2.1 Signal Flow

Let \(r\) be the reference, \(y\) the plant output, and \(u\) the actuator command. The 2-DOF law is:

\[u = \text{sat}\!\left(u_{ff}(r) + u_{fb}(r - y),\; u_{min},\; u_{max}\right)\]

where: - \(u_{ff}(r)\) is the feedforward map evaluated on the reference alone (open-loop), - \(u_{fb}(e) = u_{fb}(r - y)\) is the feedback law driven by the tracking error, - \(\text{sat}(\cdot)\) clips the combined command to the actuator range \([u_{min}, u_{max}]\).

3.8.2.2 Feedforward Design

The ideal feedforward is an inverse of the plant model. For a linear plant \(P(z)\), the ideal map is \(u_{ff}(r) = P^{-1}(z)\, r\), yielding perfect steady-state tracking. A simpler approximation is a scalar gain \(K_{ff}\) tuned to match the DC plant gain: \(u_{ff}(r) = K_{ff} \cdot r\). Non-minimum-phase plants require a causal approximation since the exact inverse is non-causal.

3.8.2.3 Feedback Design (Independence)

The closed-loop characteristic equation depends only on the feedback law and the plant; \(u_{ff}\) does not appear in it. Stability margins, bandwidth, and disturbance rejection are therefore set entirely by \(u_{fb}\). This decoupling is the defining property of the 2-DOF structure.

3.8.2.4 Saturation

The output clamp \(\text{sat}(u, u_{min}, u_{max})\) acts on the combined command to protect the actuator. When the sum saturates, both the feedforward and feedback contributions are implicitly reduced.

3.8.3 Complexity Analysis

3.8.4 Step-by-Step Walkthrough

Given \(r = 0.5\), \(y = 0.1\), feedforward gain \(K_{ff} = 0.6\), and a proportional feedback with gain \(K_p = 0.5\), clamped to \([-1, 1]\):

1. \(u_{ff} = K_{ff} \cdot r = 0.6 \times 0.5 = 0.3\)
2. \(e = r - y = 0.5 - 0.1 = 0.4\)
3. \(u_{fb} = K_p \cdot e = 0.5 \times 0.4 = 0.2\)
4. \(u_{raw} = u_{ff} + u_{fb} = 0.3 + 0.2 = 0.5\)
5. \(u = \text{sat}(0.5, -1, 1) = 0.5\)

If the system were already tracking perfectly (\(y = r = 0.5\)), then \(e = 0\), \(u_{fb} = 0\), and the feedforward alone supplies the command: \(u = K_{ff} \cdot 0.5 = 0.3\).

3.8.5 Pitfalls & Edge Cases

• Saturation interacts with both terms. When the actuator saturates, the feedback integrator (if used) can wind up. Anti-windup logic in the feedback law is recommended.
• Feedforward does not aid disturbance rejection. If \(u_{ff}\) is tuned for tracking and a disturbance enters the plant, only \(u_{fb}\) responds.
• Inverse-plant feedforward for non-minimum-phase systems is non-causal; use a truncated or approximate inverse.
• Sign convention. The error passed to feedback is \(r - y\), not \(y - r\). Swapping signs destabilises positive-gain plants.

3.8.6 Variants & Generalizations

• Gain-only feedforward: \(u_{ff} = K_{ff} \cdot r\) — the simplest form, requiring only a scalar.
• Filter-based feedforward: a low-pass or derivative filter applied to \(r\) before multiplication; smooths the command for actuators with bandwidth limits.
• Inverse-plant feedforward: uses a model of the plant to produce the exact command required; maximises tracking performance at the cost of model accuracy sensitivity.
• Model-reference 2-DOF: the feedforward block is an explicit reference model; the feedback corrects deviations from the model response rather than from the raw reference.

3.8.7 Applications

• Motion control: hitting aggressive position or velocity trajectories while maintaining stability against load disturbances.
• Process control: tracking ramp set-points in temperature or flow loops where a simple P or PI controller lags behind.
• Robotics: feed-forward gravity/inertia compensation combined with joint-level PD feedback.
• Power electronics: duty-cycle feedforward in DC-DC converters to cancel input voltage disturbances before the voltage-mode feedback acts.

3.8.8 Connections to Other Algorithms

• PID / PidIncremental is a natural choice for the injected feedback law. The incremental (velocity) form avoids integrator wind-up issues at start-up.
• SaturationRateLimiter can replace or augment the output clamp to add slew-rate limiting on the command.
• Gain-scheduled controller extends the 2-DOF concept by varying feedforward and feedback gains across operating points.
• LQR / LQG provides an optimal state-feedback law for the feedback path; the feedforward term then handles reference shaping independently.

3.9 Luenberger Observer

3.9.1 Overview & Motivation

State-feedback controllers require the complete state vector, but physical systems typically expose only a few measured outputs. The Luenberger observer reconstructs unmeasured states from input and output measurements using a copy of the plant model continuously corrected by the difference between predicted and actual outputs.

Unlike the Kalman filter, the Luenberger observer is deterministic — it requires no noise statistics and no covariance propagation. For embedded systems with well-characterized models and deterministic environments, it offers the same state reconstruction at a fraction of the computational cost.

3.9.2 Mathematical Theory

3.9.2.1 Discrete-Time State-Space System

\[x[k+1] = A x[k] + B u[k]\] \[y[k] = C x[k] + D u[k]\]

3.9.2.2 Observer Update Law

The observer maintains a state estimate \(\hat{x}[k]\) updated by:

\[\hat{y}[k] = C \hat{x}[k] + D u[k]\] \[\hat{x}[k+1] = A \hat{x}[k] + B u[k] + L (y[k] - \hat{y}[k])\]

The term \(y[k] - \hat{y}[k]\) is the innovation (output prediction error). The gain matrix \(L \in \mathbb{R}^{n \times p}\) scales how aggressively the estimate is corrected.

3.9.2.3 Error Dynamics

Defining the estimation error \(e[k] = x[k] - \hat{x}[k]\):

\[e[k+1] = (A - LC)\, e[k]\]

The error decays to zero if and only if all eigenvalues of \((A - LC)\) lie strictly inside the unit circle. Choosing \(L\) to place those eigenvalues at desired locations is the pole placement problem for observers.

3.9.2.4 Ackermann’s Formula (SISO Output)

For single-output systems (\(p = 1\)), the observer gain that places eigenvalues of \((A - LC)\) at \(\{\mu_1, \dots, \mu_n\}\) is:

\[L = \varphi_d(A)\, \mathcal{O}^{-1}\, e_n\]

where: - \(\varphi_d(z) = \prod_{i=1}^n (z - \mu_i)\) is the desired characteristic polynomial, evaluated at \(A\) - \(\mathcal{O} = \begin{bmatrix} C \\ CA \\ \vdots \\ CA^{n-1} \end{bmatrix}\) is the observability matrix - \(e_n = [0, \dots, 0, 1]^T\) is the last standard basis vector

This is the dual of Ackermann’s controller placement formula.

3.9.2.5 Observability Condition

Ackermann’s formula requires \(\mathcal{O}\) to be invertible, which holds if and only if the pair \((A, C)\) is observable: every state affects the output through some combination of shifts. An unobservable pair makes the formula degenerate — the gain cannot force arbitrary error convergence.

3.9.3 Complexity Analysis

The design phase is offline. The real-time cost per sample is dominated by the \(n \times n\) state-transition multiply.

3.9.4 Step-by-Step Walkthrough

System: Double integrator, \(n=2\), \(p=1\), desired observer poles \(\{\mu_1, \mu_2\} = \{0.2,\, 0.3\}\)

\[A = \begin{bmatrix}1 & 1\\0 & 1\end{bmatrix}, \quad B = \begin{bmatrix}0\\1\end{bmatrix}, \quad C = \begin{bmatrix}1 & 0\end{bmatrix}\]

Step 1 — Build observability matrix:

\[\mathcal{O} = \begin{bmatrix}C\\CA\end{bmatrix} = \begin{bmatrix}1 & 0\\1 & 1\end{bmatrix}\]

Step 2 — Evaluate desired polynomial at \(A\):

\[\varphi_d(z) = (z - 0.2)(z - 0.3) = z^2 - 0.5z + 0.06\]

\[\varphi_d(A) = A^2 - 0.5A + 0.06I = \begin{bmatrix}0.56 & 1.5\\0 & 0.56\end{bmatrix}\]

Step 3 — Solve for gain:

\[\mathcal{O}^{-1} = \begin{bmatrix}1 & 0\\-1 & 1\end{bmatrix}, \quad \mathcal{O}^{-1} e_2 = \begin{bmatrix}0\\1\end{bmatrix}\]

\[L = \varphi_d(A) \cdot \begin{bmatrix}0\\1\end{bmatrix} = \begin{bmatrix}1.5\\0.56\end{bmatrix}\]

Verification: eigenvalues of \(A - LC = \begin{bmatrix}-0.5 & 1\\-0.56 & 1\end{bmatrix}\) are \(\{0.2, 0.3\}\). ✓

3.9.5 Pitfalls & Edge Cases

• Unobservable pair. If \(\mathcal{O}\) is rank-deficient, the linear solve in Ackermann’s formula fails. Verify observability with a rank test before design.
• Pole placement too aggressive. Observer poles much faster than the controller poles amplify measurement noise, since every output error is fed back through \(L\). Typical practice: observer poles 2–5× faster than controller poles.
• Deadbeat design (all poles at zero) converges in exactly \(n\) steps but maximizes noise sensitivity and requires large \(L\) entries, risking numerical overflow in fixed-point.
• MIMO output limitation. Ackermann’s formula applies to SISO output (\(p=1\)). Multi-output systems require alternative pole-placement methods (e.g., Brogan’s formula or numerical optimization).
• Model mismatch. If the observer plant differs from the true plant, the error dynamics no longer satisfy \(e[k+1] = (A-LC)e[k]\) exactly; a bounded steady-state error results rather than zero convergence.

3.9.6 Variants & Generalizations

3.9.7 Applications

• Motor control — estimating velocity and back-EMF from position and current sensors.
• Automotive suspension — reconstructing unsprung mass velocity from chassis accelerometers.
• Satellite attitude estimation — inferring angular rates from gyros and star trackers.
• Observer-based compensator — pairing with LQR to form the deterministic equivalent of LQG when noise statistics are unavailable.

3.9.8 Connections to Other Algorithms

graph LR
    LO["Luenberger Observer"]
    LTI["LinearTimeInvariant"]
    GE["Gaussian Elimination"]
    LQR["LQR Controller"]
    KF["Kalman Filter"]
    LTI -->|"plant model"| LO
    GE -->|"solve O^{-1}"| LO
    LO -->|"state estimate"| LQR
    KF -.->|"stochastic counterpart"| LO

3.10 Gain-Scheduled Controller

3.10.1 Overview & Motivation

Many physical plants behave as linear systems only near a specific operating point. A motor’s electrical dynamics change with rotational speed, an aircraft’s aerodynamic response shifts with altitude and Mach number, and a chemical reactor’s behaviour depends on temperature. Designing a single linear controller that performs well across the entire operating envelope is often impossible or overly conservative.

The gain-scheduled controller addresses this by pre-computing a family of linear controllers — each optimal near one operating point — and blending between them at runtime based on a measured scheduling variable. This extends the reach of proven linear design methods to mildly nonlinear plants at negligible runtime cost, making it the dominant approach to flight control, powertrain management, and process control on resource-constrained hardware.

3.10.2 Mathematical Theory

3.10.2.1 Setup

Let \(\sigma \in \mathbb{R}\) be the scheduling variable (e.g., speed, load, altitude) and let \(\mathbf{g}(\sigma) \in \mathbb{R}^p\) denote the vector of \(p\) controller gains as a function of \(\sigma\).

A finite set of \(N\) breakpoints \(\sigma_1 < \sigma_2 < \cdots < \sigma_N\) partitions the scheduling axis. At each breakpoint \(\sigma_i\) a gain vector \(\mathbf{g}_i \in \mathbb{R}^p\) is stored, pre-designed offline (e.g., by LQR at each linearised operating point).

3.10.2.2 Piecewise-Linear Interpolation

For a scheduling value \(\sigma\) in the interval \([\sigma_i, \sigma_{i+1}]\), the active gain vector is the linear interpolant:

\[ \mathbf{g}(\sigma) = \mathbf{g}_i + w \, (\mathbf{g}_{i+1} - \mathbf{g}_i), \qquad w = \frac{\sigma - \sigma_i}{\sigma_{i+1} - \sigma_i} \in [0,1]. \]

Each gain component is interpolated independently. The rearranged form \(a + w(b - a)\) is numerically preferable to \((1-w)a + wb\) because it avoids catastrophic cancellation near \(w \approx 0\) and performs one fewer multiply.

3.10.2.3 Endpoint Saturation

Outside the table range the gains are held constant at the boundary values:

\[ \mathbf{g}(\sigma) = \begin{cases} \mathbf{g}_1 & \sigma \leq \sigma_1 \\ \mathbf{g}_N & \sigma \geq \sigma_N \end{cases} \]

Extrapolation is avoided because gain values outside the design envelope are undefined and potentially destabilising.

3.10.2.4 Stability Considerations

Gain scheduling does not guarantee closed-loop stability in general. The standard sufficient conditions are: (1) the scheduling variable changes slowly relative to the closed-loop bandwidth (quasi-static assumption), and (2) each frozen-\(\sigma\) system is stable. When the scheduling variable changes rapidly, a full parameter-varying analysis (LPV, \(\mathcal{H}_\infty\)) is required.

3.10.3 Complexity Analysis

All storage is stack-allocated. The table occupies \(N \cdot p\) words; the active gain vector occupies \(p\) words. No dynamic allocation occurs at any point.

3.10.4 Step-by-Step Walkthrough

Table: \(\{(0, 1), (0.5, 2), (1, 4)\}\) — three breakpoints, one gain each.

Query \(\sigma = 0.75\):

1. \(\sigma = 0.75 > \sigma_1 = 0\) and \(\sigma = 0.75 < \sigma_3 = 1.0\), so no saturation.
2. Locate interval: \(\sigma_2 = 0.5 \leq 0.75 < \sigma_3 = 1.0\), so \(i = 2\).
3. Blend weight: \(w = (0.75 - 0.5) / (1.0 - 0.5) = 0.5\).
4. Active gain: \(g = 2.0 + 0.5 \times (4.0 - 2.0) = 3.0\).

Query \(\sigma = -1.0\) (below table): saturate to \(g = g_1 = 1.0\).

3.10.5 Pitfalls & Edge Cases

• Non-monotone breakpoints. The interpolation formula divides by \(\sigma_{i+1} - \sigma_i\); equal or reversed breakpoints cause division by zero or sign inversion. Strictly increasing order must be asserted at construction.
• Rapid scheduling. If \(\dot{\sigma}\) is large relative to the closed-loop bandwidth, the quasi-static stability argument breaks down. The scheduling variable should be filtered or the breakpoint density increased near fast-varying regions.
• Extrapolation is unsafe. Outside the design envelope, gains are unknown quantities. Endpoint saturation is the only safe embedded policy; never extrapolate control gains.
• Gain discontinuities. If adjacent breakpoint gain sets differ sharply, the interpolated gains may produce a transient step in the control output when \(\sigma\) crosses a breakpoint. Smooth scheduling designs (e.g., LPV synthesis) eliminate this.
• Single-breakpoint interval. Degenerate tables with \(N = 1\) offer no interpolation; require \(N \geq 2\) at compile time.

3.10.6 Variants & Generalizations

3.10.7 Applications

• Flight control — gain sets designed at different Mach / altitude corners; \(\sigma =\) dynamic pressure or Mach number.
• Automotive powertrain — PID gains scheduled on engine speed and load to compensate torque-speed nonlinearity.
• Industrial process control — reactor temperature or pH controllers scheduled on throughput or feed concentration.
• Servo drives — current/velocity loop gains scheduled on rotor speed to compensate back-EMF and inductance variation.

3.10.8 Connections to Other Algorithms

3.11 Lead-Lag Compensator

3.11.1 Overview & Motivation

Classical feedback loops require a mechanism to reshape the open-loop frequency response without the full overhead of a state-space design. A one-pole/one-zero compensator achieves this with three tuning parameters and two state words, making it practical for any microcontroller control loop. When phase margin is insufficient, a lead configuration injects extra phase near the gain crossover frequency, raising stability margin and permitting a higher bandwidth. When steady-state error is the concern, a lag configuration boosts low-frequency gain to drive the error toward zero while leaving the crossover region essentially unchanged.

3.11.2 Mathematical Theory

3.11.2.1 Continuous-Time Transfer Function

The compensator is defined in the Laplace domain as:

\[C(s) = K \cdot \frac{s + z}{s + p}\]

where \(K\) is the overall gain, \(z\) is the zero frequency (rad/s), and \(p\) is the pole frequency (rad/s).

The DC gain is \(C(0) = K \cdot z / p\).

• Lead network (\(z < p\)): the zero sits below the pole, so phase rises at mid frequencies and then falls again, providing a phase bump near crossover.
• Lag network (\(z > p\)): the pole sits below the zero, so the compensator acts as a high-gain integrator approximation at low frequencies and rolls back to unity at high frequencies.

3.11.2.2 Bilinear (Tustin) Discretization

The bilinear transform substitutes \(s \leftarrow \frac{2}{T_s} \cdot \frac{1 - z^{-1}}{1 + z^{-1}}\), mapping the entire left half of the \(s\)-plane to the interior of the unit circle in the \(z\)-plane and preserving stability.

Define \(c = 2/T_s\). The numerator and denominator polynomials in \(z\) are:

\[n_0 = K(c + z), \quad n_1 = K(z - c)\] \[d_0 = c + p, \quad d_1 = p - c\]

Normalizing by \(d_0\):

\[b_0 = \frac{n_0}{d_0}, \quad b_1 = \frac{n_1}{d_0}, \quad a_1 = \frac{d_1}{d_0}\]

3.11.2.3 Discrete-Time Recurrence (Direct Form I)

\[y[n] = b_0 \, x[n] + b_1 \, x[n-1] - a_1 \, y[n-1]\]

The sign convention places the feedback term with a minus sign on \(a_1\), so positive \(a_1\) in the formula corresponds to a pole at \(+a_1\) inside the unit disk.

3.11.3 Complexity Analysis

Design (Tustin coefficient computation) is \(O(1)\) and occurs once in the constructor.

3.11.4 Step-by-Step Walkthrough

Parameters: \(K=1\), \(z=1\,\text{rad/s}\), \(p=10\,\text{rad/s}\), \(T_s = 0.01\,\text{s}\) (lead network).

1. \(c = 2/0.01 = 200\)
2. \(n_0 = 1 \cdot (200 + 1) = 201\), \(\quad n_1 = 1 \cdot (1 - 200) = -199\)
3. \(d_0 = 200 + 10 = 210\), \(\quad d_1 = 10 - 200 = -190\)
4. \(b_0 = 201/210 \approx 0.9571\), \(\quad b_1 = -199/210 \approx -0.9476\), \(\quad a_1 = -190/210 \approx -0.9048\)

Unit-step response (first two samples):

The first output (≈ 0.957) already exceeds the DC steady-state gain of 0.1, illustrating the phase-lead kick.

Verification of DC gain: \(b_0 + b_1 = 2/210\); \(1 + a_1 = 20/210\); ratio \(= 2/20 = 0.1 = K \cdot z/p\).

3.11.5 Pitfalls & Edge Cases

• Near-Nyquist poles/zeros: when \(z\) or \(p\) is comparable to \(\pi/T_s\), the bilinear transform introduces frequency warping. Pre-warp the analog corner frequencies to \(\hat\omega = (2/T_s)\tan(\omega T_s/2)\) before applying Tustin if exact placement matters.
• Degenerate case \(z = p\): the compensator collapses to a pure gain \(K\) with no dynamics. The discrete recurrence remains valid; the pole and zero cancel.
• Unstable discretization: a plant with a very fast analog pole relative to \(T_s\) can map outside the unit disk; verify \(|a_1| < 1\) after computing coefficients.
• Floating-point accumulation: the two state variables accumulate rounding error indefinitely. For long-running loops, periodic resets or double-precision state registers mitigate drift.

3.11.6 Variants & Generalizations

• Lead-lag cascade: a lead section followed by a lag section in series provides simultaneous bandwidth improvement and steady-state accuracy. Reuse two first-order sections rather than chaining first-order blocks through a single instance.
• Phase-lead only / phase-lag only: selecting \(z\) and \(p\) exclusively achieves single-objective shaping; the structure is unchanged.
• Pre-warped Tustin: replace \(z, p\) with \(\hat z = (2/T_s)\tan(z T_s/2)\) and \(\hat p = (2/T_s)\tan(p T_s/2)\) before computing Tustin coefficients to achieve exact frequency matching.
• Second-order extension: cascading two first-order sections or using a biquad second-order section enables lead-lag-lead or other compound shapes.

3.11.7 Applications

• Motor velocity loops: a lead compensator raises phase margin to allow a higher proportional gain, which directly increases bandwidth and disturbance rejection.
• Voltage regulators: a lag compensator adds integrating action at mains frequency to eliminate steady-state regulation error without destabilizing the switching loop.
• Flight control inner loops: classical lead-lag design from Bode plots remains the dominant method for aircraft inner-loop stabilization due to its transparency and robustness to model uncertainty.
• Thermal control: a lag network boosts gain at low frequencies to null steady-state temperature offset while keeping the loop stable against slow sensor dynamics.

3.11.8 Connections to Other Algorithms

• BiquadCascade: a second-order section generalizes to two poles and two zeros; cascading biquads is the standard approach when more than one lead-lag stage is required.
• PidIncremental: a PID controller contains an implicit lead-lag structure; tuning the derivative and integral terms is equivalent to placing the compensator zero and pole.
• FrequencyResponse: use the Bode magnitude and phase plots to verify that the shaped open-loop response achieves the desired gain crossover frequency and phase margin after adding the compensator.

3.12 Saturation / Rate-Limiter / Slew Blocks

3.12.1 Overview & Motivation

Real actuators — valves, motors, heaters, servos — impose two distinct constraints on any command signal: a travel limit (the actuator cannot physically move past a hard stop) and a speed limit (the mechanism cannot slew arbitrarily fast without damaging itself or the system). Ignoring either constraint causes physically impossible commands, mechanical stress, and, in closed-loop systems, integrator wind-up that degrades recovery after saturation.

These three primitives model exactly those constraints. Saturation enforces the travel limit by clipping the output to a fixed interval. The rate limiter enforces the speed limit by allowing the output to move at most a fixed amount per sample period. Composing them — applying the rate limiter first, then the saturator — guarantees every emitted sample simultaneously respects both bounds, making them the mandatory prerequisite for anti-windup in any integrating controller.

3.12.2 Mathematical Theory

3.12.2.1 Saturation

Given bounds \([\ell, h]\) with \(\ell < h\), saturation is defined as:

\[y = \text{sat}(u) = \min\!\bigl(\max(u,\, \ell),\, h\bigr)\]

This is a memoryless, instantaneous projection onto the interval \([\ell, h]\).

3.12.2.2 Rate Limiter (Slew Limiter)

Let \(\Delta_{\max} = r \cdot T_s\) be the maximum permitted change per sample, where \(r\) is the slew rate (units per second) and \(T_s\) is the sample period (seconds). Define the state \(y[n-1]\) as the last emitted output. The output at tick \(n\) is:

\[\delta[n] = \text{sat}_{\Delta_{\max}}(u[n] - y[n-1]) = \min\!\bigl(\max(u[n] - y[n-1],\,-\Delta_{\max}),\,+\Delta_{\max}\bigr)\]

\[y[n] = y[n-1] + \delta[n]\]

On the first sample (priming), \(y[0] = u[0]\) and no limiting is applied, so the output tracks the first command exactly.

3.12.2.3 Composed Block (Slew-Then-Clamp)

\[y_{\text{composed}}[n] = \text{sat}\!\bigl(y_{\text{slew}}[n]\bigr)\]

Applying the rate limiter before the saturator is essential. If the order were reversed (clamp then slew), a step command to a saturated bound would allow the rate limiter to move the output outside the saturation interval for one tick before the clamp could correct it.

3.12.3 Complexity Analysis

Each block maintains at most two scalar parameters and one scalar state word. The hot path is branch-light: min/max compile to conditional-move instructions on modern architectures.

3.12.4 Step-by-Step Walkthrough

Parameters: \(\ell = -0.5\), \(h = 0.5\), \(r = 0.1\,\text{s}^{-1}\), \(T_s = 1\,\text{s}\) (\(\Delta_{\max} = 0.1\)). Step request: \(u = 1.0\) for all ticks, starting from \(y[-1] = 0\).

The ramp is linear until the saturator clips it at \(h = 0.5\). The rate limiter continues to advance its internal state, but the composed output is held at the bound.

3.12.5 Pitfalls & Edge Cases

• Zero slew rate: \(r = 0\) means \(\Delta_{\max} = 0\); the output freezes at the primed value for all subsequent inputs. This is valid for a hold block but surprising if unintentional.
• Collapsed saturation (\(\ell = h\)): every output equals that constant regardless of input. The rate limiter still advances but the saturator immediately overwrites the result.
• Priming semantics: the first call to the rate limiter always passes through without limiting. Systems that require immediate rate-limiting from the first sample must call Reset with the desired initial state before the first control tick.
• Order of composition: slew-then-clamp is the only order that guarantees simultaneous satisfaction of both constraints at every tick.
• Floating-point accumulation: repeated addition of \(\delta[n]\) can accumulate rounding error over many ticks. For long-running loops at high rate, the error remains bounded by machine epsilon times the number of steps times \(\Delta_{\max}\).

3.12.6 Variants & Generalizations

• Asymmetric rate limiter: use separate positive and negative slew rates \(r^+\), \(r^-\) to model actuators with different rise and fall speeds (e.g., a valve that opens slowly but closes fast).
• Variable-rate limiter: replace the fixed \(\Delta_{\max}\) with a schedule or feedback signal for gain-scheduled slew limiting.
• Deadband + saturation: insert a deadband nonlinearity before the saturator to model mechanical backlash or sensor noise floors.

3.12.7 Applications

• Motor drives: limit acceleration commands to prevent over-current trips while hard-bounding the velocity or position command within safe travel.
• Thermal control: ramp heater power slowly to avoid thermal shock while clamping to the maximum safe power level.
• Anti-windup: feed the saturated (clamped) output back to the integrator of a PID controller so accumulation stops when the actuator is pinned at a bound.
• Sensor fusion pre-processing: slew-limit noisy sensor inputs before they enter an estimator to attenuate impulsive outliers without a dedicated filter.

3.12.8 Connections to Other Algorithms

• PidIncremental: consumes the clamped output for anti-windup; the saturation block provides the feedback signal that stops integrator accumulation.
• BangBangHysteresis: an alternative, discontinuous actuator constraint that switches between two fixed outputs; saturation is the continuous analog.
• ExponentialMovingAverage: can approximate a low-pass slew effect but does not enforce a hard rate bound and has no priming concept.

4 Dynamics & Modeling

4.1 Articulated Body Algorithm (ABA)

4.1.1 Overview & Motivation

The Articulated Body Algorithm (ABA) is the most efficient method for computing forward dynamics of serial kinematic chains. Given joint positions \(q\), velocities \(\dot{q}\), and applied torques \(\tau\), it computes joint accelerations \(\ddot{q}\) in \(O(n)\) time — linear in the number of links.

This is the forward-dynamics counterpart of the Recursive Newton-Euler Algorithm (which solves inverse dynamics). Together, they form the two fundamental \(O(n)\) algorithms in rigid-body dynamics.

Alternatives like Euler-Lagrange require explicitly forming and inverting the \(n \times n\) mass matrix (\(O(n^3)\)), making ABA significantly faster for chains with more than 3-4 links.

4.1.2 Mathematical Theory

4.1.2.1 Problem Statement

Given a serial chain of \(n\) rigid bodies connected by revolute joints, find:

\[\ddot{q} = M(q)^{-1} [\tau - C(q, \dot{q})\dot{q} - g(q)]\]

ABA computes this without ever forming or inverting \(M\).

4.1.2.2 Algorithm Structure

ABA has three passes over the kinematic chain:

1. Forward pass (base → tip): Compute link kinematics — angular velocities, linear velocities, and velocity-product accelerations from joint positions and velocities.

2. Backward pass (tip → base): Compute articulated-body inertias \(I^A_i\) and bias wrenches \(p^A_i\) for each link. At each step, the joint projection removes one degree of freedom from the spatial inertia, and the remainder is propagated to the parent.

3. Forward pass (base → tip): Resolve joint accelerations using the articulated-body quantities. Each joint acceleration \(\ddot{q}_i\) is computed from the projected articulated inertia and the transformed parent acceleration.

4.1.2.3 Key Formulas

Articulated-body inertia starts as the rigid-body spatial inertia of each link, then accumulates child contributions:

\[I^A_i \leftarrow I^A_i + X^*_{i+1} \hat{I}^A_{i+1} X_{i+1}\]

where \(\hat{I}^A\) is the inertia after removing the joint DOF (the “downdate”):

\[\hat{I}^A_i = I^A_i - \frac{U_i U_i^T}{D_i}\]

with \(U_i = I^A_i s_i\), \(D_i = s_i^T U_i\), and \(s_i\) the joint motion subspace (joint axis for revolute joints).

Joint acceleration:

\[\ddot{q}_i = \frac{u_i - U_i^T \hat{a}_i}{D_i}\]

where \(u_i = \tau_i - s_i^T p^A_i\) and \(\hat{a}_i\) is the spatial acceleration contribution from the parent.

4.1.3 Complexity Analysis

Each pass visits every link exactly once, performing constant-time spatial algebra (3×3 matrix operations) per link.

4.1.4 Step-by-Step Walkthrough

Consider a 2-link planar arm with unit masses, unit lengths, Y-axis joints, zero velocities, and zero applied torques under gravity \(g = [0, 0, -9.81]^T\).

Pass 1 — Forward kinematics: both links at \(q = [0, 0]^T\) with \(\dot{q} = [0, 0]^T\), so all velocities and velocity-product terms are zero.

Pass 2 — Backward pass: starting from link 2, compute the rigid-body inertia, project out the joint DOF, transform to link 1’s frame, and accumulate. The articulated inertia at the base now represents the effective inertia of the entire chain.

Pass 3 — Forward pass: at the base, the gravitational acceleration is transformed into the base frame and used to compute \(\ddot{q}_1\). The resulting acceleration is propagated to link 2 to compute \(\ddot{q}_2\).

The result: both joints accelerate downward due to gravity, with the base joint carrying the larger moment from the full chain.

4.1.5 Pitfalls & Edge Cases

• Singular configurations: When \(D_i \to 0\), the joint becomes locked. This should not happen for well-formed revolute joint models with positive inertia.
• Numerical precision: The division by \(D_i\) amplifies errors if \(D_i\) is small. Use float (not fixed-point) for dynamics computations.
• Floating-point only: The algorithm involves trigonometric functions, divisions, and large dynamic ranges that are unsuitable for Q15/Q31 fixed-point.

4.1.6 Variants & Generalizations

• Composite Rigid Body Algorithm (CRBA): Computes the mass matrix \(M(q)\) explicitly in \(O(n^2)\). Useful when \(M\) itself is needed (e.g., for operational-space control).
• Extended ABA: Supports branching chains (trees) by processing children before parents in the backward pass.
• ABA with friction: Joint friction torques can be added directly to \(\tau\).

4.1.7 Applications

• Real-time simulation of robot arms and multi-body systems
• Model-based control (computed torque, impedance control)
• Forward integration of manipulator dynamics

4.1.8 Connections to Other Algorithms

• Recursive Newton-Euler: solves the inverse problem (\(q, \dot{q}, \ddot{q} \to \tau\)). ABA and RNEA are duals: RNEA(q, qDot, ABA(q, qDot, tau)) ≈ tau.
• Euler-Lagrange: equivalent \(O(n^3)\) formulation using the mass matrix.
• Gaussian Elimination: used if the mass matrix approach is taken instead.

4.2 Euler-Lagrange Dynamics

4.2.1 Overview & Motivation

The Euler-Lagrange equations provide a systematic way to derive the equations of motion for mechanical systems from energy principles. Instead of tracking forces on each body (as in Newton’s method), they express dynamics in terms of generalized coordinates \(q\) and the system’s kinetic and potential energy.

For a robotic manipulator or any articulated mechanism with \(n\) degrees of freedom, the Euler-Lagrange formulation yields a compact matrix equation:

\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau\]

Where: - \(q \in \mathbb{R}^n\) — generalized coordinates (joint angles/positions) - \(\dot{q}, \ddot{q}\) — generalized velocities and accelerations - \(M(q) \in \mathbb{R}^{n \times n}\) — mass (inertia) matrix, symmetric positive definite - \(C(q, \dot{q})\dot{q} \in \mathbb{R}^n\) — Coriolis and centrifugal terms - \(g(q) \in \mathbb{R}^n\) — gravitational terms - \(\tau \in \mathbb{R}^n\) — generalized forces/torques (inputs)

This formulation supports two fundamental computations: - Forward dynamics: given torques \(\tau\), compute accelerations \(\ddot{q} = M^{-1}(\tau - C\dot{q} - g)\) - Inverse dynamics: given desired accelerations \(\ddot{q}\), compute required torques \(\tau = M\ddot{q} + C\dot{q} + g\)

4.2.2 Mathematical Theory

4.2.2.1 Prerequisites

The Lagrangian of a system is defined as:

\[\mathcal{L}(q, \dot{q}) = T(q, \dot{q}) - V(q)\]

where \(T\) is kinetic energy and \(V\) is potential energy.

4.2.2.2 Core Definitions

The Euler-Lagrange equation for coordinate \(q_i\) is:

\[\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}_i} - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i \quad \text{for } i = 1, \ldots, n\]

For a rigid-body system, kinetic energy takes the form:

\[T = \frac{1}{2}\dot{q}^T M(q) \dot{q}\]

Applying the Euler-Lagrange equation to this quadratic form yields the standard manipulator equation:

\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau\]

4.2.2.3 Mass Matrix \(M(q)\)

The mass matrix is always symmetric (\(M = M^T\)) and positive definite (\(\dot{q}^T M \dot{q} > 0\) for \(\dot{q} \neq 0\)). These properties guarantee: - The forward dynamics linear system \(M\ddot{q} = f\) always has a unique solution - Physical kinetic energy is always non-negative

4.2.2.4 Coriolis Matrix \(C(q, \dot{q})\)

The Coriolis matrix captures velocity-dependent forces using Christoffel symbols of the first kind:

\[c_{ij} = \sum_{k=1}^{n} \frac{1}{2}\left(\frac{\partial m_{ij}}{\partial q_k} + \frac{\partial m_{ik}}{\partial q_j} - \frac{\partial m_{jk}}{\partial q_i}\right)\dot{q}_k\]

A key property: \(\dot{M} - 2C\) is skew-symmetric, which is important for passivity-based control.

4.2.2.5 Gravity Vector \(g(q)\)

\[g_i(q) = \frac{\partial V}{\partial q_i}\]

where \(V(q)\) is the total potential energy of the system.

4.2.3 Complexity Analysis

For systems with \(n \leq 6\) DOF (typical robotic manipulators), the \(O(n^3)\) cost of Gaussian elimination is negligible. For larger systems, consider the Recursive Newton-Euler Algorithm (RNEA) which achieves \(O(n)\) inverse dynamics.

4.2.4 Step-by-Step Walkthrough

4.2.4.1 Example: Simple Pendulum (1-DOF)

A point mass \(m\) on a rigid rod of length \(l\):

System parameters: \(m = 1\,\text{kg}\), \(l = 1\,\text{m}\), \(g_0 = 9.81\,\text{m/s}^2\)

Model matrices: - \(M(q) = ml^2 = 1.0\) - \(C(q, \dot{q})\dot{q} = 0\) (single DOF → no Coriolis) - \(g(q) = mgl\sin(q) = 9.81\sin(q)\)

Forward dynamics at \(q = 0.5\,\text{rad}\), \(\dot{q} = 0\), \(\tau = 0\):

\[\ddot{q} = M^{-1}(\tau - C\dot{q} - g) = \frac{0 - 0 - 9.81\sin(0.5)}{1.0} = -4.703\,\text{rad/s}^2\]

Inverse dynamics for holding position (\(\ddot{q} = 0\)):

\[\tau = M\ddot{q} + C\dot{q} + g = 0 + 0 + 9.81\sin(0.5) = 4.703\,\text{N·m}\]

4.2.4.2 Example: Two-Link Planar Arm (2-DOF)

For a 2-DOF arm with equal links (\(m_1 = m_2 = 1\,\text{kg}\), \(l_1 = l_2 = 1\,\text{m}\)):

At rest (\(q = [0, 0]^T\), \(\dot{q} = [0, 0]^T\)): - \(\sin(q_1) = 0\), \(\sin(q_1 + q_2) = 0\) - \(g = [0, 0]^T\) → gravity terms vanish when links hang vertically - Forward dynamics with \(\tau = 0\) produces \(\ddot{q} = [0, 0]^T\) (equilibrium)

4.2.5 Pitfalls & Edge Cases

• Singular mass matrix: Should not happen for physically valid systems, but numerical conditioning degrades near kinematic singularities (e.g., fully extended arm). The Gaussian elimination solver uses partial pivoting to help.
• Fixed-point arithmetic: Dynamics values (torques, accelerations) typically exceed the \([-1, 1)\) range of Q15/Q31 formats. Use float unless inputs are carefully scaled.
• Coriolis computation: The choice of Coriolis parameterization (Christoffel vs. factored form) affects the skew-symmetry property of \(\dot{M} - 2C\). The interface returns the product \(C(q,\dot{q})\dot{q}\) directly, leaving the parameterization choice to the user.
• Energy consistency: For simulation, ensure the model matrices satisfy the skew-symmetry property; otherwise, energy may not be conserved in numerical integration.
• Units: Be explicit about whether angles are in radians (standard) and torques in N·m.

4.2.6 Variants & Generalizations

4.2.7 Applications

• Robot control: Computed torque control, gravity compensation, impedance control
• Simulation: Forward dynamics integration for physics simulation
• System identification: Estimate dynamic parameters from measured motion
• Trajectory optimization: Inverse dynamics as constraints in optimization
• Vibration analysis: Linearized dynamics around equilibrium points

4.2.8 Connections to Other Algorithms

• Gaussian Elimination (GaussianElimination.md): Used internally to solve \(M\ddot{q} = f\) in forward dynamics
• LQR Controller (Lqr.md): Euler-Lagrange dynamics can be linearized to produce the \((A, B)\) state-space matrices needed for LQR design
• Kalman Filter (../filters/active/): State estimation for partially observed dynamical systems derived from Euler-Lagrange models
• DARE (DiscreteAlgebraicRiccatiEquation.md): Discretized Euler-Lagrange systems feed into DARE for optimal control gain computation

4.3 Newton-Euler Dynamics

4.3.1 Overview & Motivation

The Newton-Euler formulation describes the dynamics of a single rigid body by directly applying Newton’s second law for translational motion and Euler’s equation for rotational motion. Unlike the Euler-Lagrange approach which works in generalized (joint-space) coordinates, Newton-Euler operates in Cartesian space, computing forces and torques on individual bodies.

In the body-fixed reference frame, the equations are:

\[F = m(\dot{v} + \omega \times v)\] \[\tau = I\dot{\omega} + \omega \times (I\omega)\]

Where: - \(F \in \mathbb{R}^3\) — applied force in the body frame - \(\tau \in \mathbb{R}^3\) — applied torque in the body frame - \(m\) — body mass (scalar) - \(I \in \mathbb{R}^{3 \times 3}\) — inertia tensor in the body frame - \(v, \dot{v} \in \mathbb{R}^3\) — linear velocity and acceleration in the body frame - \(\omega, \dot{\omega} \in \mathbb{R}^3\) — angular velocity and acceleration in the body frame - \(\omega \times v\) — Coriolis term due to body-frame formulation - \(\omega \times (I\omega)\) — gyroscopic coupling term

This supports two computations: - Forward dynamics: given forces/torques, compute linear and angular accelerations - Inverse dynamics: given desired accelerations, compute required forces/torques

4.3.2 Mathematical Theory

4.3.2.1 Newton’s Second Law (Translation)

In an inertial frame, Newton’s second law is simply \(F = ma\). In a body-fixed (rotating) frame, the transport theorem introduces an additional term:

\[F = m(\dot{v} + \omega \times v)\]

The \(\omega \times v\) term is the Coriolis acceleration that arises because the reference frame itself is rotating.

Forward: \(\dot{v} = F/m - \omega \times v\)

Inverse: \(F = m(\dot{v} + \omega \times v)\)

4.3.2.2 Euler’s Equation (Rotation)

Euler’s equation governs rotational dynamics:

\[\tau = I\dot{\omega} + \omega \times (I\omega)\]

The \(\omega \times (I\omega)\) term is the gyroscopic torque — it couples different rotational axes when the inertia tensor is non-spherical. For a body spinning about a non-principal axis, this term causes precession even without external torque.

Forward: \(\dot{\omega} = I^{-1}(\tau - \omega \times (I\omega))\)

This requires solving the \(3 \times 3\) linear system \(I\dot{\omega} = \tau - \omega \times (I\omega)\) via Gaussian elimination.

Inverse: \(\tau = I\dot{\omega} + \omega \times (I\omega)\)

This is a direct matrix-vector multiply plus cross product — no linear solve needed.

4.3.2.3 Properties of the Inertia Tensor

The inertia tensor \(I\) is: - Symmetric: \(I = I^T\) (6 independent components, not 9) - Positive definite: \(\omega^T I \omega > 0\) for \(\omega \neq 0\) - Constant in body frame: For a rigid body, \(I\) does not change with the body’s orientation when expressed in the body frame

When \(I\) is diagonal (principal axes frame), components are the principal moments of inertia \(I_{xx}, I_{yy}, I_{zz}\), and Euler’s equation simplifies to the well-known form:

\[\tau_x = I_{xx}\dot{\omega}_x + (I_{zz} - I_{yy})\omega_y\omega_z\] \[\tau_y = I_{yy}\dot{\omega}_y + (I_{xx} - I_{zz})\omega_z\omega_x\] \[\tau_z = I_{zz}\dot{\omega}_z + (I_{yy} - I_{xx})\omega_x\omega_y\]

4.3.3 Complexity Analysis

All operations are constant-time since the spatial dimension is fixed at 3. The \(3 \times 3\) Gaussian elimination is effectively a small constant cost.

4.3.4 Step-by-Step Walkthrough

4.3.4.1 Example: Uniform Sphere

Parameters: mass \(m = 2\,\text{kg}\), radius \(r = 0.5\,\text{m}\)

Inertia tensor: \(I = \frac{2}{5}mr^2 \cdot \mathbf{I}_3 = 0.2 \cdot \mathbf{I}_3\)

Forward dynamics with \(F = [6, 0, 0]^T\,\text{N}\), \(\tau = [0, 0, 1]^T\,\text{N·m}\), \(v = \omega = 0\):

\[\dot{v} = F/m - 0 = [3, 0, 0]^T \,\text{m/s}^2\] \[\dot{\omega} = I^{-1}\tau = [0, 0, 5]^T \,\text{rad/s}^2\]

4.3.4.2 Example: Gyroscopic Precession

Asymmetric body with \(I = \text{diag}(1, 2, 3)\,\text{kg·m}^2\)

Spinning at \(\omega = [1, 1, 0]^T\,\text{rad/s}\) with no external torque:

\[I\omega = [1, 2, 0]^T\] \[\omega \times (I\omega) = [1,1,0] \times [1,2,0] = [0, 0, 1]^T\] \[\dot{\omega} = I^{-1}(0 - [0,0,1]) = [0, 0, -\tfrac{1}{3}]^T \,\text{rad/s}^2\]

The body accelerates about the z-axis despite no external torque — this is the gyroscopic coupling effect due to asymmetric inertia.

4.3.5 Pitfalls & Edge Cases

• Singular inertia: An inertia tensor must be positive definite. Zero or negative eigenvalues indicate a non-physical body definition.
• Body-frame vs. inertial frame: The equations assume all quantities (forces, torques, velocities) are expressed in the body-fixed frame. Users must transform between frames externally.
• Spherical inertia: When \(I = c \cdot \mathbf{I}_3\), the gyroscopic term \(\omega \times (I\omega) = c(\omega \times \omega) = 0\), simplifying Euler’s equation to \(\tau = I\dot{\omega}\).
• Fixed-point arithmetic: Like Euler-Lagrange, Newton-Euler involves physical quantities (forces in Newtons, torques in N·m) that typically exceed Q15/Q31 range. Use float.
• Energy conservation: For torque-free motion, kinetic energy \(T = \frac{1}{2}\omega^T I \omega\) and angular momentum \(L = I\omega\) magnitude should be conserved. Numerical integration may violate this.

4.3.6 Variants & Generalizations

4.3.7 Applications

• Spacecraft attitude control: Reaction wheel and thruster control using Euler’s equations
• Drone / UAV dynamics: Quadrotor translational and rotational dynamics
• Projectile dynamics: Spinning projectile with aerodynamic forces
• Gyroscope modeling: Precession and nutation analysis
• Foundation for RNEA: Body-level Newton-Euler equations form the building block for recursive multi-body algorithms

4.3.8 Connections to Other Algorithms

• Gaussian Elimination (GaussianElimination.md): Used to solve the \(3 \times 3\) inertia system \(I\dot{\omega} = \tau_{net}\) in forward dynamics
• Euler-Lagrange (EulerLagrange.md): Joint-space counterpart; Euler-Lagrange derives the same dynamics from energy principles in generalized coordinates
• Recursive Newton-Euler (RNEA): Extends single-body Newton-Euler to kinematic chains by propagating velocities/accelerations forward and forces/torques backward through the chain

4.4 Recursive Newton-Euler Algorithm (RNEA)

4.4.1 Overview & Motivation

The Recursive Newton-Euler Algorithm (RNEA) is the most efficient method for computing inverse dynamics of serial kinematic chains (robot arms, manipulators). Given joint positions, velocities, and desired accelerations, it computes the required joint torques in \(O(n)\) time — linear in the number of links.

This contrasts with the Euler-Lagrange approach, which requires explicitly forming and multiplying \(n \times n\) matrices (\(O(n^3)\)). For a 6-DOF robot arm, RNEA is roughly 10× faster than the matrix-based approach.

The algorithm has two passes over the kinematic chain:

1. Forward pass (base → tip): propagate angular velocities, angular accelerations, and linear accelerations from link to link using kinematic relationships
2. Backward pass (tip → base): accumulate forces and torques from the tip back to the base using Newton-Euler equations on each link

The joint torque at each joint is then the projection of the net torque onto the joint axis.

4.4.2 Mathematical Theory

4.4.2.1 Kinematic Chain Model

A serial chain of \(n\) rigid bodies (links) connected by revolute joints. Each link \(i\) is described by:

• \(m_i\) — mass
• \(I_i \in \mathbb{R}^{3 \times 3}\) — inertia tensor at the center of mass, in the link frame
• \(\hat{z}_i \in \mathbb{R}^3\) — joint axis (unit vector in the link frame)
• \(r_{p \to j}^{(i)} \in \mathbb{R}^3\) — position of joint \(i\) in the parent (link \(i-1\)) frame
• \(r_{j \to c}^{(i)} \in \mathbb{R}^3\) — position of the center of mass in the link \(i\) frame

The rotation matrix \(R_i(q_i)\) transforms vectors from link \(i\) frame to parent frame, computed via Rodrigues’ formula from the joint angle \(q_i\) and joint axis \(\hat{z}_i\).

4.4.2.2 Forward Pass — Velocity and Acceleration Propagation

For link \(i\), given parent link \(i-1\) quantities:

Angular velocity: \[\omega_i = R_i^T \omega_{i-1} + \dot{q}_i \hat{z}_i\]

Angular acceleration: \[\dot{\omega}_i = R_i^T \dot{\omega}_{i-1} + \ddot{q}_i \hat{z}_i + \omega_i \times (\dot{q}_i \hat{z}_i)\]

Linear acceleration at joint origin of link \(i\): \[a_{J_i} = R_i^T \left( a_{J_{i-1}} + \dot{\omega}_{i-1} \times r_{p \to j}^{(i)} + \omega_{i-1} \times (\omega_{i-1} \times r_{p \to j}^{(i)}) \right)\]

Linear acceleration at center of mass: \[a_{C_i} = a_{J_i} + \dot{\omega}_i \times r_{j \to c}^{(i)} + \omega_i \times (\omega_i \times r_{j \to c}^{(i)})\]

For the base link (\(i = 0\)), the “parent” is the world frame with \(\omega_{-1} = 0\) and \(a_{J_{-1}} = -g\) (gravity expressed as a base acceleration, a standard RNEA convention).

4.4.2.3 Backward Pass — Force and Torque Accumulation

For each link \(i\), starting from the tip:

Net force on link \(i\): \[f_i = m_i a_{C_i} + \sum_{\text{children } j} R_j f_j\]

Net torque on link \(i\) (about joint): \[\tau_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i) + r_{j \to c}^{(i)} \times (m_i a_{C_i}) + \sum_{\text{children } j} \left( R_j \tau_j + r_{p \to j}^{(j)} \times R_j f_j \right)\]

Joint torque: \[\tau_{q_i} = \hat{z}_i^T \tau_i\]

4.4.2.4 Rodrigues’ Rotation Formula

The rotation matrix for angle \(\theta\) about unit axis \(\hat{u} = [u_x, u_y, u_z]^T\):

\[R = \cos\theta \cdot \mathbf{I}_3 + (1 - \cos\theta) \hat{u}\hat{u}^T + \sin\theta [\hat{u}]_\times\]

where \([\hat{u}]_\times\) is the skew-symmetric matrix of \(\hat{u}\).

4.4.3 Complexity Analysis

Compared to Euler-Lagrange \(O(n^3)\) inverse dynamics, RNEA is dramatically faster for large \(n\). For \(n = 6\) (typical robot arm), RNEA performs roughly 780 floating-point operations vs. ~14,000 for the matrix approach.

4.4.4 Step-by-Step Walkthrough

4.4.4.1 Example: 1-DOF Pendulum

A single thin rod of mass \(m = 2\,\text{kg}\) and length \(l = 1\,\text{m}\), rotating about the z-axis. Joint at origin, CoM at \([l/2, 0, 0]^T\).

Inertia at CoM: \(I = ml^2/12 = 0.167\,\text{kg·m}^2\) (about y and z axes)

Setup: \(q = 0\), \(\dot{q} = 0\), \(\ddot{q} = 1\,\text{rad/s}^2\), zero gravity.

Forward pass: - \(\omega_0 = [0, 0, 0]^T + 0 \cdot [0, 0, 1]^T = [0, 0, 0]^T\) - \(\dot{\omega}_0 = [0, 0, 1]^T\) (from \(\ddot{q}\)) - \(a_{C_0} = 0 + [0, 0, 1] \times [0.5, 0, 0] + 0 = [0, 0.5, 0]^T\,\text{m/s}^2\)

Backward pass: - \(f_0 = 2 \cdot [0, 0.5, 0]^T = [0, 1, 0]^T\,\text{N}\) - \(\tau_0 = I \cdot [0, 0, 1]^T + 0 + [0.5, 0, 0] \times [0, 1, 0]^T\) - \(= [0, 0, 0.167]^T + [0, 0, 0.5]^T = [0, 0, 0.667]^T\) - \(\tau_q = [0, 0, 1]^T \cdot [0, 0, 0.667]^T = 0.667\,\text{N·m}\)

This matches \(I_{\text{end}} \cdot \ddot{q} = \frac{ml^2}{3} \cdot 1 = 0.667\).

4.4.5 Pitfalls & Edge Cases

• Joint axis convention: The joint axis must be a unit vector. Non-unit vectors will produce incorrect rotation matrices and torque projections.
• Frame consistency: All link parameters (inertia, CoM position, joint axis) must be expressed in the link’s own body frame. Mixing world-frame and body-frame quantities is a common source of bugs.
• Gravity convention: Gravity enters as \(a_0 = -g\) (negative) in the base acceleration. This “fictitious force” convention avoids special-casing gravity in each link’s force calculation.
• Serial chains only: This implementation assumes a single serial chain with no branches. Tree-structured or closed-chain mechanisms require extensions.
• Numerical precision: For long chains, the recursive propagation can accumulate floating-point errors. For \(n \leq 10\), this is typically negligible.
• Fixed-point arithmetic: Like other dynamics algorithms, physical quantities exceed Q15/Q31 range. Use float.

4.4.6 Variants & Generalizations

4.4.7 Applications

• Computed torque control: RNEA provides the feedforward torque \(\tau = M\ddot{q}_d + h(q, \dot{q})\) for tracking controllers at each control cycle
• Gravity compensation: RNEA with \(\dot{q} = \ddot{q} = 0\) yields gravity torques directly
• Simulation: Build the mass matrix column-by-column using \(n\) RNEA calls, then solve \(M\ddot{q} = \tau - h\) for forward dynamics
• Model-based friction estimation: Compare RNEA-predicted torques with measured motor currents
• Real-time control loops: The \(O(n)\) complexity makes RNEA suitable for 1 kHz+ control rates

4.4.8 Connections to Other Algorithms

• Newton-Euler (NewtonEuler.md): RNEA applies the single-body Newton-Euler equations recursively across the chain. The per-link force/torque computation is identical.
• Euler-Lagrange (EulerLagrange.md): Both compute the same dynamics (\(M\ddot{q} + C\dot{q} + g = \tau\)) but RNEA is \(O(n)\) vs. \(O(n^3)\). RNEA is preferred for computation; Euler-Lagrange for analytical derivation.
• Gaussian Elimination (GaussianElimination.md): When using RNEA for forward dynamics, the mass matrix built via RNEA is solved with Gaussian elimination.

5 Estimators

5.1 Linear Regression

5.1.1 Overview & Motivation

Given a set of observations \(\{(x_i, y_i)\}\), we often want to find the best-fit linear relationship between features \(x\) and target \(y\). Linear regression does this by finding the coefficients \(\beta\) that minimize the sum of squared residuals — the gap between predicted and observed values.

The approach is fundamental because: - It has a closed-form solution (the normal equation), so no iterative optimization is needed. - It provides a baseline model against which more complex methods are measured. - The solution is the maximum likelihood estimator under Gaussian noise assumptions.

This library solves the normal equation directly using Gaussian elimination, making it suitable for small-to-moderate feature counts typical in embedded estimation tasks.

5.1.2 Mathematical Theory

5.1.2.1 The Model

\[y = \beta_0 + \beta_1 x_1 + \beta_2 x_2 + \cdots + \beta_p x_p + \varepsilon\]

In matrix form, augmenting \(X\) with a column of ones for the intercept:

\[\mathbf{y} = \mathbf{X}\boldsymbol{\beta} + \boldsymbol{\varepsilon}\]

where \(\mathbf{X} \in \mathbb{R}^{n \times (p+1)}\), \(\boldsymbol{\beta} \in \mathbb{R}^{p+1}\), \(\mathbf{y} \in \mathbb{R}^n\).

5.1.2.2 Normal Equation

Minimizing \(\|\mathbf{y} - \mathbf{X}\boldsymbol{\beta}\|^2\) with respect to \(\boldsymbol{\beta}\) yields:

\[\boldsymbol{\beta} = (\mathbf{X}^T \mathbf{X})^{-1} \mathbf{X}^T \mathbf{y}\]

This is solved in practice by forming \(\mathbf{X}^T\mathbf{X}\) and \(\mathbf{X}^T\mathbf{y}\), then using Gaussian elimination to solve the \((p+1) \times (p+1)\) system.

5.1.2.3 Geometric Interpretation

The prediction \(\hat{\mathbf{y}} = \mathbf{X}\boldsymbol{\beta}\) is the orthogonal projection of \(\mathbf{y}\) onto the column space of \(\mathbf{X}\). The residual \(\mathbf{y} - \hat{\mathbf{y}}\) is perpendicular to every column of \(\mathbf{X}\).

5.1.3 Complexity Analysis

For embedded use with small \(p\) (say \(\leq 10\)), the entire fit completes in microseconds.

5.1.4 Step-by-Step Walkthrough

Data: 3 samples, 1 feature.

Step 1 — Design matrix (with bias column):

\[\mathbf{X} = \begin{bmatrix} 1 & 1 \\ 1 & 2 \\ 1 & 3 \end{bmatrix}\]

Step 2 — Compute \(\mathbf{X}^T\mathbf{X}\) and \(\mathbf{X}^T\mathbf{y}\):

\[\mathbf{X}^T\mathbf{X} = \begin{bmatrix} 3 & 6 \\ 6 & 14 \end{bmatrix}, \quad \mathbf{X}^T\mathbf{y} = \begin{bmatrix} 11 \\ 23 \end{bmatrix}\]

Step 3 — Solve \(\begin{bmatrix} 3 & 6 \\ 6 & 14 \end{bmatrix} \boldsymbol{\beta} = \begin{bmatrix} 11 \\ 23 \end{bmatrix}\):

Forward elimination → back-substitution: \(\beta_1 = 1.5\), \(\beta_0 = 2/3 \approx 0.667\)

Result: \(\hat{y} = 0.667 + 1.5x\)

Prediction at \(x = 4\): \(\hat{y} = 0.667 + 6.0 = 6.667\)

5.1.5 Pitfalls & Edge Cases

• Multicollinearity. If features are nearly linearly dependent, \(\mathbf{X}^T\mathbf{X}\) is ill-conditioned and the solution is unstable. Consider regularization (Ridge/Lasso) or removing redundant features.
• Fewer samples than features (\(n < p+1\)). The system is underdetermined and \(\mathbf{X}^T\mathbf{X}\) is singular. At a minimum, \(n \geq p+1\).
• Outliers have outsized influence because the squared loss amplifies large residuals. Robust alternatives (Huber loss, RANSAC) exist outside this library.
• Extrapolation danger. The linear model has no mechanism to detect when it is being queried far from the training data range.
• Fixed-point precision. For Q15/Q31 types, features should be scaled to \([-1, 1)\) to avoid overflow in \(\mathbf{X}^T\mathbf{X}\).

5.1.6 Variants & Generalizations

5.1.7 Applications

• Sensor calibration — Fitting a linear transfer function between raw ADC counts and physical units.
• Trend estimation — Extracting linear trends from noisy time series (temperature, voltage drift).
• System identification — Estimating static gain or simple dynamic relationships.
• Feature importance — The magnitude of \(\beta_i\) indicates the influence of feature \(i\) (after feature scaling).
• Predictive maintenance — Modeling degradation rate from operating-condition features.

5.1.8 Connections to Other Algorithms

graph LR
    LR["Linear Regression"]
    GE["Gaussian Elimination"]
    YW["Yule-Walker"]
    NN["Neural Network"]
    LR --> GE
    YW -.->|"similar normal-equation structure"| LR
    NN -.->|"single linear layer = regression"| LR

5.2 Polynomial Least-Squares Fitting

5.2.1 Overview & Motivation

Sensor calibration curves, ADC linearization, thermistor transfer functions, and drift trends all require fitting a smooth curve to a discrete set of measured points. A degree-\(d\) polynomial captures these behaviors with only \(d+1\) coefficients, making evaluation at runtime a handful of multiply-adds rather than a table lookup or expensive transcendental.

The least-squares formulation finds the polynomial that minimizes the sum of squared residuals across all measurement samples. Unlike exact interpolation, it is robust to measurement noise: extra samples average out errors rather than being forced to pass through noisy points.

5.2.2 Mathematical Theory

5.2.2.1 The Model

Given \(n\) scalar observations \(\{(x_i, y_i)\}_{i=0}^{n-1}\), the degree-\(d\) polynomial model is

\[p(x) = c_0 + c_1 x + c_2 x^2 + \cdots + c_d x^d\]

The goal is to find the coefficient vector \(\mathbf{c} \in \mathbb{R}^{d+1}\) that minimizes

\[\min_{\mathbf{c}} \sum_{i=0}^{n-1} \bigl(y_i - p(x_i)\bigr)^2\]

5.2.2.2 Vandermonde Design Matrix

Stacking the model evaluations at all sample abscissae gives the Vandermonde matrix

\[\mathbf{V} \in \mathbb{R}^{n \times (d+1)}, \quad V_{i,j} = x_i^j\]

The least-squares problem then becomes \(\min_{\mathbf{c}} \|\mathbf{V}\mathbf{c} - \mathbf{y}\|^2\).

5.2.2.3 Normal Equations

Setting the gradient of the squared residual with respect to \(\mathbf{c}\) to zero yields

\[(\mathbf{V}^\top \mathbf{V})\,\mathbf{c} = \mathbf{V}^\top \mathbf{y}\]

The \((d+1)\times(d+1)\) matrix \(\mathbf{V}^\top\mathbf{V}\) is symmetric and, when the abscissae are distinct and \(n \geq d+1\), positive-definite. Its small size allows direct solution by Gaussian elimination or Cholesky factorization in bounded time on embedded hardware.

5.2.2.4 Horner Evaluation

Once \(\mathbf{c}\) is known, evaluating \(p(x)\) at a new point uses Horner’s method

\[p(x) = c_0 + x\bigl(c_1 + x\bigl(c_2 + \cdots + x\,c_d\bigr)\cdots\bigr)\]

This requires exactly \(d\) multiplications and \(d\) additions — optimal for a degree-\(d\) polynomial.

5.2.3 Complexity Analysis

All dimensions are compile-time constants; no heap allocation is required.

5.2.4 Step-by-Step Walkthrough

Data: \(n = 4\) samples, \(d = 2\) (quadratic fit).

Step 1 — Build \(\mathbf{V}\):

\[\mathbf{V} = \begin{bmatrix} 1 & 0 & 0 \\ 1 & 1 & 1 \\ 1 & 2 & 4 \\ 1 & 3 & 9 \end{bmatrix}\]

Step 2 — Normal equations:

\[\mathbf{V}^\top\mathbf{V} = \begin{bmatrix} 4 & 6 & 14 \\ 6 & 14 & 36 \\ 14 & 36 & 98 \end{bmatrix}, \qquad \mathbf{V}^\top\mathbf{y} = \begin{bmatrix} 4.5 \\ 7.25 \\ 19.75 \end{bmatrix}\]

Step 3 — Solve: Gaussian elimination → \(\mathbf{c} \approx [1,\,-0.5,\,0.25]^\top\).

Result: \(p(x) = 1 - 0.5\,x + 0.25\,x^2\).

Prediction at \(x = 1.5\):

\[p(1.5) = 0.25\cdot1.5^2 - 0.5\cdot1.5 + 1 = 0.5625 - 0.75 + 1 = 0.8125\]

5.2.5 Pitfalls & Edge Cases

• Ill-conditioning of the Vandermonde system. The condition number of \(\mathbf{V}^\top\mathbf{V}\) grows exponentially with \(d\) and with the spread of abscissae. Center and scale the abscissa \(x \leftarrow (x - \bar{x})/\sigma_x\) before fitting to reduce condition numbers by orders of magnitude. Recommended for \(d \geq 3\) or when abscissae are far from the origin.

• Degree selection. Over-fitting occurs when \(d\) is too large relative to \(n\) or to the signal-to-noise ratio. Keep \(d \leq 4\) for typical embedded calibration tasks.

• Exactly \(n = d+1\) points. The normal equation system has a unique solution equal to the interpolating polynomial; the residual is zero. The system is well-posed only if all abscissae are distinct.

• Repeated or nearly-coincident abscissae. \(\mathbf{V}^\top\mathbf{V}\) becomes singular or nearly so. Partial-pivoting in the Gaussian solver will flag this via really_assert; avoid duplicate \(x\) values in practice.

• Large degree with float arithmetic. Powers \(x^d\) for \(|x| \gg 1\) can exceed the float dynamic range. Centering/scaling eliminates this risk.

5.2.6 Variants & Generalizations

5.2.7 Applications

• Sensor linearization — converting thermistor resistance or pressure-sensor ADC counts to engineering units via a quadratic or cubic polynomial.
• Drift and aging compensation — fitting a polynomial to sampled drift data and subtracting the trend from future measurements.
• Compact lookup-table replacement — replacing a 256-entry table with a degree-3 polynomial evaluated in four MACs.
• Calibration curve storage — a handful of coefficients in flash replace a bulky lookup table.

5.2.8 Connections to Other Algorithms

graph LR
    PF["Polynomial Fitting"]
    GE["Gaussian Elimination"]
    LR["Linear Regression"]
    SG["Savitzky-Golay (planned)"]
    RLS["Recursive Least Squares"]

    PF --> GE
    PF -.->|"polynomial features = special case"| LR
    SG -.->|"local polynomial fit per window"| PF
    RLS -.->|"online counterpart"| PF

5.3 Yule-Walker Estimation

5.3.1 Overview & Motivation

Many signals — vibration data, speech waveforms, financial time series — exhibit short-term correlations. An autoregressive (AR) model captures these correlations by expressing the current sample as a linear combination of the previous \(p\) samples plus white noise. The Yule-Walker method estimates the AR coefficients directly from the signal’s autocovariance structure.

The appeal is threefold: 1. The resulting system of equations has Toeplitz structure, enabling efficient \(O(p^2)\) solvers. 2. The method is guaranteed to produce a stable model (all poles inside the unit circle) when the autocovariance matrix is positive definite. 3. It provides a parametric spectral estimate — the PSD is a smooth rational function rather than a noisy periodogram.

5.3.2 Mathematical Theory

5.3.2.1 Autoregressive Model

An AR(\(p\)) process:

\[x[t] = \varphi_1 x[t-1] + \varphi_2 x[t-2] + \cdots + \varphi_p x[t-p] + \varepsilon[t]\]

where \(\varepsilon[t]\) is white noise with variance \(\sigma^2\).

5.3.2.2 Yule-Walker Equations

Multiplying both sides by \(x[t-k]\) and taking expectations:

\[R[k] = \varphi_1 R[k-1] + \varphi_2 R[k-2] + \cdots + \varphi_p R[k-p], \quad k = 1, \ldots, p\]

where \(R[k] = E[x[t] \cdot x[t-k]]\) is the autocovariance at lag \(k\). In matrix form:

\[\underbrace{\begin{bmatrix} R[0] & R[1] & \cdots & R[p-1] \\ R[1] & R[0] & \cdots & R[p-2] \\ \vdots & & \ddots & \vdots \\ R[p-1] & R[p-2] & \cdots & R[0] \end{bmatrix}}_{\mathbf{R} \text{ (Toeplitz)}} \begin{bmatrix} \varphi_1 \\ \varphi_2 \\ \vdots \\ \varphi_p \end{bmatrix} = \begin{bmatrix} R[1] \\ R[2] \\ \vdots \\ R[p] \end{bmatrix}\]

5.3.2.3 Autocovariance Estimation

The biased estimator is used:

\[\hat{R}[k] = \frac{1}{N} \sum_{t=k}^{N-1} x[t] \cdot x[t-k]\]

after centering the series by subtracting its mean. The biased estimator (dividing by \(N\) instead of \(N-k\)) guarantees the Toeplitz matrix is positive semi-definite.

5.3.2.4 Spectral Interpretation

The PSD of the estimated AR model is:

\[S(f) = \frac{\sigma^2}{\left|1 - \sum_{i=1}^{p} \varphi_i \, e^{-j2\pi fi}\right|^2}\]

This gives a smooth, parametric spectral estimate.

5.3.3 Complexity Analysis

5.3.4 Step-by-Step Walkthrough

Input: \(x = [2, 4, 6, 5, 3, 1]\), AR order \(p = 2\).

Step 1 — Compute mean and center:

\(\bar{x} = 3.5\), centered: \(x' = [-1.5, 0.5, 2.5, 1.5, -0.5, -2.5]\)

Step 2 — Autocovariances (biased, divide by \(N = 6\)):

• \(R[0] = \frac{(-1.5)^2 + 0.5^2 + 2.5^2 + 1.5^2 + (-0.5)^2 + (-2.5)^2}{6} = \frac{2.25 + 0.25 + 6.25 + 2.25 + 0.25 + 6.25}{6} = 2.917\)
• \(R[1] = \frac{(-1.5)(0.5) + (0.5)(2.5) + (2.5)(1.5) + (1.5)(-0.5) + (-0.5)(-2.5)}{6} = \frac{-0.75 + 1.25 + 3.75 - 0.75 + 1.25}{6} = 0.792\)
• \(R[2] = \frac{(-1.5)(2.5) + (0.5)(1.5) + (2.5)(-0.5) + (1.5)(-2.5)}{6} = \frac{-3.75 + 0.75 - 1.25 - 3.75}{6} = -1.333\)

Step 3 — Build Toeplitz system:

\[\begin{bmatrix} 2.917 & 0.792 \\ 0.792 & 2.917 \end{bmatrix} \begin{bmatrix} \varphi_1 \\ \varphi_2 \end{bmatrix} = \begin{bmatrix} 0.792 \\ -1.333 \end{bmatrix}\]

Step 4 — Solve (e.g., via Levinson-Durbin or Gaussian elimination):

\(\varphi_1 \approx 0.478\), \(\varphi_2 \approx -0.587\)

Prediction: \(\hat{x}[6] = \bar{x} + \varphi_1 (x[5] - \bar{x}) + \varphi_2 (x[4] - \bar{x})\)

5.3.5 Pitfalls & Edge Cases

• Non-stationary data. The Yule-Walker method assumes wide-sense stationarity. Trends, seasonal components, or varying variance must be removed first.
• Model order selection. Choosing \(p\) too small misses important dynamics; too large overfits noise. Use AIC or BIC criteria.
• Near-singular autocovariance matrix. Happens when \(p\) is too large relative to \(N\), or the signal contains very little variation. The solver may fail or produce unreliable coefficients.
• Fixed-point range. Autocovariance values can be large (proportional to signal variance squared). Scale the input signal to prevent overflow.
• Biased vs. unbiased estimator. The biased estimator (dividing by \(N\)) is deliberately chosen to guarantee positive semi-definiteness; the unbiased estimator (dividing by \(N-k\)) does not.

5.3.6 Variants & Generalizations

5.3.7 Applications

• Speech analysis — AR models underlie linear predictive coding (LPC), the foundation of speech codecs.
• Spectral estimation — Parametric PSD estimation via the AR model gives smoother spectra than periodograms, especially for short records.
• System identification — Estimating transfer function poles from input-output data.
• Time series forecasting — Predicting next values in economic, meteorological, or sensor data.
• Vibration analysis — Extracting resonant frequencies from structural vibration measurements.

5.3.8 Connections to Other Algorithms

graph LR
    YW["Yule-Walker"]
    LD["Levinson-Durbin"]
    GE["Gaussian Elimination"]
    PSD["Power Spectral Density"]
    LR["Linear Regression"]
    YW --> LD
    YW --> GE
    YW -.->|"parametric alternative"| PSD
    LR -.->|"similar structure"| YW

5.4 Expectation-Maximization for Kalman Filter Parameters

5.4.1 Overview & Motivation

A Kalman filter requires four parameter matrices — the state-transition matrix \(F\), measurement matrix \(H\), process-noise covariance \(Q\), and measurement-noise covariance \(R\) — to be known a priori. In many real systems these parameters are unknown or only approximately known.

The Expectation-Maximization (EM) algorithm for linear Gaussian state-space models (Shumway & Stoffer, 1982; 2000) treats the latent state sequence \(x_{1:T}\) as missing data and iteratively finds the maximum-likelihood parameter estimate from a batch of observations \(z_{1:T}\). Each iteration of EM is guaranteed to be non-decreasing in log-likelihood, providing a principled convergence criterion.

This implementation follows the Shumway-Stoffer formulation and operates exclusively in float because typical dynamics (forces, velocities, torques) exceed the \([-1, 1]\) range of Q15/Q31 fixed-point formats.

5.4.2 Mathematical Theory

5.4.2.1 State-Space Model

\[ x_{t+1} = F x_t + w_t, \quad w_t \sim \mathcal{N}(0, Q) \]

\[ z_t = H x_t + v_t, \quad v_t \sim \mathcal{N}(0, R) \]

with \(x_0 \sim \mathcal{N}(\mu_0, P_0)\) and \(T\) observations \(z_0, \ldots, z_{T-1}\).

5.4.2.2 EM Overview

EM alternates between:

• E-step: compute expected sufficient statistics of \(x_{1:T}\) given current parameters and all observations, using the RTS smoother.
• M-step: analytically maximize the expected complete-data log-likelihood over parameters.

5.4.2.3 E-Step (RTS Smoother)

Run KalmanSmoother::Smooth() to obtain, for each \(t\):

5.4.2.4 Sufficient Statistics

From the smoother output, accumulate six matrices:

\[ A = \sum_{t=1}^{T-1} P_{t,t-1|T} \qquad B = \sum_{t=0}^{T-2} \hat{x}_{t|T}\hat{x}_{t|T}^\top + P_{t|T} \]

\[ C = \sum_{t=1}^{T-1} \hat{x}_{t|T}\hat{x}_{t|T}^\top + P_{t|T} \qquad D = \sum_{t=0}^{T-1} \hat{x}_{t|T}\hat{x}_{t|T}^\top + P_{t|T} \]

\[ W = \sum_{t=0}^{T-1} z_t \hat{x}_{t|T}^\top \qquad \Sigma_{zz} = \sum_{t=0}^{T-1} z_t z_t^\top \]

5.4.2.5 M-Step (Closed-Form Updates)

\[ F \leftarrow A B^{-1} \]

\[ Q \leftarrow \frac{C - F A^\top}{T - 1}, \quad Q \leftarrow \tfrac{1}{2}(Q + Q^\top) + \varepsilon I \]

\[ H \leftarrow W D^{-1} \]

\[ R \leftarrow \frac{\Sigma_{zz} - H W^\top}{T}, \quad R \leftarrow \tfrac{1}{2}(R + R^\top) + \varepsilon I \]

\[ \mu_0 \leftarrow \hat{x}_{0|T}, \quad P_0 \leftarrow P_{0|T} \]

The symmetrisation and ridge term \(\varepsilon I\) (\(\varepsilon = 10^{-6}\)) enforce that \(Q\) and \(R\) remain symmetric positive definite, preventing numerical collapse.

Matrix “division” — e.g. \(A B^{-1}\) — is implemented as SolveSystem(B, A^T)^T (Gaussian elimination) to avoid explicit matrix inversion.

5.4.2.6 Convergence

After each EM iteration, compute the log-likelihood \(\ell(\theta)\) from the RTS smoother. Convergence is declared when

\[ |\ell^{(k)} - \ell^{(k-1)}| < \delta \]

for a user-supplied tolerance \(\delta\).

5.4.3 Complexity Analysis

All storage is stack-allocated inside KalmanSmoother (owned by ExpectationMaximization) and in local M-step temporaries.

5.4.4 Step-by-Step Walkthrough

Consider \(N=1\), \(M=1\), \(T=3\), observations \(z=[0.5, 0.3, 0.8]\), initial guess \(F=1, H=1, Q=0.5, R=1, \mu_0=0, P_0=1\).

Iteration 1:

1. E-step via RTS smoother: obtain \(\hat{x}_{0|3}, \hat{x}_{1|3}, \hat{x}_{2|3}\) and corresponding covariances and lag cross-covariances.
2. Accumulate scalars:
   • \(A = P_{2,1|3} + P_{3,2|3}\) (indices 2 and 3 in the 1-indexed convention). (In the implementation indices 1..T-1 are used.)
   • \(B, C, D, W, \Sigma_{zz}\) as sums over the smoothed moments.
3. M-step: \(F \leftarrow A/B\), \(Q \leftarrow (C - F\cdot A)/(T-1)\), etc.
4. Evaluate new log-likelihood \(\ell^{(1)}\).

Repeat until \(|\ell^{(k)} - \ell^{(k-1)}| < \delta\) or maximum iterations reached.

5.4.5 Pitfalls & Edge Cases

• Local optima: EM converges to a local maximum of the likelihood, not necessarily the global maximum. The quality of the solution depends heavily on the initial parameter guess.
• Short sequences (\(T \ll N\)): With fewer observations than state dimensions, some sufficient-statistic matrices become rank-deficient and SolveSystem may produce inaccurate results. Prefer \(T \gg N\).
• \(Q\) or \(R\) collapse: Without the \(\varepsilon I\) regularisation, a maximally tight model (zero-noise) can collapse \(Q\) or \(R\) to zero, making the likelihood degenerate. If this happens, increase the regularisation or constrain the parameters.
• Float only: ExpectationMaximization has no template type parameter. Dynamics values typically exceed the \([-1, 1]\) range of Q15/Q31. Scale your state variables into \([-1, 1]\) and use a float wrapper if fixed-point EM is required.
• Re-entrant smoother: ExpectationMaximization owns a KalmanSmoother instance. The smoother state is overwritten on every call to Run(). Do not share an ExpectationMaximization instance across threads.

5.4.6 Variants & Generalizations

• EM for EKF/UKF: Replace the RTS smoother with an extended or unscented smoother for nonlinear models. The M-step remains identical.
• Constrained EM: Fix some parameters (e.g., \(H\) is known) by simply not updating them in the M-step.
• Online (incremental) EM: Accumulate sufficient statistics incrementally to handle streaming data.
• Structural constraints on \(F\): If \(F\) has a known parametric form (e.g., a companion matrix), the M-step can be modified to enforce the structure.

5.4.7 Applications

• System identification: Learn the dynamics \(F\) and noise statistics \(Q\), \(R\) of an unknown linear system from input-output data.
• Sensor calibration: Identify measurement noise covariance \(R\) from reference measurements.
• Hyperparameter tuning for KF: Replace hand-tuned \(Q\), \(R\) with data-driven estimates before deploying a real-time Kalman filter.
• Time-series model selection: Compare log-likelihoods of different state-space structures to select model order.

5.4.8 Connections to Other Algorithms

• filters::KalmanSmoother: Provides the complete E-step. ExpectationMaximization owns a KalmanSmoother instance internally.
• filters::KalmanFilter: After EM converges, pass the resulting EmParameters to KalmanFilter::SetStateTransition(), SetProcessNoise(), and SetMeasurementNoise() for real-time deployment.
• solvers::GaussianElimination: Used internally (via KalmanSmoother) for numerically stable matrix solves in both the smoother and the M-step.

5.5 Recursive Least Squares (RLS)

5.5.1 Overview & Motivation

Recursive Least Squares (RLS) is an online parameter estimation algorithm that incrementally updates a least-squares fit as new data arrives, without reprocessing past observations. It converges faster than gradient-descent methods (e.g., LMS) and is well-suited for real-time system identification and adaptive filtering on embedded systems.

5.5.2 Mathematical Theory

5.5.2.1 Problem Setting

Given a linear model \(y = \mathbf{x}^T \boldsymbol{\theta} + \epsilon\), where \(\mathbf{x} \in \mathbb{R}^p\) is the regressor, \(\boldsymbol{\theta} \in \mathbb{R}^p\) are the unknown parameters, and \(\epsilon\) is noise, RLS minimizes the exponentially weighted cost:

\[J(\boldsymbol{\theta}) = \sum_{k=1}^{n} \lambda^{n-k} (y_k - \mathbf{x}_k^T \boldsymbol{\theta})^2\]

where \(\lambda \in (0, 1]\) is the forgetting factor.

5.5.2.2 Update Equations

Given a new observation \((\mathbf{x}, y)\):

1. Gain vector: \(\mathbf{g} = P \mathbf{x}\)
2. Denominator: \(d = \lambda + \mathbf{x}^T P \mathbf{x}\)
3. Innovation (pre-update error): \(e = y - \mathbf{x}^T \hat{\boldsymbol{\theta}}\)
4. Parameter update: \(\hat{\boldsymbol{\theta}} \leftarrow \hat{\boldsymbol{\theta}} + \frac{\mathbf{g} \cdot e}{d}\)
5. Covariance update: \(P \leftarrow \frac{1}{\lambda}\left(P - \frac{\mathbf{g} \mathbf{g}^T}{d}\right)\)

5.5.2.3 Forgetting Factor

• \(\lambda = 1\): Full memory — equivalent to batch least squares with all past data equally weighted.
• \(\lambda < 1\): Exponential forgetting — recent data is weighted more heavily, enabling tracking of time-varying systems.
• Typical values: \(0.95 \leq \lambda \leq 1.0\).

5.5.2.4 Convergence Evaluation

The implementation provides convergence diagnostics via EstimationMetrics: - Innovation: Pre-update prediction error magnitude - Residual: Post-update prediction error magnitude - Uncertainty: Trace of the covariance matrix \(P\) (overall parameter uncertainty)

5.5.3 Complexity Analysis

Per-update cost is \(O(p^2)\) for \(p\) parameters, dominated by the covariance update. This is significantly cheaper than the \(O(p^3)\) batch solution via matrix inversion.

5.5.4 Step-by-Step Walkthrough

Estimating the slope of \(y = 2x\) with RLS (\(p = 2\): bias + slope, \(\lambda = 1\)):

Convergence is achieved in 2 steps for this noiseless system.

5.5.5 Pitfalls & Edge Cases

• Covariance blow-up: With \(\lambda < 1\) and insufficient excitation, the covariance matrix \(P\) can grow unbounded. Monitor the uncertainty metric and consider covariance resetting.
• Numerical conditioning: The covariance update can lose symmetry and positive-definiteness due to floating-point errors. For long-running applications, consider using the Joseph form: \(P \leftarrow (I - K\mathbf{x}^T)P(I - K\mathbf{x}^T)^T + K K^T / \lambda\).
• Initial covariance: A large initial \(P\) (e.g., \(\alpha I\) with \(\alpha = 100\)) gives fast initial convergence but higher initial sensitivity. A small \(P\) gives conservative updates.
• Regressor scaling: Large differences in regressor magnitudes can cause numerical issues. Normalize inputs when possible.

5.5.6 Variants & Generalizations

• Weighted RLS: Use a general weighting matrix instead of scalar forgetting.
• QR-decomposition RLS: Better numerical stability by avoiding explicit covariance updates.
• Kalman filter: RLS is equivalent to a steady-state Kalman filter for the linear regression problem.
• Regularized RLS: Add a regularization term \(\mu \|\boldsymbol{\theta}\|^2\) to prevent overfitting.

5.5.7 Applications

• Online system identification for adaptive control
• Adaptive noise cancellation
• Channel equalization in communications
• Real-time parameter tracking in engine/motor control
• Sensor calibration

5.5.8 Connections to Other Algorithms

• Linear Regression: Batch (offline) version of the same problem. Linear regression solves via matrix inversion; RLS updates incrementally.
• Yule-Walker: Both estimate AR model parameters, but Yule-Walker uses autocorrelation statistics (offline) while RLS processes samples sequentially.
• Kalman Filter: RLS is a special case of the Kalman filter where the state transition is identity and the process noise is zero.
• Gradient Descent: LMS/gradient descent is a simpler adaptive algorithm with \(O(p)\) cost but slower convergence compared to RLS’s \(O(p^2)\).

5.6 Estimator Consistency Metrics (NEES / NIS)

5.6.1 Overview & Motivation

State estimators such as the Kalman filter, Extended Kalman filter, and Unscented Kalman filter produce both a state estimate and a covariance matrix that quantifies estimation uncertainty. A filter is said to be consistent when the true errors are statistically compatible with the reported covariance — that is, the filter neither overestimates nor underestimates its own uncertainty.

Raw error metrics such as RMSE cannot distinguish a consistent filter (correct covariance) from an inconsistent one (wrong covariance that happens to produce low errors in a specific trial). Consistency metrics address this gap by normalising errors with respect to the predicted covariance and comparing the result against a chi-squared distribution.

5.6.2 Mathematical Theory

5.6.2.1 Normalised Estimation Error Squared (NEES)

Given the state error \(\varepsilon_k = x_k - \hat{x}_{k|k}\) and the posterior state covariance \(P_{k|k}\), the NEES at time step \(k\) is

\[\varepsilon_k^{\mathsf{T}} P_{k|k}^{-1} \varepsilon_k\]

Under the hypothesis that the filter is consistent and the errors are Gaussian, this quantity is chi-squared distributed with \(n_x\) degrees of freedom, where \(n_x\) is the state dimension.

5.6.2.2 Normalised Innovation Squared (NIS)

Given the innovation \(\nu_k = z_k - \hat{z}_{k|k-1}\) and the innovation covariance \(S_k\), the NIS at time step \(k\) is

\[\nu_k^{\mathsf{T}} S_k^{-1} \nu_k\]

Under consistency, NIS follows a chi-squared distribution with \(n_z\) degrees of freedom, where \(n_z\) is the measurement dimension. Unlike NEES, NIS is computable without knowledge of the true state and is therefore usable in real deployments.

5.6.2.3 Chi-Squared Confidence Gate

A two-sided 95% confidence region for a single NEES or NIS sample is

\[\chi^2_{n,\,0.025} \;\le\; \varepsilon_k^{\mathsf{T}} P_{k|k}^{-1} \varepsilon_k \;\le\; \chi^2_{n,\,0.975}\]

where \(\chi^2_{n,p}\) is the \(p\)-th quantile of the chi-squared distribution with \(n\) degrees of freedom.

5.6.2.4 Time-Averaged Gate

Averaging NEES or NIS over \(N\) independent samples gives a statistic that is chi-squared with \(N \cdot n\) degrees of freedom, scaled by \(1/N\). The 95% bounds for the time-averaged value are therefore

\[\frac{\chi^2_{Nn,\,0.025}}{N} \;\le\; \bar{\varepsilon} \;\le\; \frac{\chi^2_{Nn,\,0.975}}{N}\]

Averaging reduces the variance of the consistency estimate and is the standard approach in Monte-Carlo filter evaluation.

5.6.2.5 Embedded Computation

Rather than forming \(P^{-1}\) explicitly, the quadratic form \(\varepsilon^{\mathsf{T}} P^{-1} \varepsilon\) is computed by solving \(P z = \varepsilon\) for \(z\) (via Gaussian elimination with partial pivoting) and then computing \(\varepsilon^{\mathsf{T}} z\). This avoids matrix inversion, reduces floating-point operations, and is numerically more stable.

Chi-squared quantiles for degrees of freedom 1–10 at the 95% confidence level are stored in a compile-time array. Dimensions outside this range are rejected via static_assert.

5.6.3 Complexity Analysis

All storage is stack-allocated; \(n\) is bounded at compile time by Dim.

5.6.4 Step-by-Step Walkthrough

Consider a 2-D state (\(n = 2\)) with error \(\varepsilon = [1, 2]^{\mathsf{T}}\) and covariance \(P = \mathrm{diag}(1, 4)\).

1. Solve \(P z = \varepsilon\): \(z_1 = 1/1 = 1\), \(z_2 = 2/4 = 0.5\).
2. Compute dot product: \(\varepsilon^{\mathsf{T}} z = 1 \cdot 1 + 2 \cdot 0.5 = 2.0\).
3. Compare against \(\chi^2_{2, 0.025} \approx 0.051\) and \(\chi^2_{2, 0.975} \approx 7.38\).
4. Since \(2.0 \in [0.051, 7.38]\), the filter is declared consistent at the 95% level.

5.6.5 Pitfalls & Edge Cases

A singular covariance matrix causes the linear solve to fail; the implementation detects near-zero diagonal pivots and returns std::nullopt. Callers must check the optional before using the value.

A single sample NEES or NIS value has high variance under chi-squared; a filter may appear inconsistent simply due to random variation. Time-averaging over many Monte-Carlo runs is the statistically correct procedure.

NEES requires ground-truth state access and is therefore only applicable in simulation. NIS is the consistency monitor of choice for deployed systems.

5.6.6 Variants & Generalizations

The average NEES over a Monte-Carlo ensemble of \(M\) runs and \(K\) time steps yields \(M \cdot K\) samples; consistency must hold jointly across the ensemble, not just per run. Extensions to non-Gaussian cases (e.g., particle filters) use empirical quantiles rather than chi-squared bounds.

5.6.7 Applications

• Monte-Carlo evaluation of Kalman-family filters during design and tuning.
• Online innovation gating in Kalman filters (rejecting outlier measurements whose NIS exceeds the threshold).
• Diagnosing process-noise or measurement-noise mis-specification.
• Formal consistency testing as part of filter validation before deployment.

5.6.8 Connections to Other Algorithms

NEES and NIS are statistical companions to the standard Kalman filter update step. They depend on the covariance propagation produced by the filters/active family (KF, EKF, UKF). The linear solve reuses solvers::GaussianElimination, and the state-error representation aligns with estimators::EstimationMetrics.

5.7 LMS / NLMS Adaptive Filter

5.7.1 Overview & Motivation

Real-time systems frequently need filters whose characteristics are unknown in advance or change over time. A fixed-coefficient FIR filter cannot cancel acoustic echo from an unknown loudspeaker-to-microphone path, equalize a time-varying channel, or identify the impulse response of a vibrating structure it has never seen before. The Least Mean Squares (LMS) algorithm and its normalized variant (NLMS) address this by adjusting a FIR filter’s coefficients online, one sample at a time, without requiring matrix inversions or covariance storage.

5.7.2 Mathematical Theory

5.7.2.1 Prerequisites

Let \(\mathbf{w} \in \mathbb{R}^{N}\) be the weight vector (FIR coefficients) and \(\mathbf{x}_n \in \mathbb{R}^{N}\) the tapped-delay-line vector at time \(n\), with \(x_n\) as the newest sample and \(x_{n-N+1}\) the oldest. The filter output is

\[y_n = \mathbf{w}_n^\top \mathbf{x}_n.\]

Given a desired signal \(d_n\), the estimation error is

\[e_n = d_n - y_n.\]

5.7.2.2 Core Definitions

The instantaneous squared error \(J_n = e_n^2\) is a quadratic function of \(\mathbf{w}_n\). Its gradient with respect to \(\mathbf{w}\) is

\[\nabla J_n = -2 e_n \mathbf{x}_n.\]

5.7.2.3 Derivation

LMS takes a noisy steepest-descent step using the instantaneous gradient:

\[\mathbf{w}_{n+1} = \mathbf{w}_n - \frac{\mu}{2} \nabla J_n = \mathbf{w}_n + \mu e_n \mathbf{x}_n.\]

NLMS divides the step by the input energy to make the effective step size input-level independent:

\[\mathbf{w}_{n+1} = \mathbf{w}_n + \frac{\mu}{\epsilon + \|\mathbf{x}_n\|^2} e_n \mathbf{x}_n,\]

where \(\epsilon > 0\) is a small regularizer that prevents division by zero during silence (\(\|\mathbf{x}_n\|^2 \approx 0\)).

5.7.2.4 Proof of Correctness

Under the independence assumption (successive input vectors are uncorrelated), the expected weight vector converges to the Wiener solution \(\mathbf{w}^* = \mathbf{R}^{-1}\mathbf{p}\) (where \(\mathbf{R}\) is the input autocorrelation matrix and \(\mathbf{p}\) is the cross-correlation vector) provided

\[0 < \mu < \frac{2}{\lambda_{\max}(\mathbf{R})},\]

a sufficient condition being \(\mu < 2 / (N \cdot \sigma_x^2)\). For NLMS the stability range simplifies to \(0 < \mu < 2\).

5.7.3 Complexity Analysis

No covariance matrix is stored; memory is exactly two vectors of length \(N\) (weights and delay line).

5.7.4 Step-by-Step Walkthrough

Consider a 2-tap system with true plant \(\mathbf{w}^* = [0.5,\ 0.5]^\top\), \(\mu = 0.1\), and a white input sequence.

After many iterations the error approaches zero and the weights stabilize near \([0.5,\ 0.5]^\top\).

5.7.5 Pitfalls & Edge Cases

• Stability bound: exceeding \(\mu_{\max}\) causes exponential weight growth. For stationary white input, \(\mu_{\max} = 2 / (N\sigma_x^2)\).
• Misadjustment: even at steady state, random gradient noise keeps the weights oscillating around \(\mathbf{w}^*\), adding excess mean-squared error proportional to \(\mu N\).
• Slow convergence vs stability tradeoff: small \(\mu\) gives low misadjustment but slow tracking; large \(\mu\) tracks fast but overshoots.
• Silent input: \(\|\mathbf{x}_n\|^2 = 0\) causes a singularity in plain NLMS; \(\epsilon\) prevents this but freezes adaptation during silence.
• Non-persistent excitation: if the input does not excite all directions, some weights may drift. Leaky-LMS (\(\mathbf{w} \leftarrow (1-\rho)\mathbf{w} + \mu e \mathbf{x}\)) bounds the drift.

5.7.6 Variants & Generalizations

• NLMS: normalizes step by input energy; removes input-power dependence; recommended for signals with variable amplitude.
• Leaky LMS: adds a forgetting term \(\rho \ll 1\) to bound weight norm under insufficient excitation.
• Block LMS: accumulates gradients over a block before updating; lower update rate but more FFT-friendly for long filters.
• Sign-LMS / Sign-Error LMS: replaces \(e\) or \(\mathbf{x}\) with their signs; extremely simple hardware implementation at the cost of convergence rate.
• RLS: replaces the stochastic gradient with the exact least-squares update; converges in \(O(N)\) steps but requires \(O(N^2)\) memory and time per update.

5.7.7 Applications

• Acoustic echo cancellation: adapt the loudspeaker-to-microphone path in teleconferencing.
• Adaptive noise cancellation: estimate and subtract a correlated noise source (e.g. engine noise on an aircraft headset).
• Channel equalization: track a time-varying ISI channel in wireless modems.
• Active noise control / vibration control: generate an anti-phase signal to cancel mechanical vibration.
• Online system identification: recover the impulse response of an unknown plant in real time.

5.7.8 Connections to Other Algorithms

• RecursiveLeastSquares: the exact-gradient counterpart; converges in far fewer samples but needs \(O(N^2)\) storage and time per step. Prefer RLS for short filter lengths or fast convergence; prefer LMS/NLMS for embedded, resource-constrained paths.
• FIR (fixed-coefficient): LMS turns a fixed FIR into a self-tuning one; the underlying convolution is identical.
• ConvolutionCorrelation: used to compute the cross-correlation \(\mathbf{p}\) in the batch Wiener solution; LMS approximates this online.

6 Filters

6.1 Kalman Filter

6.1.1 Overview & Motivation

In any real system, sensors are noisy and models are imperfect. The Kalman filter answers a deceptively simple question: given a noisy measurement and an imperfect prediction, what is the best estimate of the true state?

The answer is an optimal, recursive, minimum-variance estimator for linear systems with Gaussian noise. At each time step it performs two operations:

1. Predict — propagate the state and covariance forward using the system model.
2. Update — correct the prediction using the new measurement, weighted by the Kalman gain, which automatically balances trust in the model vs. trust in the sensor.

The filter is computationally lightweight (matrix operations on small state vectors), requires no storage of past measurements, and adapts its confidence in real time via the covariance matrix.

6.1.2 Mathematical Theory

6.1.2.1 State-Space Model

Discrete-time linear system with process noise \(w\) and measurement noise \(v\):

\[x_k = F_{k-1} x_{k-1} + B_{k-1} u_{k-1} + w_{k-1}, \quad w \sim \mathcal{N}(0, Q)\] \[z_k = H_k x_k + v_k, \quad v \sim \mathcal{N}(0, R)\]

where: - \(x_k \in \mathbb{R}^n\) — state vector - \(z_k \in \mathbb{R}^m\) — measurement vector - \(F\) — state transition matrix (\(n \times n\)) - \(B\) — control input matrix (\(n \times l\)) - \(H\) — measurement matrix (\(m \times n\)) - \(Q\) — process noise covariance (\(n \times n\)) - \(R\) — measurement noise covariance (\(m \times m\))

6.1.2.2 Predict Step

\[\hat{x}_k^- = F_{k-1} \hat{x}_{k-1}\] \[P_k^- = F_{k-1} P_{k-1} F_{k-1}^T + Q_{k-1}\]

6.1.2.3 Update Step

\[y_k = z_k - H_k \hat{x}_k^- \qquad \text{(innovation)}\] \[S_k = H_k P_k^- H_k^T + R_k \qquad \text{(innovation covariance)}\] \[K_k = P_k^- H_k^T S_k^{-1} \qquad \text{(Kalman gain)}\] \[\hat{x}_k = \hat{x}_k^- + K_k y_k \qquad \text{(state update)}\] \[P_k = (I - K_k H_k) P_k^- (I - K_k H_k)^T + K_k R_k K_k^T \qquad \text{(Joseph form covariance update)}\]

6.1.2.4 Optimality

The Kalman filter is the minimum mean-square error (MMSE) estimator among all linear estimators. Under the assumption that \(w\) and \(v\) are Gaussian, it is also the MMSE among all estimators (linear or not).

6.1.3 Complexity Analysis

For typical embedded use with \(n \leq 10\) and \(m \leq 5\), each step completes in microseconds.

6.1.4 Step-by-Step Walkthrough

System: Estimating position and velocity from noisy position measurements.

State: \(x = \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix}\), \(\Delta t = 1\) s.

\[F = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}, \quad H = \begin{bmatrix} 1 & 0 \end{bmatrix}, \quad Q = \begin{bmatrix} 0.1 & 0 \\ 0 & 0.1 \end{bmatrix}, \quad R = [1]\]

Initial: \(\hat{x}_0 = [0, 0]^T\), \(P_0 = \begin{bmatrix} 10 & 0 \\ 0 & 10 \end{bmatrix}\)

Measurement: \(z_1 = 1.2\) (true position = 1.0)

Predict:

\[\hat{x}_1^- = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 0 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}\]

\[P_1^- = F P_0 F^T + Q = \begin{bmatrix} 20.1 & 10 \\ 10 & 10.1 \end{bmatrix}\]

Update:

\[y_1 = 1.2 - [1\; 0] \begin{bmatrix} 0 \\ 0 \end{bmatrix} = 1.2\]

\[S_1 = H P_1^- H^T + R = 20.1 + 1 = 21.1\]

\[K_1 = P_1^- H^T / S_1 = \begin{bmatrix} 20.1 \\ 10 \end{bmatrix} / 21.1 = \begin{bmatrix} 0.953 \\ 0.474 \end{bmatrix}\]

\[\hat{x}_1 = \begin{bmatrix} 0 \\ 0 \end{bmatrix} + \begin{bmatrix} 0.953 \\ 0.474 \end{bmatrix} \cdot 1.2 = \begin{bmatrix} 1.143 \\ 0.569 \end{bmatrix}\]

The filter quickly moves toward the measurement because the initial covariance \(P_0\) is large (low confidence). Over subsequent steps, \(P\) shrinks and the gain decreases — the filter becomes more reliant on the model.

6.1.5 Pitfalls & Edge Cases

• Model mismatch. If the true system is nonlinear or the noise is non-Gaussian, the Kalman filter is suboptimal. Consider the Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF) for moderate nonlinearities.
• Covariance divergence. Numerical errors can cause \(P\) to lose positive-definiteness over time. Symmetrize \(P\) after each update; consider a square-root or UD factorization form for long-running filters.
• Tuning \(Q\) and \(R\). These are rarely known exactly. Increasing \(Q\) makes the filter more responsive (trusts model less); increasing \(R\) makes it smoother (trusts measurements less). Tune empirically or use adaptive methods.
• Innovation covariance singularity. If \(S_k\) is singular, the measurement does not provide new information in some direction. Regularize \(R\) or reduce the measurement model.
• Missing measurements. If no measurement is available, skip the update step entirely and only predict. The covariance will grow, correctly reflecting increasing uncertainty.
• Fixed-point caution. Matrix inversions and multiplications in the update step can overflow Q15/Q31 ranges. Use floating-point or carefully scale all matrices.

6.1.6 Variants & Generalizations

6.1.7 Applications

• GPS/INS navigation — Fusing inertial measurements (high rate, drifty) with GPS (low rate, noisy).
• Object tracking — Radar, lidar, or camera-based tracking of moving targets.
• Sensor fusion — Combining accelerometer, gyroscope, and magnetometer for attitude estimation.
• Process control — State estimation for model-based controllers (LQG = LQR + Kalman).
• Battery state-of-charge estimation — Estimating internal states from voltage and current measurements.
• Econometrics — Estimating latent economic variables from noisy indicators.

6.1.8 Connections to Other Algorithms

graph LR
    KF["Kalman Filter"]
    LQR["LQR Controller"]
    DARE["DARE Solver"]
    PID["PID Controller"]
    KF -->|"state estimate"| LQR
    DARE -->|"steady-state gain"| KF
    KF -->|"filtered measurement"| PID

6.2 Extended Kalman Filter

6.2.1 Overview & Motivation

Many real-world systems are nonlinear: a pendulum’s restoring force is \(-\sin\theta\), not \(-\theta\); a radar measured range depends on \(\sqrt{x^2 + y^2}\), not a linear combination of states. The standard Kalman filter assumes linear dynamics \((x_k = Fx_{k-1})\) and cannot handle this directly.

The Extended Kalman Filter (EKF) extends the Kalman framework to nonlinear systems by linearizing the state transition and measurement functions around the current estimate at each step. It uses the same predict–update structure, but replaces the constant system matrices with Jacobians evaluated at the current operating point.

This makes the EKF the simplest and most widely-used nonlinear state estimator — at the cost of requiring the user to supply Jacobian functions and the assumption that linearization remains accurate within the filter’s uncertainty.

6.2.2 Mathematical Theory

6.2.2.1 Nonlinear State-Space Model

\[x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}, \quad w \sim \mathcal{N}(0, Q)\] \[z_k = h(x_k) + v_k, \quad v \sim \mathcal{N}(0, R)\]

where \(f(\cdot)\) and \(h(\cdot)\) are arbitrary (differentiable) nonlinear functions.

6.2.2.2 Predict Step

1. Compute the Jacobian of \(f\) at the current estimate:

\[F_k = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1}}\]

2. Nonlinear state prediction:

\[\hat{x}_k^- = f(\hat{x}_{k-1}, u_{k-1})\]

3. Covariance prediction (using the linearized model):

\[P_k^- = F_k P_{k-1} F_k^T + Q_{k-1}\]

6.2.2.3 Update Step

1. Compute the Jacobian of \(h\) at the predicted state:

\[H_k = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_k^-}\]

2. Nonlinear innovation:

\[y_k = z_k - h(\hat{x}_k^-)\]

3. Innovation covariance, Kalman gain, state and covariance updates follow the standard Kalman filter equations using the Jacobian \(H_k\):

\[S_k = H_k P_k^- H_k^T + R_k\] \[K_k = P_k^- H_k^T S_k^{-1}\] \[\hat{x}_k = \hat{x}_k^- + K_k y_k\] \[P_k = (I - K_k H_k) P_k^-\]

6.2.3 Complexity Analysis

6.2.4 Step-by-Step Walkthrough

System: Estimating angle and angular velocity of a simple pendulum from noisy angle measurements.

State: \(x = \begin{bmatrix} \theta \\ \dot\theta \end{bmatrix}\), \(\Delta t = 0.1\) s.

Nonlinear dynamics: \[f(x) = \begin{bmatrix} \theta + \dot\theta \Delta t \\ \dot\theta - \sin(\theta) \Delta t \end{bmatrix}\]

Jacobian: \[F = \begin{bmatrix} 1 & \Delta t \\ -\cos(\theta) \Delta t & 1 \end{bmatrix}\]

Linear measurement: \(h(x) = \theta\), \(H = [1 \; 0]\).

At \(\hat{x}_0 = [0.3, 0]^T\): - Predict: \(\hat{x}_1^- = [0.3, -0.0296]^T\) (velocity decreases due to restoring force \(-\sin 0.3\)) - Update with \(z_1 = 0.28\): The Kalman gain weights the measurement against the prediction, pulling \(\theta\) toward 0.28.

After 50 iterations the EKF tracks the true pendulum oscillation closely, despite the nonlinear dynamics.

6.2.5 Pitfalls & Edge Cases

• Linearization error. The EKF assumes the nonlinearity is approximately linear within the filter’s uncertainty. Large initial errors or highly nonlinear systems can cause divergence.
• Jacobian correctness. Incorrect Jacobians silently produce wrong estimates. Always validate Jacobians numerically (e.g., finite-difference check) before deploying.
• Non-observable modes. If \(H\) does not observe all states, convergence of unobserved states depends entirely on the model \(f\).
• Large time steps. For stiff or fast dynamics, large \(\Delta t\) amplifies linearization errors. Reduce the step size or use a higher-order integration scheme.
• Covariance symmetry. Numerical drift can break symmetry of \(P\); the Joseph form of the covariance update is more robust for long runs.

6.2.6 Variants & Generalizations

• Iterated EKF (IEKF). Re-linearizes the measurement function at the updated state estimate and repeats the update step until convergence. Reduces linearization error in the measurement model at the cost of multiple Jacobian evaluations per time step.
• Second-Order EKF. Includes second-order terms of the Taylor expansion in the prediction and update equations. Improves accuracy for moderately nonlinear systems but requires Hessians of \(f\) and \(h\).
• Error-State EKF. Tracks a small error state \(\delta x\) rather than the full state. Popular in inertial navigation where the nominal trajectory is integrated separately and the filter corrects deviations.
• EKF with Joseph Form. Replaces the standard covariance update \(P = (I - KH)P^-\) with the symmetric Joseph form \(P = (I - KH)P^-(I - KH)^T + KRK^T\) to guarantee positive-definiteness under finite-precision arithmetic.
• EKF with Control Input. When the control \(u\) is present, the state transition becomes \(f(x, u)\) and the Jacobian is \(\partial f / \partial x\) evaluated at \((\hat{x}, u)\). This variant is supported directly in the implementation via the ControlSize template parameter.

6.2.7 Comparison with Other Filters

6.2.8 Applications

• Attitude estimation — Fusing gyroscope and accelerometer for orientation (nonlinear quaternion kinematics).
• Robot localization — SLAM (Simultaneous Localization and Mapping) with range-bearing measurements.
• Pendulum control — Estimating angle and angular velocity for swing-up or balancing.
• Battery management — Nonlinear electrochemical models for state-of-charge estimation.
• Spacecraft navigation — Orbit determination from range and range-rate measurements.

6.2.9 Connections to Other Algorithms

graph LR
    EKF["Extended Kalman Filter"]
    KF["Kalman Filter"]
    UKF["Unscented Kalman Filter"]
    EL["Euler-Lagrange"]
    EKF -->|"linear limit"| KF
    UKF -->|"alternative to"| EKF
    EL -->|"plant model"| EKF

6.3 Unscented Kalman Filter

6.3.1 Overview & Motivation

The Extended Kalman Filter (EKF) linearizes nonlinearities using Jacobians. This works well for mild nonlinearities, but the first-order approximation can introduce significant errors when:

• The nonlinear functions are highly curved (large second derivatives).
• The state uncertainty is large (the linearization point is far from the true state).
• Jacobians are difficult or expensive to derive analytically.

The Unscented Kalman Filter (UKF) addresses all three problems by replacing linearization with a deterministic sampling approach. Instead of approximating the nonlinear function, it approximates the probability distribution by choosing a small set of carefully weighted sample points — called sigma points — and propagating them through the true nonlinear function. The statistics (mean and covariance) are then reconstructed from the transformed points.

This captures the true mean and covariance to second-order accuracy for any nonlinearity, without computing a single Jacobian.

6.3.2 Mathematical Theory

6.3.2.1 Sigma Point Generation

Given state estimate \(\hat{x} \in \mathbb{R}^n\) and covariance \(P\), generate \(2n+1\) sigma points: