A Field Guide to Embedded DSP, Control, and Estimation Algorithms
October 23, 2026
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.
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}\).
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.
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(N \log_2 N)\) | \(O(N)\) | \(N\) must be a power of 2 |
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.
Input: \(x = [1, 2, 3, 4]\), \(N = 4\)
Step 1 — Bit-reversal permutation
| Index (binary) | Reversed | Value |
|---|---|---|
| 0 (00) | 0 (00) | 1 |
| 1 (01) | 2 (10) | 3 |
| 2 (10) | 1 (01) | 2 |
| 3 (11) | 3 (11) | 4 |
Reordered: \([1, 3, 2, 4]\)
Step 2 — Stage 1 (size-2 butterflies), twiddle \(W_2^0 = 1\)
Result: \([4, -2, 6, -2]\)
Step 3 — Stage 2 (size-4 butterfly), twiddles \(W_4^0 = 1\), \(W_4^1 = -j\)
Output: \(X = [10,\; -2+2j,\; -2,\; -2-2j]\)
Verification: \(X[0] = 1+2+3+4 = 10\) ✓
| Variant | Key Difference |
|---|---|
| Mixed-radix FFT | Supports arbitrary composite \(N\) using radix-2, -3, -5, etc. stages |
| Split-radix FFT | Reduces multiplications by ~33 % compared to standard radix-2 |
| Real-valued FFT | Exploits conjugate symmetry of real inputs to halve work and memory |
| Bluestein’s algorithm | Handles arbitrary \(N\) by converting DFT to a circular convolution |
| Inverse FFT | Same structure; negate twiddle exponents and scale by \(1/N\) |
graph LR
FFT --> PSD["Power Spectral Density"]
FFT --> DCT["Discrete Cosine Transform"]
WIN["Window Functions"] --> FFT
FFT -.-> DK["Durand-Kerner (polynomial eval)"]| Algorithm | Relationship |
|---|---|
| Power Spectral Density | Calls FFT internally on each windowed segment |
| Discrete Cosine Transform | Computed via FFT with input rearrangement and twiddle-factor post-processing |
| Window Functions | Applied before FFT to reduce spectral leakage |
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.
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.
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].\]
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\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Forward | \(O(N \log N / 2)\) | \(O(N)\) stack (bounded) | Two half-length DFTs plus \(O(N)\) combine pass |
| Inverse | \(O(N \log N / 2)\) | \(O(N)\) stack (bounded) | Two half-length IDFTs plus interleave pass |
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).
Input: \(x = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\) (unit impulse, \(N = 16\)).
static_assert enforces it at
compile time.ConvolutionCorrelation.PowerDensitySpectrum.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.
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.
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\).
\[x_i[n] = x[n + iS], \quad i = 0, 1, \ldots, K-1\]
\[w_i[n] = w[n] \cdot x_i[n]\]
\[P_i[k] = \frac{1}{N} |W_i[k]|^2\]
\[\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).
\[\Delta f = \frac{f_s}{N}\]
Increasing segment size \(N\) improves resolution but reduces the number of averages \(K\) for a fixed-length record.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(K \cdot N \log N)\) | \(O(N)\) | \(K\) segments, each requiring one FFT |
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.
Input: \(x = [1, 3, 5, 7, 6, 4, 2, 0]\), segment size \(N=4\), 50 % overlap, rectangular window.
Step 1 — Segmentation (step \(S = 2\)):
| Segment | Indices | Values |
|---|---|---|
| \(x_0\) | 0–3 | \([1, 3, 5, 7]\) |
| \(x_1\) | 2–5 | \([5, 7, 6, 4]\) |
| \(x_2\) | 4–7 | \([6, 4, 2, 0]\) |
Step 2 — Window (rectangular → no change)
Step 3 — FFT and periodogram for \(x_0 = [1, 3, 5, 7]\):
(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}\).
| Variant | Key Difference |
|---|---|
| Bartlett’s method | Non-overlapping segments, rectangular window (special case of Welch with 0 % overlap) |
| Modified periodogram | Single segment with a non-rectangular window |
| Multitaper method | Uses multiple orthogonal windows (tapers) per segment for lower bias |
| Cross-spectral density | Computes \(S_{xy}\) between two signals instead of the auto-spectrum |
| Coherence | \(|\gamma_{xy}|^2 = |S_{xy}|^2 / (S_{xx} S_{yy})\) — measures linear dependence between two signals at each frequency |
graph LR
SIG["Input Signal"] --> WIN["Window Functions"]
WIN --> FFT["Fast Fourier Transform"]
FFT --> PSD["Power Spectral Density"]
PSD --> YW["Yule-Walker (parametric alternative)"]| Algorithm | Relationship |
|---|---|
| Fast Fourier Transform | Core building block — each segment is transformed via FFT |
| Window Functions | Applied to each segment before FFT to control leakage |
| Yule-Walker | Parametric alternative — estimates PSD from an AR model instead of averaging periodograms |
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.
This implementation uses the orthonormal DCT-II. For a length-\(N\) sequence \(x[n]\):
\[X[0] = \frac{1}{\sqrt{N}} \sum_{n=0}^{N-1} x[n]\]
\[X[k] = \sqrt{\frac{2}{N}} \sum_{n=0}^{N-1} x[n] \cos\!\left(\frac{\pi}{N}\left(n + \tfrac{1}{2}\right) k\right), \quad k = 1, \ldots, N-1\]
This normalisation makes the transform matrix unitary, so Parseval’s theorem holds exactly: \(\sum_k X[k]^2 = \sum_n x[n]^2\). The inverse (orthonormal DCT-III) recovers \(x[n]\):
\[x[n] = \frac{X[0]}{\sqrt{N}} + \sqrt{\frac{2}{N}} \sum_{k=1}^{N-1} X[k] \cos\!\left(\frac{\pi}{N}\left(n + \tfrac{1}{2}\right) k\right)\]
The DCT can be computed via the DFT of a reordered sequence:
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(N \log N)\) | \(O(N)\) | Dominated by the internal FFT; twiddle post-processing is \(O(N)\) |
The reordering step and twiddle multiplication are both linear, so the overall cost is determined by the FFT.
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,\; 0,\; -3+j]\]
Step 3 — Apply twiddle factors and orthonormal scale (\(N=4\), \(W[k] = e^{-j\pi k/8}\))
| \(k\) | \(W[k]\) | \(W[k] \cdot Y[k]\) | \(X[k]\) |
|---|---|---|---|
| 0 | — | — | \(10 / \sqrt{4} = 5\) |
| 1 | \(e^{-j\pi/8}\) | \(\approx -3.154 + 0.224j\) | \(\sqrt{0.5} \cdot (-3.154) \approx -2.23\) |
| 2 | \(e^{-j\pi/4}\) | \(0\) | \(0\) |
| 3 | \(e^{-j3\pi/8}\) | \(\approx -0.224 + 3.154j\) | \(\sqrt{0.5} \cdot (-0.224) \approx -0.16\) |
Output: \(X \approx [5,\; -2.23,\; 0,\; -0.16]\)
Notice how most of the energy is in \(X[0]\) (the DC component) — energy compaction in action. The orthonormal scale ensures \(\sum_k X[k]^2 = 1^2+2^2+3^2+4^2 = 30\).
| Variant | Description |
|---|---|
| DCT-I | Symmetric extension; used in some filter bank designs |
| DCT-III (IDCT) | Inverse of DCT-II; used for reconstruction in JPEG decoding |
| DCT-IV | Self-inverse; basis of the MDCT used in MP3 and AAC |
| MDCT | Modified DCT with 50 % overlap; foundation of modern audio codecs |
| 2-D DCT | Applied row-then-column for image compression (JPEG 8×8 blocks) |
graph LR
DCT["Discrete Cosine Transform"]
FFT["Fast Fourier Transform"]
WIN["Window Functions"]
FFT --> DCT
WIN -.-> DCT| Algorithm | Relationship |
|---|---|
| Fast Fourier Transform | DCT is computed via FFT with input reordering and twiddle post-processing |
| Window Functions | Some DCT applications use windowing to control boundary effects |
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.
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.
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}\]
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.
| Detector | Time per sample | State words |
|---|---|---|
| Peak Hold | O(1) — 1 mul, 1 compare, 1 abs | 1 scalar |
| Zero-Crossing Counter | O(1) — 1 compare, 1 abs | 1 scalar + 1 uint32 |
| RMS Envelope | O(1) — 1 MAC + 1 sqrt | 1 scalar |
No buffers, no windows, no dynamic allocation.
| n | \(x_n\) | \(m_n\) | \(d \cdot p_{n-1}\) | \(p_n\) |
|---|---|---|---|---|
| 0 | 0.8 | 0.8 | 0.0 | 0.8 |
| 1 | 0.0 | 0.0 | 0.4 | 0.4 |
| 2 | 0.0 | 0.0 | 0.2 | 0.2 |
| 3 | 0.0 | 0.0 | 0.1 | 0.1 |
| n | \(x_n\) | prev sign | curr sign | crossed | C |
|---|---|---|---|---|---|
| 0 | +1 | 0 | + | no | 0 |
| 1 | -1 | + | - | yes | 1 |
| 2 | +1 | - | + | yes | 2 |
| 3 | -1 | + | - | yes | 3 |
\(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\).
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.
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.
\[r = 10^{L_{\mathrm{dB}}/20}\]
This inverse is exact for all finite \(L_{\mathrm{dB}}\); no guard is needed on the output side.
\(\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.
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\).
| Operation | Time | Space | Notes |
|---|---|---|---|
ToDecibels |
O(1) | O(1) | One log10 + one max + one
mul |
FromDecibels |
O(1) | O(1) | One pow |
AttenuationDb |
O(1) | O(1) | Two ToDecibels + one subtraction |
RippleDb |
O(1) | O(1) | Two ToDecibels + one subtraction |
No state, no buffers. All operations are pure functions.
Converting a ratio of 10 to decibels:
Round-trip for \(r = 0.5\):
ToDecibels(0.5) = \(20 \cdot
\log_{10}(0.5) \approx -6.0206\,\mathrm{dB}\).FromDecibels(-6.0206) = \(10^{-6.0206/20} \approx 0.5\).log10; the
floor guard prevents propagation into downstream computations.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.FrequencyResponse::Calculate() already returns magnitude in
dB using \(20 \log_{10}\); these
helpers provide the same conversion for ad-hoc analysis.ToDecibels avoids duplication.control_analysis::FrequencyResponse internally applies
\(20 \log_{10}(\|H\|)\) on its
magnitude output vector; these helpers are the scalar equivalent exposed
for library consumers.RippleDb feeds directly
into filter-design acceptance testing alongside the
step/transient-response metrics in the evaluation primitives
family.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.
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)\]
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 the \(N\) input samples, one further step with \(x[N] = 0\) gives \(s[N] = c\,s[N-1] - s[N-2]\), and the DFT bin follows:
\[X[k] = s[N] - s[N-1]\,e^{-j2\pi k/N}\]
Separating real and imaginary parts:
\[\mathrm{Re}\{X[k]\} = s[N] - s[N-1]\cos\!\left(\tfrac{2\pi k}{N}\right)\]
\[\mathrm{Im}\{X[k]\} = s[N-1]\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.
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\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Per sample | \(O(1)\) | \(O(1)\) | One multiply, two adds, no table lookup |
| Full block | \(O(N)\) | \(O(1)\) | Two state words; trig only at block end |
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.
Consider \(N = 4\), \(k = 1\), \(x = [1, 0, -1, 0]\).
\(c = 2\cos(\pi/2) = 0\)
| \(n\) | \(x[n]\) | \(s[n] = x[n] + 0\cdot s[n-1] - s[n-2]\) |
|---|---|---|
| 0 | 1 | \(1 + 0 - 0 = 1\) |
| 1 | 0 | \(0 + 0 - 0 = 0\) (note: \(s[-1]=0\)) |
| 2 | -1 | \(-1 + 0 - 1 = -2\) |
| 3 | 0 | \(0 + 0 - 0 = 0\) |
Extra step: \(s[4] = 0\cdot s[3] - s[2] = -(-2) = 2\).
\(\cos(2\pi/4) = 0\), \(\sin(2\pi/4) = 1\)
\(\mathrm{Re}\{X[1]\} = s[4] - s[3]\cdot 0 = 2\)
\(\mathrm{Im}\{X[1]\} = s[3]\cdot 1 = 0\)
Direct DFT check: \(X[1] = 1 - (-1) = 2\). Matches.
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.
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.
The recurrence and the final step need only \(\cos\omega\) and \(\sin\omega\) of the bin, \(\omega = 2\pi k/N\), evaluated once per configuration. A caller that already evaluates them by other means — a sine table refined by a short series, or an exact value for a known angle — can supply the unit bin phasor \(e^{j\omega}\) directly. On targets whose sine and cosine come from a software math library, that keeps the library’s general argument-reduction code, several kilobytes on a Cortex-M4, out of the image. The phasor’s accuracy sets the resonator frequency: an error \(\varepsilon\) in \(\cos\omega\) detunes it by about \(\varepsilon / \sin\omega\) radians per sample, which over a block of \(N\) samples rotates the result by about \(N\varepsilon / (2\sin\omega)\).
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.
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.
The Discrete Wavelet Transform (DWT) decomposes a finite-length signal into a hierarchy of approximation and detail coefficients at multiple scales. Unlike the DFT, which projects onto infinitely-supported sinusoids, wavelets are compactly supported: they are simultaneously localised in time and frequency. This dual localisation makes the DWT the canonical tool for detecting transients, edges, and non-stationary features — phenomena that a pure frequency representation blurs across the entire spectrum.
In embedded condition-monitoring and edge-ML pipelines, the DWT is valued for three properties: its \(O(N)\) complexity (a factor of \(\log_2 N\) faster than the FFT), its ability to concentrate signal energy in very few coefficients (sparsity), and the perfect-reconstruction guarantee that lets processed coefficients be inverted exactly.
A single DWT stage passes a length-\(N\) sequence \(x[n]\) through two complementary finite-impulse- response filters, then downsamples each output by two.
\[ c_A[i] = \sum_{k=0}^{P-1} h[k]\, x[2i + k \bmod N], \qquad c_D[i] = \sum_{k=0}^{P-1} g[k]\, x[2i + k \bmod N], \qquad i = 0,\ldots,\tfrac{N}{2}-1 \]
where \(h\) is the scaling (low-pass) filter with \(P\) taps and \(g\) is the wavelet (high-pass) filter. For an orthogonal wavelet the two filters satisfy the quadrature-mirror relation:
\[g[k] = (-1)^k\, h[P - 1 - k]\]
This guarantees that the two sub-bands together cover the full bandwidth without overlap or gap.
The approximation \(c_A\) is fed back into the same filter pair. After \(L\) levels the coefficient array contains \(L\) detail bands and one coarse approximation:
\[ \text{coeffs} = [\underbrace{c_D^{(1)}}_{N/2},\; \underbrace{c_D^{(2)}}_{N/4},\; \ldots,\; \underbrace{c_D^{(L)}}_{N/2^L},\; \underbrace{c_A^{(L)}}_{N/2^L}] \]
The layout is critical: level \(\ell\) detail starts at offset \(\sum_{j=1}^{\ell-1} N/2^j\) within the coefficient buffer, and the final approximation occupies the last \(N/2^L\) positions.
The synthesis (inverse) stage upsamples each sub-band by two and applies the dual filter pair \((h_r, g_r)\). For orthogonal wavelets the synthesis filters are time-reversals of the analysis filters:
\[h_r[k] = h[P - 1 - k], \qquad g_r[k] = g[P - 1 - k]\]
Combined, analysis and synthesis satisfy \(H(z)H_r(z^{-1}) + G(z)G_r(z^{-1}) = 2\), the alias-cancellation and distortion-free conditions, giving exact reconstruction:
\[\hat{x}[n] = x[n]\]
to floating-point rounding.
For an orthogonal wavelet the DWT is a unitary transform:
\[\sum_{n=0}^{N-1} x[n]^2 = \sum_{n=0}^{N-1} c[n]^2\]
where \(c\) contains all detail and approximation coefficients.
Haar (\(P = 2\)): \(h = [1,\, 1]/\sqrt{2}\). Piecewise-constant basis; discontinuous; simplest possible.
Daubechies-2 (\(P = 4\), denoted db2 or D4): coefficients chosen so the wavelet has two vanishing moments — it annihilates linear trends. This produces smoother reconstructions and better energy compaction for smooth signals.
| Case | Time | Space | Notes |
|---|---|---|---|
| Forward | \(O(N)\) | \(O(N)\) | Geometric series: \(N + N/2 + \cdots \leq 2N\) |
| Inverse | \(O(N)\) | \(O(N)\) | Identical pass through synthesis bank |
Memory is one length-\(N\) coefficient buffer, two length-\(N/2\) working arrays, and the static filter table of size \(P\) — no heap allocation.
Input: \(x = [1, 2, 3, 4]\), Haar wavelet, \(L = 1\).
Stage 1 (analysis):
| \(i\) | \(2i\) | \(2i+1\) | \(c_A[i] = (x[2i]+x[2i+1])/\sqrt{2}\) | \(c_D[i] = (x[2i]-x[2i+1])/\sqrt{2}\) |
|---|---|---|---|---|
| 0 | 0 | 1 | \((1+2)/\sqrt{2} \approx 2.121\) | \((1-2)/\sqrt{2} \approx -0.707\) |
| 1 | 2 | 3 | \((3+4)/\sqrt{2} \approx 4.950\) | \((3-4)/\sqrt{2} \approx -0.707\) |
Coefficient buffer (layout detail | approx):
\[\text{coeffs} = [-0.707,\; -0.707,\; 2.121,\; 4.950]\]
Reconstruction: upsample \(c_A\) and \(c_D\), apply synthesis filters, add:
\[\hat{x} = [1.0,\; 2.0,\; 3.0,\; 4.0] \checkmark\]
-ffast-math
floating-point associativity relaxes; reconstruction residuals may reach
\(10^{-5}\) rather than \(10^{-7}\) for 32-bit floats.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.
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.
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\).
| Quantity | Formula |
|---|---|
| Amplitude (envelope) | \(A(t) = | z(t) | = \sqrt{x^2 + \mathcal{H}^2\{x\}}\) |
| Phase | \(\phi(t) = \arg z(t) = \operatorname{atan2}(\mathcal{H}\{x\}, x)\) |
| Frequency | \(f_i(t) = \frac{1}{2\pi}\frac{d\phi}{dt}\) (phase unwrapped before differencing) |
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\).
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.
| Method | Time per call | Space | Notes |
|---|---|---|---|
| FFT (block) | \(O(N \log N)\) | \(2N\) complex words | Exact, latency \(N\) |
| FIR (streaming) | \(O(P)\) per sample | \(P\) words state | Approx., latency \((P-1)/2\) |
| Feature extraction | \(O(1)\) | None | atan2, sqrt, subtract |
\(P\) = number of FIR taps.
FFT method on \(N = 8\), input \(x = \cos(2\pi n / 8)\):
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.Keyword-spotting and small speech models on microcontrollers need a compact, perceptually motivated description of a short audio frame. The log-mel spectrum and its decorrelated form, the Mel-Frequency Cepstral Coefficients (MFCC), compress a frame of \(N\) samples into a few tens of values that follow the coarse spectral envelope on a frequency axis that matches human pitch perception. Since Davis and Mermelstein (1980) the pipeline has been the standard speech front end, and every stage has a fixed size, so it fits a no-heap, real-time budget.
The pipeline is: window → real FFT → power spectrum → triangular mel filterbank → floored natural log (log-mel) → orthonormal DCT-II (MFCC, first \(C\) coefficients).
Two mel scales are in common use.
HTK (O’Shaughnessy):
\[m = 2595 \log_{10}\!\left(1 + \frac{f}{700}\right), \qquad f = 700\left(10^{m/2595} - 1\right)\]
Slaney (Auditory Toolbox, librosa default): linear below 1 kHz and logarithmic above, with \(f_{sp} = 200/3\) Hz per mel, a break point \(f_b = 1000\) Hz (\(m_b = f_b / f_{sp} = 15\)) and step \(\lambda = \ln(6.4)/27\):
\[m = \begin{cases} f / f_{sp} & f < f_b \\ m_b + \ln(f / f_b)/\lambda & f \ge f_b \end{cases} \qquad f = \begin{cases} f_{sp}\, m & m < m_b \\ f_b\, e^{\lambda (m - m_b)} & m \ge m_b \end{cases}\]
Both pairs are exact inverses, so a Hz → mel → Hz round trip reproduces the input up to rounding.
For \(M\) bands over \([f_\text{min}, f_\text{max}]\), place \(M+2\) points equally spaced in mel and map them back to Hz: \(f_0 = f_\text{min} < f_1 < \dots < f_{M+1} = f_\text{max}\). Band \(m\) rises from \(f_m\) to its centre \(f_{m+1}\) and falls to \(f_{m+2}\). FFT bin \(k\) sits at \(\nu_k = k f_s / N\):
\[H_m(k) = \max\!\left(0,\; \min\!\left(\frac{\nu_k - f_m}{f_{m+1} - f_m},\; \frac{f_{m+2} - \nu_k}{f_{m+2} - f_{m+1}}\right)\right)\]
Sparse structure. If \(\nu_k\) lies in segment \(j\), \([f_j, f_{j+1}]\), only two triangles are non-zero there. Band \(j\) rises with \(r_k = (\nu_k - f_j)/(f_{j+1} - f_j)\) and band \(j-1\) falls with \(1 - r_k\). Storing one segment index and one weight per bin is therefore enough, so applying the filterbank costs one pass over the \(N/2+1\) bins and needs no \(M \times (N/2+1)\) weight matrix.
Partition of unity. Without normalisation, for every bin between the first centre \(f_1\) and the last centre \(f_M\), the two active weights add up to \(r_k + (1 - r_k) = 1\). This holds per bin and exactly, not just in the continuous limit.
Slaney (area) normalisation. Each triangle is scaled
by \(2/(f_{m+2} - f_m)\), so its
continuous area is 1 and wide high-frequency bands do not dominate. The
scale and the normalisation are independent choices. With the Slaney
scale, Slaney normalisation and the same \(f_s, N, M, f_\text{min}, f_\text{max}\),
the weights equal librosa’s filters.mel defaults.
\[S_m = \sum_k H_m(k)\,|X_k|^2, \qquad \tilde S_m = \ln \max(S_m, \varepsilon)\]
The floor \(\varepsilon\) bounds silent or band-limited frames at \(\ln \varepsilon\) instead of \(-\infty\). The cepstrum is the orthonormal DCT-II, truncated to the first \(C \le M\) terms:
\[c_k = s_k \sum_{m=0}^{M-1} \tilde S_m \cos\!\left(\frac{\pi k (2m+1)}{2M}\right), \qquad s_0 = \sqrt{1/M},\; s_{k>0} = \sqrt{2/M}\]
This is scipy.fft.dct(·, type=2, norm='ortho'). The
\(C \times M\) cosine basis and the
\(N\) window coefficients are computed
once at construction.
| Stage | Time | Memory (words) | Notes |
|---|---|---|---|
| Construction | \(O(N + M + C M)\) | — | Mel points, bin→segment map, window, DCT basis |
| Window + real FFT | \(O(N \log N)\) | \(O(N)\) | Dominant cost |
| Power spectrum | \(O(N)\) | \(N/2+1\) | |
| Filterbank | \(O(N)\) | \(2(N/2+1) + M\) | Two weights per bin plus a gain per band |
| Log | \(O(M)\) | \(M\) | |
| DCT-II | \(O(C M)\) | \(C M\) | Direct product with the precomputed basis |
With \(N=512\), \(M=40\), \(C=13\), the tables take about 1.6 k words, where a dense filterbank matrix would need 10 k.
\(f_s = 16\) kHz, \(N = 256\) (\(\Delta\nu = 62.5\) Hz), \(M = 20\), \(C = 13\), \([20, 8000]\) Hz, Slaney scale and normalisation, periodic Hann window, \(\varepsilon = 10^{-10}\).
Keyword spotting (DS-CNN / “Hello Edge” class models), speaker verification, audio event detection and voice activity detection. A typical MCU configuration is 25–40 ms frames, a 10–20 ms hop, \(M = 40\) and \(C = 10\)–\(13\).
The stage consumes the half spectrum of the real-input FFT and the analysis windows. The cepstral stage is a truncated DCT-II. Power-spectral-density estimation shares the window → FFT → \(|X|^2\) front.
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.
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\).
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.
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}\]
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]\).
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.
| Operation | Time | Space (extra) | Notes |
|---|---|---|---|
| Linear convolution | \(O(MK)\) | \(O(1)\) | Writes into caller-owned buffer |
| Circular convolution | \(O(N^2)\) | \(O(1)\) | Same as linear for equal-length input |
| Cross-correlation | \(O(MK)\) | \(O(K)\) | Reverses one operand on the stack |
| Auto-correlation | \(O(M^2)\) | \(O(1)\) | Alias of cross-correlation |
| Fast convolution | \(O(L \log L)\) | \(O(L)\) | \(L = 2^{\lceil\log_2(M+K-1)\rceil}\) |
The fast path is beneficial when \(MK > L \log_2 L\), roughly when both operands exceed 32–64 samples.
Linear convolution of \(x = [1, 2, 3]\) and \(h = [1, 1]\) (output length \(= 4\)):
| \(n\) | Active \(k\) range | Computation | \(y[n]\) |
|---|---|---|---|
| 0 | \(k=0\) | \(1 \cdot 1\) | 1 |
| 1 | \(k=0,1\) | \(2 \cdot 1 + 1 \cdot 1\) | 3 |
| 2 | \(k=1,2\) | \(3 \cdot 1 + 2 \cdot 1\) | 5 |
| 3 | \(k=2\) | \(3 \cdot 1\) | 3 |
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).
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\).AutoCorrelation.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.
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\).
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}))\]
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}\]
| Case | Time | Space | Notes |
|---|---|---|---|
| Average | \(O(N \cdot (P+Q))\) | \(O(N)\) | \(N\) frequency points, \(P+Q\) coefficients |
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:
math::ToFloat() before evaluation. This ensures sufficient
dynamic range for the complex arithmetic.std::arg function
returns phase in \((-\pi, \pi]\). Phase
unwrapping is not performed.Fir and
Iir filters.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.
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}]\).
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(N_g \cdot n^2 \cdot k)\) | \(O(N_g \cdot n)\) | \(N_g\) = gain steps, \(n\) = polynomial order, \(k\) = Durand-Kerner iterations per step |
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).
System: \(G(s) = K \cdot \frac{s + 1}{s(s + 2)}\), sweep \(K\) from 0.01 to 10.
Step 1 — Identify poles and zeros
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 poleAs \(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\).
| Variant | Key Difference |
|---|---|
| Complementary root locus | Traces roots for \(K < 0\) (positive feedback) |
| Root contour | Varies two or more parameters simultaneously |
| Discrete root locus | Same concept applied to \(z\)-domain polynomials for digital control |
| Evans rules | Analytical rules for sketching the root locus by hand (angle/magnitude criteria) |
graph LR
RL["Root Locus"] --> DK["Durand-Kerner"]
RL --> LQR["LQR (pole placement alternative)"]
DK --> RL| Algorithm | Relationship |
|---|---|
| Durand-Kerner | Used at each gain step to find the roots of the characteristic polynomial |
| LQR | Alternative approach to pole placement — LQR optimizes a cost rather than manually selecting gain via root locus |
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.
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\).
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\).
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\).
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\).
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.
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.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Controllability matrix | \(O(n^2 m)\) per column, \(n\) columns → \(O(n^3 m)\) | \(O(nm)\) output | Iterative matmul |
| Observability matrix | \(O(n^2 p)\) per row-block, \(n\) blocks → \(O(n^3 p)\) | \(O(np)\) output | Iterative matmul |
| Rank (Gaussian elim) | \(O(r^2 \min(r,c))\) where \(r,c\) are matrix dims | \(O(rc)\) copy | Partial pivoting |
| Gramian (Lyapunov fixed-point) | \(O(K n^3)\) iterations | \(O(n^2)\) | Up to 200 iterations |
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.
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.
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\).
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.
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).
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.
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.
| Method | Time | Space | Notes |
|---|---|---|---|
| ZOH | \(O((n+m)^3)\) | \((n+m)^2\) augmented mat | One matrix exponential via scaling/squaring |
| Tustin | \(O(n^3)\) | \(O(n^2)\) | One \(n \times n\) LU factorisation |
| Forward Euler | \(O(n^2)\) | \(O(n^2)\) | Pure matrix multiply + add |
| Backward Euler | \(O(n^3)\) | \(O(n^2)\) | One \(n \times n\) LU factorisation |
All working memory is allocated on the stack; no heap is used.
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.
expm of the Van Loan augmented block; the
exponential accuracy directly sets the ZOH accuracy.LinearTimeInvariant structs; the discrete model drops
straight into the state-update loop.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.
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.
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.
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.
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\).
| Operation | Time | Space | Notes |
|---|---|---|---|
ToControllableCanonical |
\(O(n)\) | \(O(n^2)\) | Direct coefficient placement |
ToObservableCanonical |
\(O(n^2)\) | \(O(n^2)\) | One transpose after CCF |
ToTransferFunction |
\(O(n^4)\) | \(O(n^2)\) | \(n\) steps, each \(O(n^3)\) matrix-matrix |
Memory is \(O(n^2)\) for \(A\) and \(O(n)\) for coefficient arrays; all
allocations are std::array on the stack.
\(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\)):
Result: \(H(s) = \dfrac{s + 2}{s^2 + 3s + 2}\). Identical to the original.
LinearTimeInvariant runtime for embedded execution.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.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.
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).
\[ 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\}\).
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(1)\) | \(O(1)\) | Two comparisons, one state bit, one select |
No arithmetic on the signal path — only comparisons — so the relay introduces no numerical error and is exactly representable in any floating-point format.
Setup: band \([\theta_L, \theta_H] = [-0.2, 0.2]\), outputs \(u_L = 0\), \(u_H = 1\), initial state Low.
| Step | \(x[k]\) | Condition | \(s[k]\) | \(u[k]\) |
|---|---|---|---|---|
| 1 | 0.0 | Low, \(x < 0.2\) | Low | 0 |
| 2 | 0.3 | Low, \(x \geq 0.2\) → switch | High | 1 |
| 3 | 0.1 | High, \(x > -0.2\) → hold | High | 1 |
| 4 | −0.3 | High, \(x \leq -0.2\) → switch | Low | 0 |
| 5 | 0.0 | Low, \(x < 0.2\) → hold | Low | 0 |
| Variant | Key Difference |
|---|---|
| Plain comparator | \(\Delta = 0\); no memory, chatters on noise |
| Asymmetric band | \(\theta_H\) and \(\theta_L\) not symmetric around the set-point; biases the duty cycle |
| Three-state relay | Adds a dead-band output level \(u_0\); used in motor direction control |
| Adaptive hysteresis | Band width tracks signal variance online to maintain a target switching rate |
| Relay feedback test (Åström–Hägglund) | Deliberate oscillation under relay feedback to identify the ultimate gain/period for automatic PID tuning |
| Algorithm | Relationship |
|---|---|
| PID Controller | The relay’s limit cycle can be used to identify PID tuning parameters via the Åström–Hägglund relay-feedback test |
| Saturation / Rate Limiter | Continuous-output counterpart for actuator constraint; often combined with a relay in cascaded loops |
In discrete-time control, most feedback laws drive the plant asymptotically toward a target — the state error decays exponentially but never reaches zero in finite time. Deadbeat control takes the opposite approach: given a linear, time-invariant plant and a desired terminal state, it computes a sequence of inputs that drives the state to the target exactly in a fixed, finite number of samples.
This property is valuable whenever settling time is a hard constraint. A motor current-loop running at 10 kHz, a grid-tied inverter that must complete a transient in two PWM cycles, or a precision positioning stage that cannot overshoot — all benefit from the guarantee that the error is identically zero after \(N\) steps, rather than merely small.
The runtime cost of a deadbeat controller is identical to LQR: a single matrix-vector multiply per sample. The extra cost (gain computation via the reachability matrix and pseudo-inverse) is paid once at initialization.
\[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 \in \mathbb{R}^{n \times n}\) is the state-transition matrix, and \(B \in \mathbb{R}^{n \times m}\) is the input matrix.
After \(N\) steps, applying inputs \(u[0], u[1], \ldots, u[N-1]\) from initial state \(x[0]\), the state satisfies:
\[x[N] = A^N x[0] + \Gamma_N \begin{bmatrix} u[0] \\ u[1] \\ \vdots \\ u[N-1] \end{bmatrix}\]
where the reachability matrix is:
\[\Gamma_N = \begin{bmatrix} A^{N-1}B & A^{N-2}B & \cdots & AB & B \end{bmatrix} \in \mathbb{R}^{n \times Nm}\]
The system is N-step reachable to target \(r\) from \(x[0]\) if and only if:
\[r - A^N x[0] \in \text{range}(\Gamma_N)\]
When \(\Gamma_N\) has full row rank (i.e., \(n \leq Nm\) and the plant is completely reachable in \(N\) steps), the minimum-Euclidean-norm input sequence that achieves \(x[N] = r\) is:
\[\mathbf{u}^* = \Gamma_N^+ \left( r - A^N x[0] \right)\]
where \(\Gamma_N^+ = \Gamma_N^T (\Gamma_N \Gamma_N^T)^{-1}\) is the Moore-Penrose pseudo-inverse, computed here via Singular Value Decomposition for numerical robustness.
Only the first \(m\) elements of \(\mathbf{u}^*\) are applied at each sample (the remaining elements are recomputed at the next step). Let \(\Pi_0\) denote the operator that extracts the first \(m\) rows of a matrix. Define the open-loop state-feedback gain:
\[K_x = \Pi_0 \left(\Gamma_N^+\right) \cdot A^N \in \mathbb{R}^{m \times n}\]
For unity closed-loop DC gain in receding-horizon (closed-loop) operation, \(K_r\) must satisfy:
\[B \, K_r = I - A + B \, K_x\]
For square systems (\(m = n\)), \(B\) is invertible and the unique solution is:
\[K_r = B^{-1}(I - A + B \, K_x) \in \mathbb{R}^{m \times n}\]
For non-square systems (\(m < n\)), the open-loop first-step gain is used directly:
\[K_r = \Pi_0 \left(\Gamma_N^+\right) \in \mathbb{R}^{m \times n}\]
The resulting deadbeat control law is:
\[u[k] = K_r \, r - K_x \, x[k]\]
This is a constant-gain affine state-feedback law that can be evaluated with two matrix-vector multiplies per sample, identical in cost to LQR.
Under this law, the state satisfies \(x[k+N] = r\) exactly (in exact arithmetic) for any initial condition \(x[k]\) reachable to \(r\) in \(N\) steps. The closed-loop poles are all placed at the origin — the system has a finite settling time of exactly \(N\) samples.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Build \(\Gamma_N\) (init) | \(O(N \cdot n^2 m)\) | \(O(n \cdot Nm)\) | \(N\) matrix-matrix multiplies of size \(n \times n\) by \(n \times m\) |
| SVD of \(\Gamma_N\) (init) | \(O(n^2 \cdot Nm)\) | \(O(n \cdot Nm)\) | Golub-Kahan bidiagonalization + diagonalization |
| Compute \(A^N\) (init) | \(O(N \cdot n^3)\) | \(O(n^2)\) | \(N\) matrix-matrix multiplies |
| Control step (online) | \(O(n \cdot m)\) | \(O(n \cdot m)\) | Two matrix-vector multiplies |
The initialization cost is negligible for the embedded use case: it is incurred once, and the gains are stored as constant matrices on the stack.
System: Double integrator — position and velocity driven by acceleration input, discretized at \(\Delta t = 0.1\,\text{s}\):
\[A = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0.005 \\ 0.1 \end{bmatrix}\]
Goal: Drive \(x[2] = [5, 0]^T\) from \(x[0] = [0, 0]^T\) in \(N = 2\) steps.
Step 1 — Build \(\Gamma_2\):
\[\Gamma_2 = \begin{bmatrix} AB & B \end{bmatrix} = \begin{bmatrix} 0.015 & 0.005 \\ 0.1 & 0.1 \end{bmatrix}\]
Step 2 — Compute \(\Gamma_2^{-1}\) (square and invertible here):
\[\det(\Gamma_2) = 0.015 \times 0.1 - 0.005 \times 0.1 = 0.001\]
\[\Gamma_2^{-1} = \begin{bmatrix} 100 & -5 \\ -100 & 15 \end{bmatrix}\]
Step 3 — Extract \(K_r\) (first row of \(\Gamma_2^{-1}\)):
\[K_r = \begin{bmatrix} 100 & -5 \end{bmatrix}\]
Step 4 — Compute \(A^2\):
\[A^2 = \begin{bmatrix} 1 & 0.2 \\ 0 & 1 \end{bmatrix}\]
Step 5 — Compute \(K_x = K_r A^2\):
\[K_x = \begin{bmatrix} 100 & -5 \end{bmatrix} \begin{bmatrix} 1 & 0.2 \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} 100 & 15 \end{bmatrix}\]
Step 6 — Apply control law (\(r = [5, 0]^T\), \(x[0] = [0, 0]^T\)):
\[u[0] = K_r r - K_x x[0] = 500 - 0 = 500\]
\[x[1] = Ax[0] + Bu[0] = [2.5,\; 50]^T\]
\[u[1] = 500 - K_x [2.5, 50]^T = 500 - (250 + 750) = -500\]
\[x[2] = Ax[1] + Bu[1] = [2.5 + 5,\; 50] + [-2.5,\; -50] = [5,\; 0]^T \checkmark\]
| Variant | Key Difference |
|---|---|
| 1-step deadbeat (\(N = 1\)) | Requires \(B\) to be square and invertible; gain is simply \(B^{-1}\) and \(B^{-1}A\) |
| N-step deadbeat (\(N > 1\)) | Handles systems where \(m < n\); uses the reachability matrix pseudo-inverse |
| Receding-horizon deadbeat | Recomputes the full \(N\)-step sequence at every sample (equivalent to unconstrained MPC with finite horizon) |
| Robust deadbeat | Adds structured uncertainty to \(A\), \(B\) and minimizes worst-case settling time |
| Output deadbeat | Drives the output \(y = Cx\) to target rather than the full state; requires an observer |
graph LR
DB["Deadbeat Control"]
SVD["SVD (Pseudo-Inverse)"]
LQR["LQR Controller"]
MPC["MPC Controller"]
LTI["LTI Plant Model"]
SVD --> DB
LTI --> DB
DB -.->|"finite horizon, no constraints"| MPC
DB -.->|"optimal alternative with noise"| LQR| Algorithm | Relationship |
|---|---|
| SVD Solver | Used to compute the pseudo-inverse of \(\Gamma_N\) during gain initialization |
| LQR Controller | LQR is the optimal alternative when measurement noise is present; places poles away from origin |
| MPC Controller | Deadbeat is MPC with a finite horizon equal to \(N\) and no inequality constraints |
| LTI Plant Model | Provides \((A, B)\) matrices used to build \(\Gamma_N\) and compute \(A^N\) |
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:
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.
\[u(t) = K_p \, e(t) + K_i \int_0^t e(\tau)\,d\tau + K_d \frac{de(t)}{dt}\]
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:
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Per sample | \(O(1)\) | \(O(1)\) | 3 multiplications, 3 additions, 1 clamp |
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.
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\)
| Step | Measured | \(e[n]\) | \(e[n-1]\) | \(e[n-2]\) | \(\Delta u\) | \(u[n]\) (clamped) |
|---|---|---|---|---|---|---|
| 0 | 0.0 | 5.0 | 0 | 0 | \(2.6 \times 5 = 13\) | 10.0 (clamped) |
| 1 | 2.0 | 3.0 | 5.0 | 0 | \(2.6(3) - 2.2(5) + 0.1(0) = -3.2\) | 6.8 |
| 2 | 4.0 | 1.0 | 3.0 | 5.0 | \(2.6(1) - 2.2(3) + 0.1(5) = -3.5\) | 3.3 |
| 3 | 4.8 | 0.2 | 1.0 | 3.0 | \(2.6(0.2) - 2.2(1) + 0.1(3) = -1.38\) | 1.92 |
The output converges toward the steady-state value needed to maintain the setpoint.
Reset() after
large setpoint changes to clear stale error history.| Variant | Key Difference |
|---|---|
| Standard (positional) PID | Computes \(u[n]\) from scratch each step using an explicit integral accumulator |
| PI controller | \(K_d = 0\); simpler, no derivative noise issues |
| PD controller | \(K_i = 0\); no steady-state error correction, used when offset is acceptable |
| PID with derivative filter | Low-passes the derivative term to reject high-frequency noise |
| Gain-scheduled PID | Tuning parameters vary as a function of operating point |
| Cascade PID | Inner and outer loops, each with its own PID — common in motor control |
graph LR
PID["PID Controller"]
LQR["LQR Controller"]
KF["Kalman Filter"]
PID -.->|"alternative"| LQR
KF -->|"state estimate"| LQR
KF -->|"filtered measurement"| PID| Algorithm | Relationship |
|---|---|
| LQR Controller | Optimal alternative when a state-space model is available; PID is model-free. |
| Kalman Filter | Can provide filtered state estimates as input to either PID or LQR |
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.
\[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.
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.
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]\).
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\)).
\[Q_{ii} = \frac{1}{(\text{max acceptable } x_i)^2}, \qquad R_{jj} = \frac{1}{(\text{max acceptable } u_j)^2}\]
| Phase | Time | Space | Notes |
|---|---|---|---|
| DARE solve (offline) | \(O(I \cdot n^3)\) | \(O(n^2)\) | \(I\) iterations, each involving \(n \times n\) matrix operations |
| Control step (online) | \(O(n \cdot m)\) | \(O(n \cdot m)\) | Single matrix-vector multiply \(u = -Kx\) |
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.
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):
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.
MaxIterations template parameter to raise the cap:
Lqr<float, StateSize, InputSize, 30000>. A
hard-assert fires if the solver does not converge within the specified
limit.| Variant | Key Difference |
|---|---|
| Continuous-time LQR | Solves the Continuous ARE instead of DARE; used for continuous-time plant models |
| LQG (Linear Quadratic Gaussian) | Combines LQR with Kalman filter for systems with noisy, partial observations |
| LQR with integral action | Augments the state with integral of tracking error to eliminate steady-state offset |
| Finite-horizon LQR | Time-varying gain \(K[k]\) for finite-duration tasks |
| Robust LQR (H∞) | Accounts for model uncertainty by optimizing the worst-case cost |
| Pre-computed gain | \(K\) is computed offline (e.g., in
MATLAB with dlqr) and hard-coded for embedded targets |
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| Algorithm | Relationship |
|---|---|
| DARE Solver | Computes the cost-to-go matrix \(P\) from which \(K\) is derived |
| Gaussian Elimination | Used inside DARE iteration to solve linear sub-systems |
| Kalman Filter | Provides state estimates when direct measurement is unavailable (forming LQG) |
| PID Controller | Model-free alternative; simpler to deploy but not optimal |
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.
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\).
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}\]
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.
\[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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Design | \(O((n+p)^3)\) | \(O((n+p)^2)\) | DARE iteration on augmented system |
| Update | \(O(m(n+p))\) | \(O(p)\) | Two matrix-vector products |
Design is a one-time offline computation. The per-sample update cost is dominated by the two gain-state products and the integral accumulation.
Consider a scalar plant (\(n=1\), \(m=1\), \(p=1\), \(T_s = 0.01\)):
MaxIterations template parameter:
IntegralStateFeedbackLqi<float, StateSize, InputSize, OutputSize, 30000>.Lqr on the augmented plant.Lqr.Lqr with a Kalman filter;
LQI could similarly pair with Lqg for output-feedback servo
control.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.
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
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}\).
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\).
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.
At each time step \(k\):
| Step | Time | Space | Notes |
|---|---|---|---|
| Construction | \(O(n^3)\) | \(O(n^2)\) | DARE solve for LQR gain |
| ComputeControl | \(O(n^2 + nm)\) | \(O(1)\) | KF update + LQR multiply |
where \(n\) = state size, \(m\) = input size, \(p\) = measurement size.
float unless the target platform
lacks an FPU.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):
Online loop (per sample):
| Step | Operation | Notes |
|---|---|---|
| 1 | Receive sensor measurement \(y_k\) | Noisy position reading |
| 2 | \(\hat{x}_{k\|k} = \hat{x}_{k\|k-1} + K_f(y_k - C\hat{x}_{k\|k-1})\) | Kalman update |
| 3 | \(u_k = -K \hat{x}_{k\|k}\) | LQR control law |
| 4 | \(\hat{x}_{k+1\|k} = A \hat{x}_{k\|k} + B u_k\) | Kalman predict |
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.
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\))
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\)).
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}\]
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.
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.
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.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Offline | \(O(N_p^2 \cdot n^3)\) | \(O((N_c m)^2 + N_p n \cdot N_c m)\) | Prediction matrices, Hessian, gradient precomputed |
| Online | \(O((N_c m)^3)\) | \(O((N_c m)^2)\) | Gaussian elimination to solve \(H\mathbf{U}=-Fx\) |
| Per step | \(O(N_c m \cdot n)\) | \(O(N_c m)\) | Matrix-vector product \(Fx\) + constraint clamping |
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.
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.
| Variant | Key Difference | Use Case |
|---|---|---|
| Nonlinear MPC (NMPC) | Nonlinear prediction model, requires iterative optimization | Nonlinear plants, high-accuracy |
| Explicit MPC | Precomputes control law as piecewise affine function | Very fast online evaluation |
| Move-Blocking MPC | Constrains control moves to change only at specific steps | Reduces QP size |
| Adaptive MPC | Updates plant model online | Time-varying or uncertain systems |
| Distributed MPC | Decomposes problem across subsystems | Large-scale multi-agent systems |
| Robust MPC | Accounts for bounded disturbances in constraints | Safety-critical applications |
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| MPCA 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.
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)
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.
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.
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.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Step (state) | \(O(n^2 + nm)\) | \(O(n)\) | One matrix-vector multiply per term |
| Output | \(O(pn + pm)\) | \(O(p)\) | One matrix-vector multiply per term |
| Construction | \(O(1)\) | \(O(n^2 + nm + pn + pm)\) | Value-type struct, no allocation |
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).
WithFullStateOutput factory,
which sets \(C\) to the identity.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.
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}]\).
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.
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.
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(1)\) | \(O(1)\) | Wrapper adds one addition and one clamp |
| Average | \(O(1)\) | \(O(1)\) | Cost dominated by the injected components |
| Worst | \(O(1)\) | \(O(1)\) | State lives entirely inside injected objects |
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]\):
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\).
Reset(). Dynamic pre-filters (e.g. reference model
filters) carry internal state that must be cleared on
Reset(). Both the feedforward and feedback components are
reset when Feedforward2Dof::Reset() is called; any
feedforward implementation that holds state must override
Reset() to clear it.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.
\[x[k+1] = A x[k] + B u[k]\] \[y[k] = C x[k] + D u[k]\]
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.
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.
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.
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.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Design (Ackermann) | \(O(n^3)\) | \(O(n^2)\) | Observability matrix build + linear solve |
| Update (per step) | \(O(n^2 + np)\) | \(O(n^2)\) | Matrix-vector products; dominant cost is \(A\hat{x}\) |
The design phase is offline. The real-time cost per sample is dominated by the \(n \times n\) state-transition multiply.
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\}\). ✓
| Variant | Key Difference |
|---|---|
| Kalman Filter | Stochastic design — minimizes covariance rather than placing poles; handles noise statistics |
| Extended Luenberger Observer | Linearizes a nonlinear plant around the estimate for quasi-linear operation |
| Unknown-Input Observer | Estimates states in the presence of unmeasured disturbances |
| Reduced-Order Observer | Estimates only the unmeasured states, using measured outputs directly |
| Continuous-time observer | Uses \(\dot{\hat{x}} = A\hat{x} + Bu + L(y - C\hat{x})\); same structure, continuous pole placement |
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| Algorithm | Relationship |
|---|---|
| Linear Time-Invariant Model | Supplies the \((A, B, C, D)\) matrices used in both design and update steps |
| Gaussian Elimination | Solves \(\mathcal{O} x = e_n\) inside Ackermann’s formula |
| LQR Controller | Primary consumer of the observer’s state estimate |
| Kalman Filter | Stochastic counterpart; adds noise covariance propagation |
Friction is the dominant unmodelled effect in geared drives, lead screws and robot joints. It causes stick-slip, tracking lag and steady-state error that a feedback loop can only remove by raising its gain, which erodes stability margins. Friction compensation instead predicts the drag from the axis velocity and adds the opposite torque (or force) to the command, so the feedback loop only has to correct the residual.
The model superposes three effects: Coulomb drag (constant magnitude, opposing motion), viscous drag (proportional to speed) and Stribeck stiction (an extra breakaway level that fades as the axis starts to slide). The ideal curve is discontinuous at zero velocity, so a smooth boundary layer replaces the hard sign function.
For one axis with velocity \(v\), the compensation torque is:
\[\tau = \operatorname{clamp}\!\Big(\big[F_c + (F_s - F_c)\,e^{-(|v|/v_s)^{\delta}}\big]\tanh\!\big(v/\varepsilon\big) + F_v\, v,\; -\tau_{max},\; \tau_{max}\Big)\]
Axes are independent; a multi-axis compensator applies the model to each velocity component with its own parameters.
The bracketed term is the friction level \(L(|v|)\). It starts at \(F_s\) for \(v = 0\) and decays monotonically to \(F_c\), which is why \(F_s \ge F_c\) is required: otherwise the “Stribeck dip” would turn into a rise. The factor \(\tanh(v/\varepsilon)\) supplies the direction, approximating \(\operatorname{sign}(v)\) with slope \(L/\varepsilon\) at the origin. The viscous term is added outside the boundary layer.
The result is a feedforward term, not a feedback law. It is added to the controller output, \(\tau_{cmd} = \tau_{control} + \tau\), and composes with any torque or current loop. Because it depends only on velocity, it does not enter the feedback characteristic equation.
In both cases \(\varepsilon\) sets the sharpness-versus-chatter trade-off.
For fixed \(v_s\), \(\delta\) and \(\varepsilon\), \(\tau\) is linear in the force coefficients: \(F_c\), \(F_s\) and \(F_v\) multiply the known regressors \((1-e^{-(|v|/v_s)^{\delta}})\tanh(v/\varepsilon)\), \(e^{-(|v|/v_s)^{\delta}}\tanh(v/\varepsilon)\) and \(v\) respectively. They can therefore be estimated by linear least squares from logged velocity and torque data, jointly with other linear parameters. \(v_s\) and \(\delta\) require an outer search.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(N)\) | \(O(N)\) | \(N\) axes; per axis one exponential, one power, one hyperbolic tangent, two divisions and a clamp; \(N\) parameter sets stored, no allocation |
Cost is dominated by the three transcendental calls; there is no iteration and no dependence on history, so the latency is constant per sample.
Parameters: \(F_c = 0.5\), \(F_s = 0.8\), \(F_v = 0.1\), \(v_s = 0.05\), \(\delta = 2\), \(\varepsilon = 10^{-3}\), with a non-binding cap \(\tau_{max} = 1\).
| \(v\) | \((\lvert v\rvert/v_s)^2\) | Level \(L\) | \(\tanh(v/\varepsilon)\) | \(F_v v\) | \(\tau\) |
|---|---|---|---|---|---|
| 1 | 400 | \(0.5 + 0.3\,e^{-400} \approx 0.5\) | 1 | 0.1 | 0.6 |
| 0.01 | 0.04 | \(0.5 + 0.3\,e^{-0.04} \approx 0.788\) | \(\tanh 10 \approx 1\) | 0.001 | 0.789 |
| 0.05 (\(=v_s\)) | 1 | \(0.5 + 0.3/e \approx 0.610\) | \(\tanh 50 \approx 1\) | 0.005 | 0.615 |
At high speed the Stribeck term has vanished and only Coulomb plus viscous drag remain. At \(v = 0.01\) the axis is still deep in the Stribeck region, so the level is close to \(F_s\). At \(v = v_s\) the stiction excess has decayed to a fraction \(1/e\) of its initial value. At \(v = 0\) the output is exactly zero, and \(v = -0.01\) gives \(-0.789\) by symmetry.
| Variant | Key Difference |
|---|---|
| Coulomb + viscous only | \(F_s = F_c\) removes the Stribeck term; the level is constant |
| Asymmetric friction | Separate parameters per direction of motion; handles direction-dependent drag |
| Karnopp model | Replaces the smooth layer with a velocity dead-zone and a stick state |
| Dahl / LuGre | Dynamic models with internal bristle state; capture presliding and hysteresis at the cost of state and a stiffer ODE |
| Inverse-dynamics term | Same static model folded into a robot inverse-dynamics computation |
| Algorithm | Relationship |
|---|---|
| Feedforward / 2-DOF | Same additive-feedforward structure; friction is a velocity-driven feedforward term |
| PID Controller | The feedback loop this term is added to; compensation reduces integrator work and stick-slip hunting |
| Bang-Bang / Hysteresis | The \(\varepsilon \to 0\) limit is a relay on velocity, with the same chattering mechanism |
| Saturation / Rate Limiter | The output clamp is the same saturation, applied to the compensation term alone |
| QR Decomposition | Backbone of the least-squares fit of the linear friction coefficients |
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.
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).
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.
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.
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Schedule (linear search) | \(O(N + p)\) | \(O(1)\) | \(N\) interval scan, \(p\) blends |
| Schedule (binary search) | \(O(\log N + p)\) | \(O(1)\) | Preferred for large tables |
| Construction | \(O(N)\) | \(O(N \cdot p)\) | Monotonicity assertion |
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.
Table: \(\{(0, 1), (0.5, 2), (1, 4)\}\) — three breakpoints, one gain each.
Query \(\sigma = 0.75\):
Query \(\sigma = -1.0\) (below table): saturate to \(g = g_1 = 1.0\).
| Variant | Key Difference |
|---|---|
| Bilinear / 2-D scheduling | Gains indexed by two variables (e.g., speed and load); uses bilinear interpolation on a grid |
| LPV (Linear Parameter-Varying) | Gains are polynomial functions of \(\sigma\); stability guaranteed by parameter-dependent Lyapunov functions |
| Velocity-form scheduling | Gains on incremental (velocity-form) controllers avoid output steps at scheduling transitions |
| Bumpless transfer | State initialisation at switchover prevents transients when \(\sigma\) jumps discontinuously |
| Algorithm | Relationship |
|---|---|
| LQR | Common source of the per-breakpoint gain sets; each \(\mathbf{g}_i\) is an LQR solution at the \(i\)-th linearised operating point |
| Feedforward/2-DOF | Complementary structure: the feedforward path can also be gain-scheduled to account for reference-dependent plant changes |
| MPC | Alternative that solves an optimisation online; eliminates the need for offline scheduling but requires more computation |
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.
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\).
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}\]
\[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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(1)\) | \(O(1)\) | Three multiply-adds, two state updates |
| Average | \(O(1)\) | \(O(1)\) | Fixed instruction count per sample |
| Worst | \(O(1)\) | \(O(1)\) | No branching; deterministic real-time behavior |
Design (Tustin coefficient computation) is \(O(1)\) and occurs once in the constructor.
Parameters: \(K=1\), \(z=1\,\text{rad/s}\), \(p=10\,\text{rad/s}\), \(T_s = 0.01\,\text{s}\) (lead network).
Unit-step response (first two samples):
| \(n\) | \(x[n]\) | \(b_0 x[n]\) | \(b_1 x[n-1]\) | \(-a_1 y[n-1]\) | \(y[n]\) |
|---|---|---|---|---|---|
| 0 | 1 | 0.9571 | 0 | 0 | 0.9571 |
| 1 | 1 | 0.9571 | −0.9476 | 0.8664 | 0.8759 |
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\).
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.
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]\).
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.
\[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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(1)\) | \(O(1)\) | Two compare-and-select operations |
| Average | \(O(1)\) | \(O(1)\) | One multiply + two compares for rate limit |
| Worst | \(O(1)\) | \(O(1)\) | Fixed instruction count; no branching on size |
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.
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\).
| Tick \(n\) | \(u[n]\) | \(\delta[n]\) | \(y_{\text{slew}}[n]\) | \(y_{\text{composed}}[n]\) |
|---|---|---|---|---|
| 0 | 1.0 | 0.1 | 0.1 | 0.1 |
| 1 | 1.0 | 0.1 | 0.2 | 0.2 |
| 2 | 1.0 | 0.1 | 0.3 | 0.3 |
| 3 | 1.0 | 0.1 | 0.4 | 0.4 |
| 4 | 1.0 | 0.1 | 0.5 | 0.5 |
| 5 | 1.0 | 0.1 | 0.6 | 0.5 (clamped) |
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.
Reset with the desired initial state before the first
control tick.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.
\[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\).
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.
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}\).
| Phase | Time | Space | Notes |
|---|---|---|---|
| \(\mathbf{X}^T\mathbf{X}\) | \(O(n p^2)\) | \(O(p^2)\) | Dominated by matrix multiply |
| \(\mathbf{X}^T\mathbf{y}\) | \(O(np)\) | \(O(p)\) | Matrix-vector multiply |
| Solve | \(O(p^3)\) | \(O(p^2)\) | Gaussian elimination |
| Predict | \(O(p)\) | \(O(1)\) | Dot product |
For embedded use with small \(p\) (say \(\leq 10\)), the entire fit completes in microseconds.
Data: 3 samples, 1 feature.
| \(x\) | \(y\) |
|---|---|
| 1 | 2 |
| 2 | 4 |
| 3 | 5 |
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\)
| Variant | Key Difference |
|---|---|
| Ridge regression (L2) | Adds \(\lambda\|\boldsymbol{\beta}\|^2\) to the cost; solves \((\mathbf{X}^T\mathbf{X} + \lambda I)\boldsymbol{\beta} = \mathbf{X}^T\mathbf{y}\) |
| Lasso regression (L1) | Adds \(\lambda\|\boldsymbol{\beta}\|_1\); promotes sparsity but requires iterative optimization |
| Polynomial regression | Adds powers of \(x\) as new features; still linear in parameters |
| Weighted least squares | Weights each sample differently; solves \((\mathbf{X}^T W \mathbf{X})\boldsymbol{\beta} = \mathbf{X}^T W \mathbf{y}\) |
| Recursive least squares | Updates \(\boldsymbol{\beta}\) incrementally as new data arrives; suited for online estimation |
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| Algorithm | Relationship |
|---|---|
| Gaussian Elimination | Used to solve the normal equation system |
| Yule-Walker | Structurally similar — also solves a linear system derived from correlations |
| Neural Network (neural-network-toobox-cpp) | A single-layer neural network with no activation and MSE loss reduces to linear regression |
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.
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\]
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\).
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.
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.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Build \(\mathbf{V}\) | \(O(n\,d)\) | \(O(n\,d)\) | Incremental powers, no pow() |
| Form \(\mathbf{V}^\top\mathbf{V}\) | \(O(n\,d^2)\) | \(O(d^2)\) | Symmetric, only upper half needed |
| Form \(\mathbf{V}^\top\mathbf{y}\) | \(O(n\,d)\) | \(O(d)\) | Matrix-vector product |
| Solve \((d+1)\times(d+1)\) system | \(O(d^3)\) | \(O(d^2)\) | Gaussian elimination |
| Predict (Horner) | \(O(d)\) | \(O(1)\) | One MAC per coefficient |
All dimensions are compile-time constants; no heap allocation is required.
Data: \(n = 4\) samples, \(d = 2\) (quadratic fit).
| \(x_i\) | \(y_i\) |
|---|---|
| 0 | 1 |
| 1 | 0.75 |
| 2 | 1 |
| 3 | 1.75 |
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\]
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.
| Variant | Key Difference |
|---|---|
| Orthogonal polynomial basis | Uses Legendre/Chebyshev basis instead of monomials; much better conditioning |
| Weighted least squares | Each sample weighted differently (e.g., by measurement precision) |
| Regularized (Ridge) fitting | Adds \(\lambda\|\mathbf{c}\|^2\) to damp large coefficients |
| Constrained fitting | Enforces derivative constraints at endpoints |
| Savitzky-Golay smoothing | Sliding-window polynomial fit for real-time derivative estimation |
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| Algorithm | Relationship |
|---|---|
| Gaussian Elimination | Solves the normal equations |
| Linear Regression | Polynomial fitting is linear regression with polynomial features |
| Recursive Least Squares | Online / streaming counterpart for time-varying models |
Ordinary least squares (OLS) assumes the regressors are known exactly and only the response is noisy — it minimizes the vertical residuals. Real embedded systems violate that assumption: calibrating one measured quantity against another (two drifting sensors, current vs. torque, strain vs. force) is an errors-in-variables problem where every channel is noisy. Fitting such data with OLS produces a systematically biased (attenuated) slope, a phenomenon known as regression dilution.
Total Least Squares (TLS) removes that bias. Geometrically it minimizes the orthogonal (perpendicular) distance from each point to the fitted hyperplane rather than the vertical distance, which is the maximum-likelihood estimate when the noise is equal on all channels.
Given a design matrix \(A \in \mathbb{R}^{m \times n}\) and a response \(b \in \mathbb{R}^{m}\), TLS seeks the smallest perturbation \([\,\Delta A \mid \Delta b\,]\) (in Frobenius norm) that makes the system consistent:
\[\min_{\Delta A,\ \Delta b} \bigl\|[\,\Delta A \mid \Delta b\,]\bigr\|_F \quad\text{s.t.}\quad (A + \Delta A)\,x = b + \Delta b\]
Stack the data into the augmented matrix \(M = [\,A \mid b\,] \in \mathbb{R}^{m \times (n+1)}\) and take its singular value decomposition \(M = U \Sigma V^{\top}\). The right-singular vector \(v_{n+1}\) associated with the smallest singular value \(\sigma_{n+1}\) spans the direction of least variance — the normal of the best-fit hyperplane. Partitioning
\[v_{n+1} = \begin{bmatrix} v_{1:n} \\ v_{n+1,\,n+1} \end{bmatrix}, \qquad x = -\,\frac{v_{1:n}}{v_{n+1,\,n+1}}\]
recovers the coefficients. The construction is exactly the Golub–Van Loan (1980) result: the minimal perturbation is \(\sigma_{n+1}\, u_{n+1} v_{n+1}^{\top}\), and \(\sigma_{n+1}\) is the orthogonal residual norm.
The solution exists (is generic) only when the last entry
\(v_{n+1,\,n+1} \neq 0\). If it
vanishes, the smallest singular direction lies entirely in the column
space of \(A\) (e.g. a rank-deficient
or all-zero regressor column) and no finite coefficient vector satisfies
the fit — Fit returns false.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(m\,n^2)\) | \(O(m\,n)\) | Golub–Kahan bidiagonalization dominates |
| Average | \(O(m\,n^2)\) | \(O(m\,n)\) | Implicit-QR sweeps converge in \(O(n)\) per value |
| Worst | \(O(m\,n^2)\) | \(O(m\,n)\) | Bounded iteration cap in the SVD engine |
All storage is stack-allocated (std::array-backed
math::Matrix); no heap, no recursion.
Fit \(b = 2a\) from four clean points \(a = (1,2,3,4)\), \(b = (2,4,6,8)\).
A and
b to comparable magnitudes before fitting, otherwise the
TLS/OLS distinction is lost.Fit reports false rather than
dividing by zero.Samples >= Features + 1 (enforced by
static_assert).SingularValueDecomposition
— the numerical engine; TLS is the SVD of [A | b] read from
the smallest-singular-value end.[A | b]ᵀ[A | b] reduces to its
smallest-eigenvalue eigenvector.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.
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\).
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}\]
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.
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.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Autocovariance | \(O(Np)\) | \(O(p)\) | One pass per lag, \(p+1\) lags |
| Build Toeplitz matrix | \(O(p^2)\) | \(O(p^2)\) | Fill from autocovariance vector |
| Solve (Levinson-Durbin) | \(O(p^2)\) | \(O(p)\) | Exploits Toeplitz structure |
| Solve (Gaussian elim.) | \(O(p^3)\) | \(O(p^2)\) | General-purpose fallback |
| Predict | \(O(p)\) | \(O(1)\) | Dot product with past samples |
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\)):
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})\)
| Variant | Key Difference |
|---|---|
| Burg’s method | Estimates AR coefficients from forward + backward prediction errors; often more accurate for short records |
| Covariance method | Uses the unbiased autocovariance; does not guarantee stability |
| ARMA models | Extends AR to include moving-average terms; more flexible but requires iterative estimation |
| Vector autoregression (VAR) | Multivariate extension for jointly modeling multiple time series |
| Recursive estimation | Updates AR coefficients online as new data arrives |
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| Algorithm | Relationship |
|---|---|
| Levinson-Durbin | Efficient \(O(p^2)\) solver exploiting the Toeplitz structure of the Yule-Walker matrix |
| Gaussian Elimination | General-purpose \(O(p^3)\) solver used as a fallback |
| Power Spectral Density | Non-parametric alternative; the AR model provides a parametric PSD estimate |
| Linear Regression | Structurally similar — both solve a linear system derived from data correlations |
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.
\[ 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}\).
EM alternates between:
Run KalmanSmoother::Smooth() to obtain, for each \(t\):
| Quantity | Notation | Description |
|---|---|---|
| \(\hat{x}_{t\|T}\) | smoothedMeans[t] |
Smoothed state mean |
| \(P_{t\|T}\) | smoothedCovariances[t] |
Smoothed state covariance |
| \(P_{t,t-1\|T}\) | lagCrossCovariances[t] |
Lag-1 cross-covariance (index 0 undefined) |
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 \]
\[ 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.
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\).
| Case | Time per iteration | Space (stack) | Notes |
|---|---|---|---|
| All | \(O(T \cdot N^3)\) | \(O(T \cdot N^2)\) | Dominated by RTS smoother forward/backward |
All storage is stack-allocated inside KalmanSmoother
(owned by ExpectationMaximization) and in local M-step
temporaries.
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:
Repeat until \(|\ell^{(k)} - \ell^{(k-1)}| < \delta\) or maximum iterations reached.
SolveSystem may produce inaccurate results. Prefer \(T \gg N\).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.ExpectationMaximization owns a KalmanSmoother
instance. The smoother state is overwritten on every call to
Run(). Do not share an ExpectationMaximization
instance across threads.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.Identification and calibration runs produce long logs — motor current against acceleration, joint torque against motion — that a microcontroller cannot store. Accumulating the normal equations fits in memory but squares the condition number, which single precision cannot afford. Real regressors are also often rank-deficient: a parameter never shows up in the data, or only in a fixed combination with another, and a plain solver then returns garbage.
Incremental QR least squares rotates each arriving row into a small triangular summary and forgets it: memory stays fixed however long the log runs and the data matrix is never squared. The final solve decomposes the summary into singular directions, inverts the strong ones, drops the weak ones, and reports which parameter combinations the data could actually determine.
Rows \((\mathbf{y}_k^T, \tau_k)\) with \(\mathbf{y}_k \in \mathbb{R}^P\) arrive one at a time, \(m\) unbounded. The task is \(\min_{\boldsymbol{\pi}} \|Y\boldsymbol{\pi} - \boldsymbol{\tau}\|_2\) over the stacked rows, taking the minimum-norm minimizer. Write \(Y = Q \begin{bmatrix} R \\ 0 \end{bmatrix}\) with \(Q\) orthogonal and \(R\) upper triangular \(P \times P\), and split \(Q^T\boldsymbol{\tau} = (\mathbf{d}, \boldsymbol{\tau}_\perp)\). Orthogonal maps preserve norms, so
\[\|Y\boldsymbol{\pi} - \boldsymbol{\tau}\|^2 = \|R\boldsymbol{\pi} - \mathbf{d}\|^2 + \rho, \qquad \rho = \|\boldsymbol{\tau}_\perp\|^2\]
The problem lives in \(R\), \(\mathbf{d}\) and \(\rho\), the part of the right-hand side no \(\boldsymbol{\pi}\) can reach: \(P^2 + P + 2\) scalars with the row count. \(Q\) is never formed.
Row update. Place the new row beneath \([\,R \mid \mathbf{d}\,]\). For \(j = 1, \dots, P\), skip the step if \(y_j = 0\); otherwise rotate row \(j\) of \(R\) and the incoming row in their common plane:
\[\begin{bmatrix} c & s \\ -s & c \end{bmatrix} \begin{bmatrix} R_{j,j:P} & d_j \\ y_{j:P} & \tau \end{bmatrix}, \qquad c = \frac{R_{jj}}{\ell},\; s = \frac{y_j}{\ell},\; \ell = \sqrt{R_{jj}^2 + y_j^2}\]
This zeroes \(y_j\), leaves \(R_{jj} = \ell \ge 0\) and touches only columns \(j\) to \(P\). After the sweep the incoming row is zero in every column, so its leftover right-hand side \(\tilde{\tau}\) reads \(0 \cdot \boldsymbol{\pi} = \tilde{\tau}\) and adds \(\tilde{\tau}^2\) to \(\rho\) whatever \(\boldsymbol{\pi}\) is. A zero pivot degenerates to a swap and a zero pair to the identity.
Why not normal equations. \(Y^TY = R^TR\), yet the singular values of \(R\) are those of \(Y\): the factor works at \(\kappa(Y)\), the normal equations at \(\kappa(Y)^2\). With single-precision \(\epsilon \approx 1.2 \times 10^{-7}\) they fail near \(\kappa(Y) \approx \epsilon^{-1/2} \approx 3 \times 10^3\); the factor lasts to about \(\epsilon^{-1}\).
Take \(R = U\Sigma V^T\) with \(\sigma_0 \ge \dots \ge \sigma_{P-1} \ge 0\). In the coordinates \(\mathbf{z} = V^T\boldsymbol{\pi}\),
\[\|R\boldsymbol{\pi} - \mathbf{d}\|^2 = \sum_{i=0}^{P-1} \left(\sigma_i z_i - \mathbf{u}_i^T\mathbf{d}\right)^2\]
The rank is \(r = \#\{\, i : \sigma_i > \text{tol} \cdot \sigma_0 \,\}\). For \(i < r\) the term is zeroed by \(z_i = \mathbf{u}_i^T\mathbf{d} / \sigma_i\); for \(i \ge r\) it cannot be removed and \(z_i\) is free, so the minimum-norm choice is \(z_i = 0\). The discarded terms are charged to the residual:
\[\hat{\boldsymbol{\pi}} = \sum_{i < r} \mathbf{v}_i \, \frac{\mathbf{u}_i^T\mathbf{d}}{\sigma_i}, \qquad \text{RMS} = \sqrt{\frac{\rho + \sum_{i \ge r} (\mathbf{u}_i^T\mathbf{d})^2}{m}}\]
The first \(r\) rows of \(V^T\) are the combinations \(\mathbf{v}_i^T\boldsymbol{\pi}\) the data determine; the rest are the unidentifiable directions. Moving \(\boldsymbol{\pi}\) along them leaves the fit unchanged, and \(\hat{\boldsymbol{\pi}}\) is orthogonal to them.
Scaling a row and its right-hand side by \(\sqrt{w_k}\) gives weighted least squares. Every step is orthogonal, so row order changes the result only by rounding. With no rows there is no result; a reset clears the state.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Row, sparse (best) | \(O(P)\) | \(P^2 + P + 2\) | Zero entries skipped; one short rotation per nonzero |
| Row, dense (worst) | \(O(P^2)\) | \(P^2 + P + 2\) | \(P\) rotations on \(P - j + 1\) pairs, \(P\) square roots |
| Solve | \(O(P^3)\) | \(O(P^2)\) extra | One \(P \times P\) SVD, independent of \(m\) |
| Whole stream, \(m\) rows | \(O(mP^2 + P^3)\) | \(P^2 + P + 2\) | Memory does not grow with \(m\) |
At \(P = 60\) the state is about 14 KB in single precision. Accumulation can run at the sample rate; the solve is an offline or commissioning step. No heap, no recursion.
Full rank. Rows \((1, x)\) at \(x = (0, 1, 2, 3)\), \(\boldsymbol{\tau} = (2.1, 4.9, 8.1, 10.9)\):
| Rows | \(R_{00}\) | \(R_{01}\) | \(R_{11}\) | \(d_0\) | \(d_1\) | \(\rho\) |
|---|---|---|---|---|---|---|
| 1 | 1 | 0 | 0 | 2.1 | 0 | 0 |
| 2 | 1.4142 | 0.7071 | 0.7071 | 4.9497 | 1.9799 | 0 |
| 3 | 1.7321 | 1.7321 | 1.4142 | 8.7180 | 4.2426 | 0.0267 |
| 4 | 2 | 3 | 2.2361 | 13 | 6.6188 | 0.0320 |
Row 1 meets a zero pivot and is adopted; row 2 rotates by \(45^\circ\) and fits two points exactly; from row 3 on, \(\rho\) grows. With \(\sigma \approx (4.1000, 1.0908)\), \(r = 2\) and back-substitution gives \(\hat{\pi}_1 = d_1 / R_{11} = 2.96\), \(\hat{\pi}_0 = (13 - 3 \cdot 2.96)/2 = 2.06\), with RMS \(= \sqrt{0.032 / 4} = 0.089443\).
Rank-deficient. Rows \((1, x, x)\) with \(\boldsymbol{\tau} = 2 + 3x\): two equal columns leave the third row of \(R\) zero, \(\rho = 0\), and \(\sigma = (5.540482,\, 1.141517,\, 0)\), so \(r = 2\). Only \(\pi_1 + \pi_2 = 3\) is determined; the minimum-norm split is \(\hat{\boldsymbol{\pi}} = (2, 1.5, 1.5)\). The last row of \(V^T\) is the unidentifiable direction \((0, 1, -1)/\sqrt2\).
Verified against double-precision normal equations on random 3-parameter problems of 5 to 205 rows: parameters agree to about \(3 \times 10^{-6}\).
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.
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.
Given a new observation \((\mathbf{x}, y)\):
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)
| Case | Time | Space | Notes |
|---|---|---|---|
| Average | \(O(p^2)\) | \(O(p^2)\) | Matrix-vector products + outer product |
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.
Estimating the slope of \(y = 2x\) with RLS (\(p = 2\): bias + slope, \(\lambda = 1\)):
| Step | \(\mathbf{x}\) | \(y\) | \(\hat{\theta}_0\) (bias) | \(\hat{\theta}_1\) (slope) | Innovation |
|---|---|---|---|---|---|
| Init | — | — | 0.0 | 0.0 | — |
| 1 | [1, 1] | 2 | 1.0 | 1.0 | 2.0 |
| 2 | [1, 2] | 4 | 0.0 | 2.0 | 1.0 |
| 3 | [1, 3] | 6 | 0.0 | 2.0 | 0.0 |
Convergence is achieved in 2 steps for this noiseless system.
Recursive least squares assumes the regressor is known exactly and only the output is noisy. When the regressor is itself measured — a derivative taken from two noisy samples, a current read through a noisy sensor — least squares reads the noise as a weaker dependence on that column and biases its coefficient toward zero. The effect is regression dilution, and it does not shrink with more data: the estimate converges, but to the wrong value.
The instrumental-variable method removes that bias without modelling the noise. It needs a second signal per observation, the instrument, that moves with the true regressor but not with its noise. A known excitation is the usual choice: the reference a controller injects, a command, or a low-noise measurement of the same quantity. The recursive form updates the estimate sample by sample at the cost of recursive least squares.
The model is \(y = \mathbf{x}^T \boldsymbol{\theta} + \epsilon\), but only \(\tilde{\mathbf{x}} = \mathbf{x} + \mathbf{v}\) is observed, with \(\mathbf{v}\) zero-mean noise. Least squares solves \(\sum \tilde{\mathbf{x}}\,(y - \tilde{\mathbf{x}}^T \boldsymbol{\theta}) = 0\). Because \(E[\tilde{\mathbf{x}} \tilde{\mathbf{x}}^T] = R_x + R_v\), its limit is
\[\hat{\boldsymbol{\theta}}_{LS} \to (R_x + R_v)^{-1} R_x\, \boldsymbol{\theta}\]
For a single slope this is the attenuation \(\sigma_x^2 / (\sigma_x^2 + \sigma_v^2)\).
Given an instrument \(\mathbf{z}\) correlated with \(\mathbf{x}\) and uncorrelated with \(\mathbf{v}\) and \(\epsilon\), the estimate solves
\[\sum_{k} \lambda^{n-k}\, \mathbf{z}_k \left(y_k - \tilde{\mathbf{x}}_k^T \boldsymbol{\theta}\right) = 0 \quad\Rightarrow\quad \hat{\boldsymbol{\theta}} = \Big(\sum_k \lambda^{n-k} \mathbf{z}_k \tilde{\mathbf{x}}_k^T\Big)^{-1} \sum_k \lambda^{n-k} \mathbf{z}_k y_k\]
The noise enters only through \(E[\mathbf{z}\,\mathbf{v}^T] = 0\), so the estimate is consistent.
With \(P\) the inverse of the weighted cross-moment \(\sum \lambda^{n-k} \mathbf{z}_k \tilde{\mathbf{x}}_k^T\), a new observation \((\tilde{\mathbf{x}}, \mathbf{z}, y)\) gives:
Steps 2 and 5 are the Sherman–Morrison update of a rank-one, non-symmetric term \(\mathbf{z}\tilde{\mathbf{x}}^T\). With \(\mathbf{z} = \tilde{\mathbf{x}}\) every step reduces to recursive least squares.
The innovation, the post-update residual and the trace of \(P\) are reported as for recursive least squares. \(P\) is no longer a scaled covariance and need not be symmetric; its trace still falls as the instrument excites the parameters.
| Case | Time | Space | Notes |
|---|---|---|---|
| Average | \(O(p^2)\) | \(O(p^2)\) | Two matrix-vector products and one outer product |
The cost per update matches recursive least squares: one more matrix-vector product, for \(\tilde{\mathbf{x}}^T P\), replaces the symmetric outer product.
A single slope, \(y = 2x\), observed through \(\tilde{x} = x + v\) with the instrument \(z = x\). The noise alternates so that it is uncorrelated with \(x\) over the four samples. \(\lambda = 1\), \(P_0 = 1000\), \(\hat{\theta}_0 = 0\):
| Step | \(x\) | \(v\) | \(\tilde{x}\) | \(y\) | Least squares \(\hat{\theta}\) | Instrumental \(\hat{\theta}\) |
|---|---|---|---|---|---|---|
| 1 | 1 | 0.5 | 1.5 | 2 | 1.333 | 1.332 |
| 2 | 1 | -0.5 | 0.5 | 2 | 1.599 | 1.999 |
| 3 | 2 | 0.5 | 2.5 | 4 | 1.600 | 1.714 |
| 4 | 2 | -0.5 | 1.5 | 4 | 1.818 | 2.000 |
After the four samples least squares holds \(2\,\sum x^2 / (\sum x^2 + \sum v^2) = 20/11\), the attenuated slope, while the instrumental estimate is back at 2: each time the noise has averaged out against the instrument, it is exact.
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.
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.\]
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.\]
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\)).
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\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Any | \(O(N)\) | \(O(N)\) | Two dot products + one AXPY, all length \(N\) |
No covariance matrix is stored; memory is exactly two vectors of length \(N\) (weights and delay line).
Consider a 2-tap system with true plant \(\mathbf{w}^* = [0.5,\ 0.5]^\top\), \(\mu = 0.1\), and a white input sequence.
| Step | Input | Delay line \(\mathbf{x}\) | Output \(y\) | Desired \(d\) | Error \(e\) | Weight update direction |
|---|---|---|---|---|---|---|
| 1 | 1.0 | [1.0, 0.0] | 0.0 | 0.5 | 0.5 | \(+0.05\mathbf{x}\) |
| 2 | 0.8 | [0.8, 1.0] | 0.04 | 0.9 | 0.86 | \(+0.086\mathbf{x}\) |
| … | … | … | … | … | … | converges to \(\mathbf{w}^*\) |
After many iterations the error approaches zero and the weights stabilize near \([0.5,\ 0.5]^\top\).
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:
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.
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\))
\[\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}\]
\[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)}\]
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).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Predict | \(O(n^2)\) | \(O(n^2)\) | Matrix multiply \(F P F^T\) |
| Update | \(O(n^2 m + m^3)\) | \(O(nm)\) | Dominated by \(S^{-1}\) (\(m \times m\) inversion) and gain computation |
| Total per step | \(O(n^2 m + m^3)\) | \(O(n^2 + nm)\) | For \(m \ll n\), approximately \(O(n^2)\) |
For typical embedded use with \(n \leq 10\) and \(m \leq 5\), each step completes in microseconds.
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.
| Variant | Key Difference |
|---|---|
| Extended Kalman Filter (EKF) | Linearizes nonlinear \(f(x)\) and \(h(x)\) around the current estimate |
| Unscented Kalman Filter (UKF) | Propagates sigma points through nonlinearities; avoids explicit Jacobians |
| Square-root Kalman Filter | Maintains \(\sqrt{P}\) instead of \(P\) for improved numerical stability |
| Information Filter | Works with the inverse covariance (information matrix); better for multi-sensor fusion |
| Kalman Smoother | Uses future measurements to refine past estimates (offline, non-causal) |
| Adaptive Kalman Filter | Estimates \(Q\) and \(R\) online from innovation statistics |
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| Algorithm | Relationship |
|---|---|
| LQR Controller | The Kalman filter + LQR = LQG (Linear Quadratic Gaussian), the separation principle |
| DARE Solver | The steady-state Kalman gain can be found by solving a DARE (dual of the LQR DARE) |
| PID Controller | Kalman-filtered measurements can serve as the PID’s process variable input |
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.
\[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.
\[F_k = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1}}\]
\[\hat{x}_k^- = f(\hat{x}_{k-1}, u_{k-1})\]
\[P_k^- = F_k P_{k-1} F_k^T + Q_{k-1}\]
\[H_k = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_k^-}\]
\[y_k = z_k - h(\hat{x}_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^-\]
| Operation | Time | Space | Notes |
|---|---|---|---|
| Jacobian eval | \(O(n^2)\) (user-defined) | \(O(n^2)\) | Depends on the specific nonlinear functions |
| Predict | \(O(n^2)\) | \(O(n^2)\) | Matrix multiply \(F P F^T\) plus nonlinear state propagation |
| Update | \(O(n^2 m + m^3)\) | \(O(nm)\) | Same as standard Kalman — the linearized equations are identical |
| Total per step | \(O(n^2 m + m^3)\) | \(O(n^2 + nm)\) | Dominated by Jacobian evaluation for complex models |
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.
ControlSize template parameter.| Filter | Jacobian Required? | Accuracy for Nonlinear Systems | Computational Cost |
|---|---|---|---|
| Kalman Filter | No (linear only) | Exact for linear | Lowest |
| EKF | Yes | First-order approximation | Low |
| UKF | No | Second-order approximation | Moderate |
| Particle | No | Arbitrary (Monte Carlo) | High |
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| Algorithm | Relationship |
|---|---|
| Kalman Filter | The EKF reduces to the standard KF when \(f\) and \(h\) are linear |
| Unscented Kalman Filter | Avoids Jacobians using sigma points; better for highly nonlinear cases |
| Euler-Lagrange | Provides the nonlinear dynamics model \(f(x)\) for mechanical systems |
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 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.
Given state estimate \(\hat{x} \in \mathbb{R}^n\) and covariance \(P\), generate \(2n+1\) sigma points:
\[\sigma_0 = \hat{x}\] \[\sigma_i = \hat{x} + \left(\sqrt{(n + \lambda) P}\right)_i, \quad i = 1, \ldots, n\] \[\sigma_{i+n} = \hat{x} - \left(\sqrt{(n + \lambda) P}\right)_i, \quad i = 1, \ldots, n\]
where \(\left(\sqrt{M}\right)_i\) denotes the \(i\)-th column of the matrix square root (Cholesky decomposition \(M = LL^T\), using columns of \(L\)), and:
\[\lambda = \alpha^2 (n + \kappa) - n\]
\[w_0^{(m)} = \frac{\lambda}{n + \lambda}, \quad w_0^{(c)} = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\] \[w_i^{(m)} = w_i^{(c)} = \frac{1}{2(n + \lambda)}, \quad i = 1, \ldots, 2n\]
Parameters: - \(\alpha\): Controls spread of sigma points (\(10^{-4} \leq \alpha \leq 1\)). Smaller values keep points closer to the mean. - \(\beta\): Incorporates prior knowledge about the distribution (\(\beta = 2\) is optimal for Gaussian). - \(\kappa\): Secondary scaling parameter (typically \(\kappa = 0\) or \(\kappa = 3 - n\)).
\[\sigma_i^* = f(\sigma_i, u)\]
\[\hat{x}_k^- = \sum_{i=0}^{2n} w_i^{(m)} \sigma_i^*\] \[P_k^- = \sum_{i=0}^{2n} w_i^{(c)} (\sigma_i^* - \hat{x}_k^-)(\sigma_i^* - \hat{x}_k^-)^T + Q\]
Generate new sigma points from the predicted state \(\hat{x}_k^-\) and \(P_k^-\).
Transform sigma points through the measurement function:
\[z_i = h(\sigma_i)\]
\[\hat{z}_k = \sum_{i=0}^{2n} w_i^{(m)} z_i\] \[S_k = \sum_{i=0}^{2n} w_i^{(c)} (z_i - \hat{z}_k)(z_i - \hat{z}_k)^T + R\] \[P_{xz} = \sum_{i=0}^{2n} w_i^{(c)} (\sigma_i - \hat{x}_k^-)(z_i - \hat{z}_k)^T\]
\[K_k = P_{xz} S_k^{-1}\] \[\hat{x}_k = \hat{x}_k^- + K_k (z_k - \hat{z}_k)\] \[P_k = P_k^- - K_k S_k K_k^T\]
| Operation | Time | Space | Notes |
|---|---|---|---|
| Sigma point gen | \(O(n^3)\) | \(O(n^2)\) | Dominated by Cholesky decomposition |
| State propagation | \(O((2n+1) \cdot c_f)\) | \(O(n^2)\) | \(c_f\) = cost of evaluating \(f(x)\) |
| Mean/cov reconstruct | \(O(n^2 \cdot (2n+1))\) | \(O(n^2)\) | Weighted outer products |
| Update | \(O(n^2 m + m^3)\) | \(O(nm)\) | Same as standard Kalman for the gain computation |
| Total per step | \(O(n^3 + n^2 m)\) | \(O(n^2 + nm)\) | Cholesky and sigma point propagation dominate |
For \(n \leq 10\), the additional cost over EKF is small — and the elimination of Jacobian derivation is a significant engineering benefit.
System: Same pendulum as the EKF example, \(x = [\theta, \dot\theta]^T\), \(n = 2\), \(\Delta t = 0.1\) s.
Parameters: \(\alpha = 0.1\), \(\beta = 2\), \(\kappa = 0\) → \(\lambda = -1.98\).
Initial: \(\hat{x}_0 = [0.3, 0]^T\), \(P_0 = \text{diag}(0.5, 0.5)\).
The UKF naturally captures the curvature of \(-\sin\theta\) without computing \(\cos\theta\) — the sigma points “feel” the nonlinearity directly.
ControlSize template parameter.| Filter | Jacobian Required? | Accuracy Order | Handles High Nonlinearity? | Computational Cost |
|---|---|---|---|---|
| Kalman Filter | No (linear only) | Exact (linear) | No | Lowest |
| EKF | Yes | 1st order | Moderate | Low |
| UKF | No | 2nd order | Good | Moderate |
| Particle | No | Arbitrary | Excellent | High |
graph LR
UKF["Unscented Kalman Filter"]
KF["Kalman Filter"]
EKF["Extended Kalman Filter"]
CHO["Cholesky Decomposition"]
UKF -->|"linear limit"| KF
EKF -->|"alternative to"| UKF
CHO -->|"sigma points"| UKF| Algorithm | Relationship |
|---|---|
| Kalman Filter | The UKF reduces to the standard KF when \(f\) and \(h\) are linear |
| Extended Kalman Filter | Uses Jacobians instead of sigma points; simpler but less accurate for nonlinear cases |
| Cholesky Decomposition | Used internally to generate sigma points from the covariance matrix |
The Kalman filter is a causal estimator: its state estimate at time \(t\) uses only observations up to and including time \(t\). When the full observation sequence is available (offline/batch setting), the Rauch-Tung-Striebel (RTS) smoother refines every filtered estimate by incorporating future measurements. The result is the minimum-mean-square-error (MMSE) state estimate given the entire sequence \(z_{1:T}\).
The RTS smoother is the prerequisite for the Expectation-Maximization (EM) algorithm for Kalman Filter parameter identification: the E-step requires smoothed means, smoothed covariances, and the lag-1 cross-covariance \(P_{t,t-1|T}\) in order to form the sufficient statistics used by the M-step.
\[ 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)\).
For \(t = 0, 1, \ldots, T-1\):
Initialise (t = 0 only):
\[\hat{x}_{0|-} = \mu_0, \quad P_{0|-} = P_0\]
Update:
\[ S_t = H P_{t|-} H^\top + R \]
\[ K_t = P_{t|-} H^\top S_t^{-1} \]
\[ \hat{x}_{t|t} = \hat{x}_{t|-} + K_t (z_t - H \hat{x}_{t|-}) \]
\[ P_{t|t} = (I - K_t H) P_{t|-} (I - K_t H)^\top + K_t R K_t^\top \quad \text{(Joseph form)} \]
Log-likelihood contribution:
\[ \ell_t = -\tfrac{1}{2}\bigl(\log\det S_t + (z_t - H\hat{x}_{t|-})^\top S_t^{-1} (z_t - H\hat{x}_{t|-}) + m\log 2\pi\bigr) \]
where \(m\) =
MeasurementSize. The total is \(\ell = \sum_{t=0}^{T-1} \ell_t\).
Predict (for \(t < T-1\)):
\[ \hat{x}_{t+1|-} = F \hat{x}_{t|t}, \quad P_{t+1|-} = F P_{t|t} F^\top + Q \]
Initialise at \(t = T-1\):
\[ \hat{x}_{T-1|T} = \hat{x}_{T-1|T-1}, \quad P_{T-1|T} = P_{T-1|T-1} \]
For \(t = T-2, T-3, \ldots, 0\):
Smoother gain:
\[ G_t = P_{t|t} F^\top P_{t+1|t}^{-1} \]
Smoothed mean and covariance:
\[ \hat{x}_{t|T} = \hat{x}_{t|t} + G_t (\hat{x}_{t+1|T} - \hat{x}_{t+1|t}) \]
\[ P_{t|T} = P_{t|t} + G_t (P_{t+1|T} - P_{t+1|t}) G_t^\top \]
The EM M-step requires \(P_{t,t-1|T} = \mathrm{Cov}(x_t, x_{t-1} \mid z_{1:T})\).
Initialisation (Shumway & Stoffer, 1982):
\[ P_{T-1, T-2|T} = (I - K_{T-1} H) F P_{T-2|T-2} \]
Recursion for \(t = T-3, \ldots, 0\):
\[ P_{t+1, t|T} = P_{t+1|t+1} G_t^\top + G_{t+1} (P_{t+2, t+1|T} - F P_{t+1|t+1}) G_t^\top \]
Note: \(P_{0,-1|T}\) is undefined;
index 0 of lagCrossCovariances is always zero.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(T \cdot N^3)\) | \(O(T \cdot N^2)\) stack | Dominant cost: \(N\times N\) solves at each step |
All storage is stack-allocated via std::array. There is
no heap usage.
Consider a 1-D constant-position model (\(N=1\), \(M=1\), \(T=3\)) with \(F=1\), \(H=1\), \(Q=0.1\), \(R=1\), \(\mu_0=0\), \(P_0=1\), and observations \(z = [0.5, 0.3, 0.8]\).
Forward pass:
| \(t\) | \(P_{t\|-}\) | \(S_t\) | \(K_t\) | \(\hat{x}_{t\|t}\) | \(P_{t\|t}\) |
|---|---|---|---|---|---|
| 0 | 1.0 | 2.0 | 0.500 | 0.250 | 0.500 |
| 1 | 0.600 | 1.600 | 0.375 | 0.363 | 0.375 |
| 2 | 0.475 | 1.475 | 0.322 | 0.620 | 0.322 |
Backward pass (\(t=1\) then \(t=0\)):
\(t=1\): \(G_1 = 0.500/0.600 = 0.833\); \(\hat{x}_{1|3} = 0.363 + 0.833(0.620 - 0.363) \approx 0.577\)
\(t=0\): \(G_0 = 0.375/0.375 = 1.000\); \(\hat{x}_{0|3} = 0.250 + 1.000(0.577 - 0.363) \approx 0.464\)
Notice how the smoothed \(\hat{x}_{0|3}=0.464\) incorporates all three observations, whereas the filtered \(\hat{x}_{0|0}=0.250\) only used the first.
SolveSystem (Gaussian elimination with partial
pivoting) rather than explicit matrix inversion. If the predicted
covariance becomes near-singular (possible when \(Q \approx 0\)), numerical accuracy
degrades. Ensure \(Q\) has positive
diagonal entries.lagCrossCovariances[0] is always zero:
The lag-1 cross-covariance for index 0 (\(P_{0,-1|T}\)) is undefined. The first valid
entry is index 1 (\(P_{1,0|T}\)).std::size_t; the break guard if (t == 0) break
prevents unsigned wrap-around.std::array members dominate stack usage. Choose
MaxSteps conservatively on resource-constrained
targets.ExpectationMaximization to compute the sufficient
statistics \(\{A, B, C, D, W,
\Sigma_{zz}\}\) for the M-step.filters::KalmanFilter: The forward
pass of KalmanSmoother replicates the KF update equations.
The smoother is applied after the filter, not instead
of it.estimators::ExpectationMaximization:
The smoother is owned and invoked by
ExpectationMaximization as its E-step. Users who only need
EM should interact with ExpectationMaximization
directly.solvers::GaussianElimination: Used for
all matrix “division” operations (smoother gain, Kalman gain) to avoid
explicit matrix inversion.solvers::CholeskyDecomposition: Used
to compute \(\log \det S_t\) for the
log-likelihood.Standard Kalman filter implementations propagate the error covariance matrix \(P\) directly. On resource-constrained hardware — where floating-point arithmetic is limited to 32 bits — roundoff errors accumulate with every predict-update cycle. After many iterations \(P\) can lose symmetry or positive-definiteness, causing the filter to diverge catastrophically.
The square-root Kalman filter eliminates this failure mode by never storing \(P\) explicitly. Instead it carries a Cholesky factor \(S\) such that \(P = S S^\top\), and rewrites every predict/update operation as an orthogonal triangularization. Because orthogonal transforms preserve matrix geometry exactly, the factor \(S\) — and therefore the covariance \(P = S S^\top\) — remains symmetric positive-definite by construction even in single-precision arithmetic.
A further benefit is numerical conditioning: \(\mathrm{cond}(S) = \sqrt{\mathrm{cond}(P)}\), so roughly twice as many significant bits survive across each filter step compared to the conventional form.
The underlying discrete-time linear system is identical to the conventional Kalman filter:
\[x_k = F x_{k-1} + B 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)\]
The square-root filter parameterises uncertainty through lower-triangular factors \(S\), \(S_Q\), \(S_R\) satisfying \(P = S S^\top\), \(Q = S_Q S_Q^\top\), \(R = S_R S_R^\top\).
The goal is to compute \(S^-\) such that \(P^- = F P F^\top + Q = S^- (S^-)^\top\) without forming \(P\) or \(P^-\).
Construct the \((2n \times n)\) pre-array:
\[\mathcal{A} = \begin{bmatrix} (F S)^\top \\ S_Q^\top \end{bmatrix}\]
Apply a QR decomposition \(\mathcal{A} = Q_\perp R\) (orthogonal \(Q_\perp\), upper-triangular \(R\)). Then \(S^- = R^\top\) is the new lower-triangular factor, because:
\[R^\top R = \mathcal{A}^\top \mathcal{A} = S^\top F^\top F S + S_Q^\top S_Q = F P F^\top + Q = P^-\]
Construct the \(((m+n) \times (m+n))\) pre-array:
\[\mathcal{B} = \begin{bmatrix} S_R & H S \\ 0 & S \end{bmatrix}\]
Apply QR triangularization: \(\mathcal{B} = Q_\perp \begin{bmatrix} S_y & \tilde{K} \\ 0 & S^+ \end{bmatrix}\)
The block \(S_y\) (upper-left, \(m \times m\)) is the innovation-covariance factor satisfying \(S_y S_y^\top = H P^- H^\top + R\). The block \(S^+\) (lower-right, \(n \times n\)) is the updated covariance factor. The Kalman gain emerges from the cross block:
\[K = \tilde{K}^\top S_y^{-\top}\]
solved via a triangular back-substitution rather than an explicit matrix inverse.
The state update is the standard correction:
\[x^+ = x^- + K(z - H x^-)\]
Both pre-arrays are triangularized by sequential Givens rotations applied column by column, annihilating one sub-diagonal entry per rotation. This is cache-friendly, in-place, and requires only \(O(n^2)\) temporary storage — no heap allocation.
| Case | Time | Space | Notes |
|---|---|---|---|
| Predict | \(O(n^3)\) | \(O(n^2)\) | Dominated by \(F S\) multiply and QR |
| Update | \(O((n+m) n^2)\) | \(O(n^2)\) | QR on \((m+n) \times (m+n)\) pre-array |
where \(n =\)
StateSize, \(m =\)
MeasurementSize. All arrays are fixed-size; no heap is
used.
Consider a 1-state system (\(n=1\), \(m=1\)) with \(F=1\), \(H=1\), \(S=0.5\), \(S_Q=0.1\), \(S_R=0.3\):
Predict:
Pre-array: \(\mathcal{A} = [0.5;\; 0.1]\) (column vector, already a column). \(R = \sqrt{0.5^2 + 0.1^2} = \sqrt{0.26} \approx 0.5099\). New factor \(S^- = 0.5099\), i.e. \(P^- \approx 0.26\).
Update (measurement \(z = 1.2\), prior \(x^- = 1.0\)):
Pre-array \(\mathcal{B} = \begin{bmatrix}0.3 & 0.5099 \\ 0 & 0.5099\end{bmatrix}\). One Givens rotation annihilates \(\mathcal{B}_{10}=0\), yielding upper-triangular form. \(S_y \approx 0.5974\), \(S^+ \approx 0.424\), \(K \approx 0.741\). \(x^+ = 1.0 + 0.741 \times 0.2 = 1.148\).
Any system that needs to know its 3-D orientation in space — a drone, a robot arm, an AR headset, a wearable device — must fuse data from multiple sensors. A gyroscope measures angular rate with high bandwidth and low short-term noise, but its integral drifts over time due to bias. An accelerometer measures the gravity vector, which gives a long-term absolute reference for pitch and roll, but it is contaminated by vibration. A magnetometer provides a heading reference for yaw, but it is affected by magnetic interference.
The Attitude and Heading Reference System (AHRS) filter solves the drift-correction problem in O(1) time per step with a fixed, small memory footprint. Two closely related algorithms — Madgwick’s gradient-descent filter and Mahony’s passive complementary filter — achieve this by continuously nudging the gyro-integrated quaternion so that the predicted sensor directions match the measured ones. Both are far cheaper to compute than a full quaternion Extended Kalman Filter, making them the standard choice for microcontroller-class attitude estimation.
Orientation is maintained as a unit quaternion \(q = [q_w, q_x, q_y, q_z]^T \in \mathbb{H}\), \(\|q\| = 1\), mapping from the body frame to the Earth frame. Quaternions avoid the gimbal lock inherent in Euler angles and require fewer trigonometric operations than rotation matrices per integration step.
The pure gyro propagation step integrates the body angular rate \(\boldsymbol{\omega} = [\omega_x, \omega_y, \omega_z]^T\) in rad/s:
\[\dot{q} = \frac{1}{2} q \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega} \end{bmatrix}\]
\[q_{k+1} = q_k + \dot{q} \, T_s\]
followed by renormalization. This is the predict step; without a correction it drifts.
Define the objective function as the alignment error between the predicted sensor directions and the measurements. For the gravity observation:
\[\mathbf{f}(q, \hat{\mathbf{a}}) = R(q)^T \mathbf{g}_{\text{ref}} - \hat{\mathbf{a}}\]
where \(\mathbf{g}_{\text{ref}} = [0, 0, 1]^T\) and \(\hat{\mathbf{a}}\) is the normalized accelerometer vector. The steepest-descent direction in quaternion space is:
\[\nabla F = J^T \mathbf{f}\]
where \(J = \partial \mathbf{f}/\partial q\) is the \(3 \times 4\) Jacobian of \(\mathbf{f}\) with respect to \(q\). This gradient is normalized and subtracted from the gyro-driven rate:
\[\dot{q} = \frac{1}{2} q \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega} \end{bmatrix} - \beta \, \frac{\nabla F}{\|\nabla F\|}\]
The parameter \(\beta\) is the gradient-descent step size; it is set proportional to the expected gyro measurement error in rad/s.
For the magnetometer (MARG mode), the earth frame reference is \(\mathbf{b} = [b_x, 0, b_z]^T\), where \(b_x\) and \(b_z\) are computed by rotating the normalized magnetometer measurement into the Earth frame and zeroing its \(y\)-component, making the heading reference dip-angle-agnostic. A second objective function \(\mathbf{f}_\text{mag}\) and its Jacobian are added to the gradient.
Rather than gradient descent, Mahony’s filter uses a cross-product error:
\[\mathbf{e} = \hat{\mathbf{a}} \times \mathbf{v}\]
where \(\mathbf{v}\) is the third column of \(R(q)\) (the predicted gravity direction in the body frame). The angular rate is corrected before integration:
\[\boldsymbol{\omega}_c = \boldsymbol{\omega} + K_p \mathbf{e} + \mathbf{b}_\text{est}\]
\[\dot{\mathbf{b}}_\text{est} = K_i \mathbf{e}\]
The integral term \(\mathbf{b}_\text{est}\) is a running estimate of the gyro bias; once it converges, the steady-state attitude error is driven to zero even under sustained gyro drift. The proportional gain \(K_p\) sets the bandwidth of the correction loop; \(K_i\) sets the bias-learning rate.
For MARG mode a magnetometer cross-product error is added to \(\mathbf{e}\):
\[\mathbf{e} = \hat{\mathbf{a}} \times \mathbf{v} + \hat{\mathbf{m}} \times \mathbf{w}\]
where \(\mathbf{w}\) is the predicted earth-field direction in the body frame from the current tilt.
Both algorithms renormalize \(q\) after every integration step to enforce the unit-norm constraint, compensating for the first-order Euler integration error that would otherwise slowly push \(q\) off the unit sphere.
| Case | Time | Space | Notes |
|---|---|---|---|
| UpdateImu | O(1) | O(1) | Fixed multiply-add count; one inverse-sqrt normalization |
| UpdateMarg | O(1) | O(1) | Two objective/gradient evaluations; same asymptotic cost |
| Memory | — | 7 T | 4 quaternion + 3 integral bias floats; no buffers or heap |
The fixed cost makes both algorithms suitable for any loop rate the MCU can sustain, from 100 Hz audio-rate IMUs to 8 kHz flight-controller IMUs.
Scenario: Quadrotor is hovering level. Gyro measures a small constant bias of 0.05 rad/s on the x-axis. Accelerometer reads \([0, 0, 9.81]\) m/s².
Madgwick step (simplified):
Over time without correction this drift accumulates; with the gradient term driving \(\mathbf{f} \to 0\), Madgwick continuously nudges \(q\) back to level.
Mahony step (simplified):
| Variant | Key Difference |
|---|---|
| 6-DOF (IMU-only) | Accelerometer alone; roll and pitch converge, yaw is unobservable |
| 9-DOF (MARG) | Adds magnetometer; all three angles converge given a non-disturbed field |
| Extended Kalman AHRS | Treats noise covariances explicitly; heavier but allows systematic tuning via \(Q\)/\(R\) matrices |
| Multiplicative EKF (MEKF) | Kalman update on the error quaternion to preserve unit-norm; best-in-class accuracy, high cost |
| Gradient-descent with adaptive β | Adjusts \(\beta\) based on the magnitude of the gradient, reducing transient overshoot at startup |
| Second-order Runge-Kutta integration | Reduces integration error at low update rates at the cost of one extra function evaluation |
| Algorithm | Relationship |
|---|---|
| Complementary Filter | The scalar 1-D ancestor; Madgwick/Mahony extend the idea to quaternion SO(3) |
| Extended Kalman Filter | The probabilistic alternative; heavier but allows noise covariance estimation |
| Quaternion | The state representation shared by all three-axis attitude estimators |
In embedded control and tracking applications, a sensor delivers a position measurement every sample period, but that measurement is corrupted by noise. A simple lowpass filter smooths the noise but cannot estimate velocity, which is needed for prediction and control. A full Kalman filter computes optimal gains but requires covariance propagation — a matrix inverse every step — which is too expensive for a fast ISR.
The alpha-beta (and its extension, alpha-beta-gamma) filter resolves this tension. It maintains a position and velocity estimate (and optionally acceleration) using only a few multiply-adds per sample. The gains are fixed constants, computed once at design time from a single scalar parameter. The result is a deterministic, constant-time predictor-corrector that delivers most of the benefit of a steady-state Kalman filter at a fraction of the cost.
The filter assumes constant-velocity (order 2) or constant-acceleration (order 3) kinematics. For order 2, the state is \(\mathbf{x} = [p, \dot{p}]^\top\); for order 3, \(\mathbf{x} = [p, \dot{p}, \ddot{p}]^\top\).
\[\hat{p}^- = \hat{p} + T_s \hat{v} + \tfrac{1}{2} T_s^2 \hat{a} \quad (\hat{a} \text{ omitted for order 2})\] \[\hat{v}^- = \hat{v} + T_s \hat{a} \quad (\hat{a} \text{ omitted for order 2})\] \[\hat{a}^- = \hat{a}\]
Let the innovation (residual) be \(r = z - \hat{p}^-\), where \(z\) is the measured position. Then:
\[\hat{p} = \hat{p}^- + \alpha r\] \[\hat{v} = \hat{v}^- + \frac{\beta}{T_s} r\] \[\hat{a} = \hat{a}^- + \frac{2\gamma}{T_s^2} r \quad (\text{order 3 only})\]
The denominators \(T_s\) and \(T_s^2\) convert the dimensionless residual into velocity and acceleration corrections.
For the order-2 case, Kalata (1984) defines the tracking index \(\lambda = \frac{\sigma_w T_s^2}{\sigma_v}\), where \(\sigma_w\) is process noise intensity and \(\sigma_v\) is measurement noise standard deviation. The critically-damped gains are:
\[r = \frac{4 + \lambda - \sqrt{8\lambda + \lambda^2}}{4}\] \[\alpha = 1 - r^2\] \[\beta = 2(2 - \alpha) - 4\sqrt{1 - \alpha}\]
A single scalar \(\lambda\) thus controls the smoothing/lag trade-off.
For the order-2 filter, Simpson’s triangle requires:
\[0 < \alpha < 1, \qquad 0 < \beta < 4 - 2\alpha\]
Violation of the second bound causes oscillatory divergence.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(1)\) | \(O(N)\) | \(N \in \{2, 3\}\) state words plus fixed scalars |
| Average | \(O(1)\) | \(O(N)\) | same |
| Worst | \(O(1)\) | \(O(N)\) | gains are precomputed; no covariance update |
The hot path is a handful of fused multiply-add operations: predict costs 2–4 MACs, correct costs 2–3 MACs.
Consider an order-2 filter with \(\alpha = 0.5\), \(\beta = 0.1\), \(T_s = 1.0\,\text{s}\), measuring a ramp \(z[n] = 0.2n\).
| Step | \(z\) | \(\hat{p}^-\) | \(\hat{v}^-\) | \(r\) | \(\hat{p}\) | \(\hat{v}\) |
|---|---|---|---|---|---|---|
| 0 (seed) | 0.0 | — | — | — | 0.0 | 0.0 |
| 1 | 0.2 | 0.0 | 0.0 | 0.2 | 0.10 | 0.020 |
| 2 | 0.4 | 0.12 | 0.020 | 0.28 | 0.26 | 0.048 |
| … | … | … | … | … | … | … |
After several hundred steps, \(\hat{v} \to 0.2\) and lag \(\to 0\).
Real-time attitude and heading estimation requires fusing two fundamentally different sensor modalities: a gyroscope that integrates angular rate to produce a short-term angle estimate (fast response, low noise, but subject to drift) and an accelerometer or magnetometer that reads the angle directly (accurate at rest, noisy during motion, slow dynamics). Neither sensor alone is sufficient. The complementary filter solves the fusion problem with two multiplies and two adds per sample, making it the default tilt estimator on virtually every IMU-based embedded project where a full Kalman filter is unaffordable.
The two sensor paths are complementary in the transfer-function sense: the gyro path acts as a first-order high-pass filter and the direct-angle path acts as a first-order low-pass filter sharing the same crossover frequency \(\omega_c = 1/\tau\). Their sum is identically unity for all frequencies:
\[H_{HP}(s) + H_{LP}(s) = 1\]
This ensures that no frequency content is amplified or attenuated by the fusion itself.
Given the fused angle \(\theta_k\), gyro rate \(\omega_k\), accelerometer angle \(\theta_{acc,k}\), sample period \(T_s\), and blend weight \(\alpha \in [0,1]\):
\[\theta_{k+1} = \alpha\,(\theta_k + \omega_k\,T_s) + (1-\alpha)\,\theta_{acc,k}\]
The term \(\theta_k + \omega_k T_s\) is the high-pass path (integration of the fast sensor), and \(\theta_{acc,k}\) is the low-pass path (direct measurement from the slow sensor).
The single tuning knob is the crossover time constant \(\tau\), which maps to \(\alpha\) via:
\[\alpha = \frac{\tau}{\tau + T_s}\]
Below \(1/\tau\) the filter trusts the accelerometer; above it, the gyroscope. Typical embedded values are \(\tau = 0.5\)–\(2\) s, corresponding to \(\alpha \approx 0.98\) at \(T_s = 10\) ms.
When the state is a heading angle in \((-\pi, \pi]\), a naive linear blend can jump by \(2\pi\) near the seam. Instead the blend is performed along the shortest arc:
\[\delta = \mathrm{WrapToPi}(\theta_{acc} - \hat{\theta})\] \[\theta_{k+1} = \mathrm{WrapToPi}\!\left(\hat{\theta} + (1-\alpha)\,\delta\right)\]
where \(\mathrm{WrapToPi}\) reduces an angle into \((-\pi, \pi]\) — the library-wide angle convention, so an angle of exactly \(-\pi\) is reported as \(+\pi\) (see Angle Utilities).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Update | \(O(1)\) | \(O(1)\) | 2 multiplies, 2 adds; 1 fmod when wrapping |
| Reset | \(O(1)\) | \(O(1)\) | Single state write |
Total storage: one angle word plus two constant coefficients.
Setup: \(\alpha = 0.98\), \(T_s = 10\) ms, initial angle \(= 0\).
Sample 1: gyro rate \(\omega = 10\) deg/s \(= 0.1745\) rad/s, accel reads \(\theta_{acc} = 0.01\) rad.
\[\hat{\theta} = 0 + 0.1745 \times 0.01 = 0.001745 \text{ rad} \quad (\text{gyro path})\] \[\theta_1 = 0.98 \times 0.001745 + 0.02 \times 0.01 = 0.001710 + 0.000200 = 0.001910 \text{ rad}\]
After many samples with zero rate and accel \(= 0.2\) rad: the low-pass term accumulates and \(\theta \to 0.2\) rad as \((1-\alpha)^n \to 0\).
| Variant | Key Difference |
|---|---|
| Mahony filter | 3-D quaternion formulation with integral gyro-bias estimator |
| Madgwick filter | Gradient-descent quaternion fusion; no linearisation |
| Alpha-Beta filter | Fixed-gain tracking without a slow sensor; pure high-pass integration |
| Kalman filter | Optimal (minimum-variance) fusion; requires noise covariance tuning |
| Two-step complementary | Separate pitch/roll from heading; common on 6-DOF IMUs |
| Algorithm | Relationship |
|---|---|
| Exponential Moving Average | The low-pass path in isolation; \(\alpha_{EMA} = 1-\alpha_{CF}\) |
| Alpha-Beta Filter | Complementary filter without a slow-sensor reference; fixed-gain tracker |
| AHRS Madgwick/Mahony | 3-D quaternion generalization with gyro-bias estimation |
| Kalman Filter | Statistically optimal generalization requiring \(Q\) and \(R\) tuning |
High-order IIR filters realized as a single monolithic direct-form transfer function suffer from severe numerical sensitivity: small perturbations in any coefficient can shift poles dramatically, and round-off noise accumulates in proportion to the filter order. These effects are catastrophic on fixed-point hardware and non-trivial even in single-precision floating-point.
The solution adopted universally in professional DSP is to factor the transfer function into quadratic terms — each with exactly two poles and two zeros — and chain them in series. This cascade of second-order sections (“biquads”) keeps each section’s coefficients well-scaled, its pole-sensitivity small, and its round-off noise bounded independently of the total filter order.
Any real-coefficient rational transfer function of order \(N\) factors as:
\[H(z) = G \prod_{k=1}^{\lceil N/2 \rceil} \frac{b_{0,k} + b_{1,k}z^{-1} + b_{2,k}z^{-2}}{1 + a_{1,k}z^{-1} + a_{2,k}z^{-2}}\]
Each factor is a second-order section (SOS). The overall frequency response is the product of the per-section responses; the total group delay is the sum of per-section group delays.
For standard shelving, peaking, and pass/reject types, closed-form normalized coefficients derive from the bilinear transform applied to an analog prototype. Define:
\[\omega_0 = \frac{2\pi f_c}{f_s}, \qquad c_\omega = \cos(\omega_0), \qquad \alpha = \frac{\sin(\omega_0)}{2Q}\]
Low-pass:
\[b_0 = \frac{1 - c_\omega}{2(1+\alpha)}, \quad b_1 = \frac{1-c_\omega}{1+\alpha}, \quad b_2 = b_0\]
\[a_1 = \frac{-2c_\omega}{1+\alpha}, \quad a_2 = \frac{1-\alpha}{1+\alpha}\]
High-pass: replace \((1 - c_\omega)\) with \((1 + c_\omega)\) and negate \(b_1\).
Notch (band-reject):
\[b_0 = b_2 = \frac{1}{1+\alpha}, \quad b_1 = a_1 = \frac{-2c_\omega}{1+\alpha}, \quad a_2 = \frac{1-\alpha}{1+\alpha}\]
All coefficients are pre-normalized by \(a_0 = 1 + \alpha\), so the feedback denominator leading coefficient is always 1 and the hot path requires no division.
TDF-II is the canonical embedded realization. It minimizes the number of state registers to two per section while achieving low round-off noise. The recurrence for section \(k\) is:
\[y[n] = b_0 x[n] + z_1[n-1]\]
\[z_1[n] = b_1 x[n] - a_1 y[n] + z_2[n-1]\]
\[z_2[n] = b_2 x[n] - a_2 y[n]\]
This is exactly five multiplies and four adds per sample — the theoretical minimum for an arbitrary biquad — and requires no intermediate storage beyond \(z_1\) and \(z_2\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Per sample | \(O(S)\) | \(O(S)\) | \(S\) = number of sections; 5 mults + 4 adds each |
| Design | \(O(1)\) | \(O(1)\) | Closed-form RBJ formulas; no iteration |
| Reset | \(O(S)\) | — | Zero two state words per section |
Example: design a 4th-order low-pass as two cascaded 2nd-order sections.
Given \(f_c = 100\) Hz, \(f_s = 1000\) Hz, \(Q = 0.707\) (Butterworth factor for each section):
Impulse response trace for bypass section \(\{b_0=1, b_1=b_2=a_1=a_2=0\}\): Input \([1, 0, 0, \ldots]\) → output \([1, 0, 0, \ldots]\) — identity passthrough.
math::RecursiveBuffer for time-domain state; the biquad
avoids the overhead by keeping only two explicit state scalars per
section.Decimation and interpolation in digital signal processing typically require a lowpass anti-aliasing filter before the rate change. Standard FIR filters require multiplications proportional to their order. The CIC filter achieves a highly efficient lowpass response using only additions and subtractions, making it ideal for resource-constrained embedded systems where multipliers are expensive or unavailable.
CIC filters are used in sigma-delta ADC interfaces, software-defined radio front-ends, and any application that must drastically reduce the sample rate of a high-frequency signal stream before further processing.
A CIC decimator of order \(N\) with decimation ratio \(R\) and differential delay \(M\) is defined by its \(z\)-domain transfer function:
\[H(z) = \left( \frac{1 - z^{-RM}}{1 - z^{-1}} \right)^N\]
The numerator factor \(1 - z^{-RM}\) is the \(z\)-transform of the comb (differencing) stage. The denominator \(\frac{1}{1 - z^{-1}}\) is the accumulator (integrator) stage.
The filter consists of two cascaded sections:
Integrator section (running at the high input rate \(f_s\)): \(N\) stages of first-order IIR accumulators,
\[y_k[n] = y_k[n-1] + y_{k-1}[n], \quad k = 1, \ldots, N\]
Comb section (running at the low output rate \(f_s / R\)): \(N\) stages of differencing with delay \(M\),
\[y_k[m] = y_{k-1}[m] - y_{k-1}[m - M], \quad k = 1, \ldots, N\]
The unnormalized DC gain of the filter is:
\[G = (R \cdot M)^N\]
All outputs are divided by \(G\) to normalize the DC gain to unity for a constant input.
The magnitude response in the baseband is approximately:
\[|H(f)| = \left| \frac{\sin(\pi f M R / f_s)}{R \sin(\pi f / f_s)} \right|^N\]
This is a sinc-like response that suppresses high-frequency content before the rate change.
| Case | Time per input sample | Space | Notes |
|---|---|---|---|
| Best | \(O(N)\) | \(O(N \cdot M)\) | \(N\) integrator ops; comb only at decimation points |
| Average | \(O(N)\) | \(O(N \cdot M)\) | Same |
| Worst | \(O(N)\) | \(O(N \cdot M)\) | Comb adds \(N\) differencing ops at rate \(f_s/R\) |
The integrator section executes \(N\) additions per input sample. The comb section executes \(N\) subtractions once every \(R\) input samples. There are no multiplications in the signal path.
Consider \(N=2\), \(R=4\), \(M=1\), input impulse \(x[0]=1\), \(x[n]=0\) for \(n>0\).
Integrator section at \(n=0,1,2,3\):
| \(n\) | Input | Integrator 1 | Integrator 2 |
|---|---|---|---|
| 0 | 1 | 1 | 1 |
| 1 | 0 | 1 | 2 |
| 2 | 0 | 1 | 3 |
| 3 | 0 | 1 | 4 |
Comb section at decimated sample \(m=0\) (triggered at \(n=3\)):
At \(m=1\) (triggered at \(n=7\)):
Integer overflow in fixed-point implementations: in fixed-point arithmetic, the integrators accumulate without bound between comb operations. Registers must be wide enough to hold \((R \cdot M)^N\) times the maximum input value. This implementation uses floating-point, which avoids this issue.
Initial transient: the filter takes several R-length blocks to settle to steady-state behavior for a constant input. DC normalization is exact only after the delay pipeline is fully flushed.
Passband droop: the sinc-shaped response causes attenuation even near DC as the input frequency increases. Compensation filters or a larger \(R\) reduce in-band droop.
Aliasing from high-order terms: if the input signal has energy above \(f_s / (2R)\), aliased components will appear at the output. A simple prefilter can reduce this.
Interpolating CIC: the comb section runs at the low rate and the integrators at the high rate, acting as an upsampler. The architecture mirrors the decimator with sections swapped.
Pruned CIC: removes multiplier-free adder stages that contribute negligibly to the response, reducing hardware at the cost of response shape.
Compensation filter: a short linear-phase FIR appended at the low rate corrects passband droop without reintroducing multipliers in the high-rate path.
Variable-rate CIC: by making \(R\) a runtime parameter, one filter structure supports multiple decimation ratios, useful in SDR front-ends.
A CIC filter of order \(N=1\), \(M=1\) is equivalent to a boxcar (rectangular window) FIR of length \(R\), identical to a Moving Average filter operating on non-overlapping blocks. The Moving Average filter in this library is the continuous-output counterpart.
Higher-order CICs approximate a Gaussian response as \(N \to \infty\), connecting them to the Gaussian filter family in theory.
The CIC is a special case of the more general Hogenauer filter structure, which can be pruned to reduce word widths at each stage.
Sensor data in embedded systems is noisy. A full moving-average buffer costs O(N) RAM and O(N) computation per sample. The exponential moving average (EMA), also called a one-pole recursive low-pass filter or leaky integrator, achieves useful smoothing with a single state word and one multiply-accumulate per sample. It is the minimal-cost filter that tracks a slowly varying DC level while suppressing high-frequency noise.
Given a smoothing factor \(\alpha \in (0, 1]\) and a discrete input sequence \(x[n]\), the EMA output \(y[n]\) obeys the first-order linear recurrence:
\[y[n] = \alpha \, x[n] + (1 - \alpha) \, y[n-1]\]
with \(y[-1] = y_0\) (the initial state, often zero).
Expanding the recurrence:
\[y[n] = y[n-1] + \alpha \bigl(x[n] - y[n-1]\bigr)\]
This form has one subtraction and one multiply-accumulate; it avoids computing \((1-\alpha)\) explicitly and keeps the error term \((x[n] - y[n-1])\) small, which is numerically preferable in fixed-point arithmetic.
The transfer function in the \(z\)-domain is:
\[H(z) = \frac{\alpha}{1 - (1-\alpha)\,z^{-1}}\]
The single pole is at \(z = 1 - \alpha\). For \(\alpha \in (0, 1]\) the pole lies in \([0, 1)\), which is strictly inside the unit circle, so the filter is unconditionally stable.
Setting \(z = 1\) (DC, \(\omega = 0\)):
\[H(1) = \frac{\alpha}{1 - (1-\alpha)} = 1\]
Unity DC gain: a constant input converges exactly to that constant.
Matching the EMA pole to a first-order RC low-pass filter with time constant \(\tau = 1/(2\pi f_c)\) and sample period \(T_s = 1/f_s\):
\[\alpha = \frac{T_s}{\tau + T_s} = \frac{T_s}{\frac{1}{2\pi f_c} + T_s}\]
The impulse response is \(h[n] = \alpha (1-\alpha)^n \, u[n]\), which decays geometrically — hence the name “exponential” moving average.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | O(1) | O(1) | Single MAC per sample, one state word |
| Average | O(1) | O(1) | Identical regardless of input |
| Worst | O(1) | O(1) | No data-dependent branches on hot path |
Suppose \(\alpha = 0.5\), \(y[-1] = 0\), and the input is the unit impulse \(x = [1, 0, 0, 0, \ldots]\).
| \(n\) | \(x[n]\) | \(x[n] - y[n-1]\) | \(\alpha \cdot \Delta\) | \(y[n]\) |
|---|---|---|---|---|
| 0 | 1 | \(1 - 0 = 1\) | \(0.5\) | \(0.5\) |
| 1 | 0 | \(0 - 0.5 = -0.5\) | \(-0.25\) | \(0.25\) |
| 2 | 0 | \(0 - 0.25 = -0.25\) | \(-0.125\) | \(0.125\) |
| 3 | 0 | \(0 - 0.125 = -0.125\) | \(-0.0625\) | \(0.0625\) |
Each output is half the previous, confirming \(h[n] = (0.5)^{n+1}\).
A Finite Impulse Response (FIR) filter is a discrete-time filter whose impulse response has a finite duration. FIR filters are inherently stable, have linear phase properties (when symmetric), and are straightforward to design and implement. They are the default choice for applications requiring guaranteed stability and predictable phase behavior.
A causal FIR filter of order \(N-1\) (with \(N\) taps) computes the output as a weighted sum of the current and past inputs (discrete convolution):
\[y[n] = \sum_{i=0}^{N-1} b_i \cdot x[n-i]\]
where: - \(b_i\) are the filter coefficients (tap weights) - \(x[n-i]\) are the current and \(N-1\) most recent input samples - \(N\) is the number of taps (filter length)
In the z-domain, the FIR filter transfer function is a polynomial in \(z^{-1}\):
\[H(z) = \sum_{i=0}^{N-1} b_i z^{-i}\]
Since the denominator is unity, FIR filters have all poles at the origin and are always stable.
Evaluating on the unit circle (\(z = e^{j\omega}\)):
\[H(e^{j\omega}) = \sum_{i=0}^{N-1} b_i e^{-j\omega i}\]
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(N)\) | \(O(N)\) | Disabled (passthrough) |
| Average | \(O(N)\) | \(O(N)\) | Convolution sum |
| Worst | \(O(N)\) | \(O(N)\) | Same — no data-dependent branching |
Time is per sample. Space is for the coefficient and delay line buffers.
Given a 3-tap FIR filter with coefficients \(b = [0.25, 0.5, 0.25]\) and input sequence \(x = [1.0, 2.0, 3.0, 4.0]\):
| \(n\) | \(x[n]\) | \(x[n-1]\) | \(x[n-2]\) | \(y[n] = 0.25 x[n] + 0.5 x[n-1] + 0.25 x[n-2]\) |
|---|---|---|---|---|
| 0 | 1.0 | 0.0 | 0.0 | 0.25 |
| 1 | 2.0 | 1.0 | 0.0 | 1.0 |
| 2 | 3.0 | 2.0 | 1.0 | 2.0 |
| 3 | 4.0 | 3.0 | 2.0 | 3.0 |
windowing module are used in FIR coefficient design.An Infinite Impulse Response (IIR) filter uses feedback from its own past outputs to achieve a desired frequency response. IIR filters can match the selectivity of high-order FIR filters using far fewer coefficients, making them efficient for resource-constrained embedded systems. The trade-off is potential instability and nonlinear phase.
A causal IIR filter with \(P\) feedforward coefficients and \(Q\) feedback coefficients computes:
\[y[n] = \sum_{i=0}^{P-1} b_i \cdot x[n-i] + \sum_{i=0}^{Q-1} a_i \cdot y[n-i]\]
where: - \(b_i\) are the feedforward (numerator) coefficients - \(a_i\) are the feedback (denominator) coefficients - \(x[n-i]\) are past input samples - \(y[n-i]\) are past output samples
Sign convention: The implementation uses addition for the feedback term. This means the feedback coefficients \(a_i\) must be provided with the appropriate sign (negated relative to the standard form \(y[n] = \sum b_i x[n-i] - \sum a_i y[n-i]\)).
\[H(z) = \frac{\sum_{i=0}^{P-1} b_i z^{-i}}{1 - \sum_{i=0}^{Q-1} a_i z^{-i}}\]
The filter is stable if and only if all poles of \(H(z)\) lie strictly inside the unit circle (\(|z| < 1\)). This must be verified at design time.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(P+Q)\) | \(O(P+Q)\) | Disabled (passthrough) |
| Average | \(O(P+Q)\) | \(O(P+Q)\) | Feedforward + feedback sums |
| Worst | \(O(P+Q)\) | \(O(P+Q)\) | Same — sequential, no branching |
Time is per sample. Space stores coefficient and delay line buffers.
Given a first-order IIR (1 feedforward, 1 feedback) with \(b_0 = 0.5\), \(a_0 = 0.3\) and input \(x = [1.0, 0.0, 0.0]\):
| \(n\) | \(x[n]\) | \(y[n-1]\) | \(y[n] = 0.5 \cdot x[n] + 0.3 \cdot y[n-1]\) |
|---|---|---|---|
| 0 | 1.0 | 0.0 | 0.5 |
| 1 | 0.0 | 0.5 | 0.15 |
| 2 | 0.0 | 0.15 | 0.045 |
The output decays exponentially — characteristic of IIR filters with \(|a_0| < 1\).
FrequencyResponse class evaluates \(H(e^{j\omega})\) for both FIR and IIR
filters by accepting both \(b\) and
\(a\) coefficient arrays.Embedded systems often require adaptive filtering — adjustable cutoff frequencies, runtime-selected filter orders, or field-reconfigurable equalizer bands — without a host toolchain to bake in coefficients. This algorithm computes second-order-section (SOS) biquad coefficients for Butterworth and Chebyshev Type-I IIR filters entirely on the device, using only fixed-point-safe arithmetic and bounded arrays.
The design pipeline converts a normalized analog prototype (unit-cutoff Butterworth or Chebyshev ellipse of poles) to a digital filter at any requested cutoff, applying the bilinear transform with pre-warping to guarantee that the digital cut-off frequency lands exactly on the requested value.
The \(N\)th-order Butterworth prototype places poles uniformly on the left half of the unit circle in the \(s\)-plane:
\[s_k = e^{j\pi(2k + N + 1)/(2N)}, \quad k = 0, 1, \ldots, N-1\]
All poles have magnitude 1 and lie strictly in the open left half-plane, guaranteeing stability and a maximally flat (Butterworth) magnitude response.
Chebyshev-I poles lie on an ellipse in the \(s\)-plane. Given ripple \(R_p\) dB:
\[\varepsilon = \sqrt{10^{R_p/10} - 1}\]
\[\alpha = \frac{1}{N}\sinh^{-1}\!\left(\frac{1}{\varepsilon}\right)\]
\[s_k = -\sinh(\alpha)\sin\theta_k + j\cosh(\alpha)\cos\theta_k, \quad \theta_k = \frac{\pi(2k+1)}{2N}\]
This trades the maximally-flat passband of Butterworth for equiripple passband with steeper transition-band roll-off at the same order.
The bilinear transform compresses the entire analog frequency axis \((-\infty, +\infty)\) onto the digital unit circle non-linearly. To ensure the digital cutoff \(f_c\) maps exactly to the correct position after this compression, the analog prototype cutoff \(\omega_c\) is pre-warped:
\[\omega_c = 2 f_s \tan\!\left(\frac{\pi f_c}{f_s}\right)\]
A normalized (LP cutoff = 1 rad/s) prototype is frequency-transformed before bilinear mapping:
Each analog root \(s_k\) is mapped to the \(z\)-plane:
\[z_k = \frac{1 + s_k / (2 f_s)}{1 - s_k / (2 f_s)}\]
This is an exact Möbius transformation that maps the left \(s\)-half-plane to the interior of the unit circle, preserving stability.
Complex-conjugate pole pairs \((z_k, z_k^*)\) are combined into real-coefficient biquad sections:
\[H_i(z) = \frac{b_0 + b_1 z^{-1} + b_2 z^{-2}}{1 + a_1 z^{-1} + a_2 z^{-2}}\]
with \(a_1 = -2\,\mathrm{Re}(z_k)\) and \(a_2 = |z_k|^2\). A single real pole yields a first-order section padded as a biquad with \(b_2 = a_2 = 0\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Design | \(O(N)\) | \(O(N)\) static | One-off; no per-sample cost |
| Filter | \(O(N/2)\) | \(O(N/2)\) state | Delegated to BiquadCascade |
All arrays are bounded by the compile-time MaxOrder
template parameter; no heap is used.
Example: 2nd-order Butterworth LP, \(f_c = 100\) Hz, \(f_s = 1000\) Hz.
Prototype poles (\(N=2\)): \(s_{0,1} = e^{j\cdot 3\pi/4}, e^{j\cdot 5\pi/4} = \frac{-1 \pm j}{\sqrt{2}}\)
Pre-warp: \(\omega_c = 2 \times 1000 \times \tan(\pi \times 100 / 1000) \approx 726.5\) rad/s
Scale LP: \(s_{0,1} \leftarrow s_{0,1} \times \omega_c\)
Bilinear map: each scaled pole \(\to z_{0,1}\) inside the unit circle.
SOS: conjugate pair forms one biquad with real coefficients \(b_0, b_1, b_2, a_1, a_2\).
Gain normalization: DC gain adjusted so \(|H(0)| = 1\).
MaxOrder
accounts for the doubled count.double instantiation
(via the generic T) improves coefficient accuracy for very
high order or narrowband designs before quantizing to
float.Impulsive noise — EMI clicks, dropped ADC bits, salt-and-pepper sensor glitches — corrupts embedded sensor streams in a way that linear averaging amplifies rather than rejects. A single outlier sample at 100× the signal level drags a moving-average output for the entire window duration. The median filter eliminates isolated spikes completely, using only comparisons and no arithmetic on the signal values themselves, making it an ideal non-linear pre-processor for noisy ADC and sensor streams.
Let \(x[n]\) be the input sequence and \(N\) the window length. Define the sorted window snapshot as \(x_{(1)} \le x_{(2)} \le \cdots \le x_{(N)}\) (order statistics). The median filter output is:
\[y[n] = x_{(\lceil N/2 \rceil)}\]
For odd \(N\) this is the unique middle order statistic. For even \(N\) one common convention takes the average of the two central statistics, but this introduces arithmetic on the signal and the associated round-off.
A spike is suppressed if and only if it does not form a strict majority of the window, i.e., if fewer than \(\lceil N/2 \rceil\) samples share the outlier level. An isolated impulse occupies exactly one slot; it is always rejected for \(N \ge 3\).
A step transition from level \(a\) to level \(b\) causes the median to switch from \(a\) to \(b\) after exactly \(\lfloor N/2 \rfloor\) samples of delay — without overshoot, Gibbs ringing, or smearing. This is the fundamental advantage over linear filters: the nonlinearity of the sort operation preserves edges while rejecting impulses.
The causal sliding-window median introduces a latency of \(\lfloor (N-1)/2 \rfloor\) samples, which is the same group delay as a symmetric FIR of the same length. Unlike a linear FIR, however, the delay is signal-dependent near edges.
For small, fixed window sizes \(N\) (typically 3–9 in embedded use), copying the ring buffer into a scratch array and applying insertion sort is cache-friendly and avoids heap allocation. The comparison count is at most \(N(N-1)/2\) — fully bounded by the compile-time constant \(N\).
Insertion sort on the scratch copy:
\[\text{for } i = 1, \ldots, N-1: \text{ shift } \text{scratch}[0 \ldots i-1] \text{ right until sorted position for scratch}[i]\]
The median is then \(\text{scratch}[\lfloor N/2 \rfloor]\).
Huang, Yang, and Tang (1979) describe an \(O(1)\)-per-sample update based on a running histogram: the departing sample decrements its histogram bin and the arriving sample increments its bin, then the median bin is walked left or right by one position. This reduces per-sample work from \(O(N)\) to \(O(1)\) but requires a histogram array indexed over the signal alphabet — impractical for floating-point streams on embedded targets.
| Variant | Time per sample | Space | Notes |
|---|---|---|---|
| Insertion sort (used) | \(O(N)\) | \(O(N)\) | Optimal for small fixed \(N\); no heap |
| Huang histogram | \(O(1)\) | \(O(K)\) | \(K\) = alphabet size; integer signals only |
For \(N \le 9\) the insertion-sort inner loop is unrolled by the compiler and runs in a handful of compare-and-swap operations — faster in practice than histogram maintenance.
Input: \(x = [0.1, 0.1, 0.9, 0.1, 0.1]\), window size \(N = 3\), initial window pre-seeded with \(0.0\).
| \(n\) | \(x[n]\) | Window (ring buffer) | Sorted scratch | \(y[n]\) |
|---|---|---|---|---|
| 0 | 0.1 | [0.0, 0.0, 0.1] | [0.0, 0.0, 0.1] | 0.0 |
| 1 | 0.1 | [0.0, 0.1, 0.1] | [0.0, 0.1, 0.1] | 0.1 |
| 2 | 0.9 | [0.1, 0.1, 0.9] | [0.1, 0.1, 0.9] | 0.1 |
| 3 | 0.1 | [0.1, 0.9, 0.1] | [0.1, 0.1, 0.9] | 0.1 |
| 4 | 0.1 | [0.9, 0.1, 0.1] | [0.1, 0.1, 0.9] | 0.1 |
The bold value is the median at index \(\lfloor 3/2 \rfloor = 1\). The spike at \(n = 2\) (\(x = 0.9\)) appears in only one of the three window slots at any given time, so it is never the majority and never reaches the output.
MovingAverage is the linear equivalent; it smears both
noise and edges, while the median preserves edges at the cost of being
nonlinear and harder to analyse in the frequency domain.ExponentialMovingAverage provides even lower memory
cost but is sensitive to impulsive outliers.FirFilter with optimal coefficients can achieve
equiripple passband/stopband behaviour but cannot reject isolated spikes
without distorting step edges.Sensor streams in embedded systems are corrupted by zero-mean white noise. The simplest way to attenuate that noise is to average consecutive samples. The moving average does this with a sliding window of length N, treating every sample inside the window equally. Its critical property for embedded use is that the recursive running-sum form requires exactly one addition, one subtraction, and one multiply per output sample — constant work regardless of how large the window is.
Let \(x[n]\) be the input sequence. The length-\(N\) boxcar output is:
\[y[n] = \frac{1}{N} \sum_{k=0}^{N-1} x[n-k]\]
Expanding \(y[n]\) in terms of \(y[n-1]\):
\[y[n] = y[n-1] + \frac{x[n] - x[n-N]}{N}\]
Equivalently, maintain a running sum \(S[n] = \sum_{k=0}^{N-1} x[n-k]\):
\[S[n] = S[n-1] + x[n] - x[n-N], \qquad y[n] = \frac{S[n]}{N}\]
The discrete-time transfer function is:
\[H(z) = \frac{1}{N} \sum_{k=0}^{N-1} z^{-k} = \frac{1}{N} \cdot \frac{1 - z^{-N}}{1 - z^{-1}}\]
The magnitude response is a \(\mathrm{sinc}\)-shaped envelope:
\[|H(e^{j\omega})| = \frac{1}{N} \left| \frac{\sin(N\omega/2)}{\sin(\omega/2)} \right|\]
DC gain is exactly 1. Group delay is constant at \((N-1)/2\) samples — the filter is linear phase.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(1)\) | \(O(N)\) | One add, one sub, one multiply |
The \(O(1)\) time cost is the key advantage over the naive \(O(N)\) direct-form sum.
Input: unit step \(x[n] = 1\) for \(n \ge 0\), \(N = 4\), initial state all zeros.
| \(n\) | \(x[n]\) | \(x[n-4]\) | \(S[n]\) | \(y[n]\) |
|---|---|---|---|---|
| 0 | 1 | 0 | 1 | 0.25 |
| 1 | 1 | 0 | 2 | 0.50 |
| 2 | 1 | 0 | 3 | 0.75 |
| 3 | 1 | 0 | 4 | 1.00 |
| 4 | 1 | 1 | 4 | 1.00 |
The output ramps up linearly and then holds at 1 once the window is fully filled.
ExponentialMovingAverage solves the same smoothing
problem with \(O(1)\) memory at the
cost of non-linear phase and an infinite impulse response.SavitzkyGolayFilter generalizes the boxcar by fitting a
polynomial, preserving higher-order signal features.Narrow-band interference — mains hum at 50/60 Hz and its harmonics — contaminates bio-signal acquisition (ECG, EEG, EMG) and precision instrumentation. A notch filter surgically eliminates a single frequency while leaving the rest of the spectrum intact. A comb filter extends this idea to an entire harmonic series using a single delay line, making it far more efficient than cascading individual notches when the interference is periodic.
The continuous-time transfer function for a band-reject filter with centre frequency \(f_0\) and quality factor \(Q\) is:
\[H(s) = \frac{s^2 + \omega_0^2}{s^2 + \frac{\omega_0}{Q}s + \omega_0^2}\]
Applying the bilinear transform with \(\omega_0 = 2\pi f_0 / f_s\), the discrete-time biquad coefficients (Bristow-Johnson “Cookbook” notch) are:
\[\alpha = \frac{\sin(\omega_0)}{2Q}, \quad c_\omega = \cos(\omega_0)\]
\[b_0 = b_2 = \frac{1}{1+\alpha}, \quad b_1 = \frac{-2c_\omega}{1+\alpha}\]
\[a_1 = \frac{-2c_\omega}{1+\alpha}, \quad a_2 = \frac{1-\alpha}{1+\alpha}\]
The zeros lie exactly on the unit circle at \(e^{\pm j\omega_0}\), guaranteeing infinite attenuation at \(f_0\) in exact arithmetic.
Transposed Direct Form II (TDF-II) realises this with two state variables \(z_1, z_2\):
\[y[n] = b_0 x[n] + z_1[n-1]\] \[z_1[n] = b_1 x[n] - a_1 y[n] + z_2[n-1]\] \[z_2[n] = b_2 x[n] - a_2 y[n]\]
A comb filter exploits the identity: subtracting (or adding) a signal delayed by \(D\) samples creates periodic nulls (or peaks) spaced every \(f_s / D\) Hz:
Feedforward (FIR) comb:
\[y[n] = x[n] - g \cdot x[n-D]\]
Zeros at \(\omega = 2\pi k / D\) for \(k = 0, 1, \ldots, D-1\). The system is always stable.
Feedback (IIR) comb:
\[y[n] = x[n] + g \cdot y[n-D]\]
Poles at the same locations. The system is stable if and only if \(|g| < 1\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Notch | \(O(1)\) | \(O(1)\) | Fixed biquad; 2 multiplies/adds |
| Comb | \(O(1)\) | \(O(D)\) | Single circular-buffer read/write |
Notch example — \(f_0 = 50\) Hz, \(f_s = 1000\) Hz, \(Q = 10\):
Comb example — \(D = 4\), \(g = 1\) (FIR):
Impulse response: \([1, 0, 0, 0, -1, 0, 0, 0, 0, \ldots]\) — zeros at 0, fs/4, fs/2, 3fs/4.
Many sensor signals — ECG, PPG, spectroscopy, vibration — carry narrow peaks whose height, width, and area carry physical meaning. A boxcar moving average suppresses noise by treating the signal as locally constant, but this assumption flattens and widens every peak. The Savitzky-Golay filter replaces that assumption with a stronger one: the signal is locally a low-degree polynomial. Fitting that polynomial to each window by least squares and evaluating it at the window centre preserves peak shape far better while still attenuating high-frequency noise.
Let \(x[n]\) be the input sequence. Define a symmetric window of length \(W\) (odd) centred at sample \(n\), with half-width \(h = (W-1)/2\). The window offsets are \(j = -h, -h+1, \ldots, h\).
Fit a polynomial \(p(j) = \sum_{d=0}^{P} a_d \, j^d\) of degree \(P < W\) to the window samples \(\{x[n+j]\}\) in the least-squares sense. The output for the \(D\)-th derivative at the centre is:
\[y[n] = D! \, a_D\]
Assemble the Vandermonde matrix \(A \in \mathbb{R}^{W \times (P+1)}\) with \(A_{k,d} = j_k^{\,d}\), \(j_k = k - h\). The least-squares coefficients solve:
\[(A^\top A)\,\mathbf{a} = A^\top \mathbf{x}\]
The projection matrix \(C = (A^\top A)^{-1} A^\top\) is \((P+1) \times W\). Row \(D\) of \(C\), multiplied by \(D!\), gives the FIR convolution kernel \(\mathbf{c} \in \mathbb{R}^W\):
\[y[n] = \sum_{k=0}^{W-1} c_k \, x[n - h + k]\]
Because \(A\) depends only on \(W\) and \(P\), the kernel \(\mathbf{c}\) is fixed and computed once.
The centre row (\(D = 0\)) reproduces a polynomial fit evaluated at \(j = 0\). For the standard 5-point, quadratic (\(P = 2\)) case the kernel is:
\[\mathbf{c} = \frac{1}{35}[-3, 12, 17, 12, -3]\]
DC gain is exactly 1; the kernel is symmetric, so the filter has linear phase with group delay \(h = (W-1)/2\) samples.
Row \(D\) of \(C\) times \(D!\) yields the \(D\)-th smoothed derivative kernel. For \(D = 1\) the output approximates \(\mathrm{d}x/\mathrm{d}n\) (in samples); multiply by \(1/T_s^D\) to convert to physical units.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(W)\) | \(O(W)\) | One multiply-accumulate per tap per sample |
| Init | \(O(P^3)\) | \(O(P^2 W)\) | Constexpr kernel precomputed at compile time |
The runtime cost is \(W\) multiply-adds — identical to a length-\(W\) FIR filter. The \(O(P^3)\) Gaussian elimination occurs entirely at compile time with zero runtime overhead.
Window \(W = 5\), polynomial order \(P = 2\), derivative \(D = 0\), half-width \(h = 2\):
Input \([0, 0, 0, 0, 1]\): output = \(-3/35 \approx -0.086\) (transient). After the delay line fills with a constant signal \(v\), output = \(v\) exactly.
MovingAverage is the \(P =
0\) special case of Savitzky-Golay (constant polynomial); it
maximises noise rejection but maximally distorts peaks.Fir with custom tap weights generalises both;
Savitzky-Golay computes those weights analytically from least-squares
polynomial fitting.PolynomialFitting (offline) performs the same
projection without the sliding-window structure.MedianFilter suppresses impulsive spikes rather than
Gaussian noise; the two are complementary.Bayesian Optimization (BO) is a gradient-free, sample-efficient strategy for optimizing black-box functions that are expensive to evaluate. Unlike gradient-based methods (gradient descent, Adam), BO does not require access to derivatives. It is ideal for calibrating numerical controllers, tuning hyperparameters, or any optimization task where each function evaluation is costly — for example, running a full closed-loop simulation.
BO maintains a probabilistic surrogate model (typically a Gaussian Process) of the objective function. At each iteration, it uses an acquisition function to select the next query point that balances exploration (uncertain regions) with exploitation (promising regions). After evaluating the true objective at that point, the surrogate is updated and the process repeats.
A Gaussian Process (GP) defines a distribution over functions:
\[f \sim \mathcal{GP}(m(\mathbf{x}), k(\mathbf{x}, \mathbf{x}'))\]
With zero mean \(m = 0\) and squared-exponential (RBF) kernel:
\[k(\mathbf{x}, \mathbf{x}') = \sigma_f^2 \exp\!\left(-\frac{\|\mathbf{x} - \mathbf{x}'\|^2}{2\ell^2}\right)\]
where \(\ell\) is the length-scale and \(\sigma_f^2\) is the signal variance.
Given \(n\) observations \(\{(\mathbf{x}_i, y_i)\}\), the GP posterior at a new point \(\mathbf{x}_*\) is Gaussian:
\[p(f_* | \mathbf{X}, \mathbf{y}, \mathbf{x}_*) = \mathcal{N}(\mu_*, \sigma_*^2)\]
\[\mu_* = \mathbf{k}_*^T (\mathbf{K} + \sigma_n^2 \mathbf{I})^{-1} \mathbf{y}\]
\[\sigma_*^2 = k_{**} - \mathbf{k}_*^T (\mathbf{K} + \sigma_n^2 \mathbf{I})^{-1} \mathbf{k}_*\]
where: - \(\mathbf{K}_{ij} = k(\mathbf{x}_i, \mathbf{x}_j)\) is the \(n \times n\) kernel matrix - \(\mathbf{k}_{*,i} = k(\mathbf{x}_*, \mathbf{x}_i)\) is the cross-kernel vector - \(\sigma_n^2\) is the observation noise variance
For minimization, the Expected Improvement (EI) at \(\mathbf{x}\) given best observed value \(f^* = \min_i y_i\) is:
\[\text{EI}(\mathbf{x}) = \mathbb{E}[\max(f^* - f(\mathbf{x}), 0)]\]
For a GP surrogate this evaluates analytically:
\[\text{EI}(\mathbf{x}) = (f^* - \mu_*)\,\Phi(z) + \sigma_*\,\phi(z), \quad z = \frac{f^* - \mu_*}{\sigma_*}\]
where \(\Phi\) is the standard normal CDF and \(\phi\) is the PDF.
To avoid dynamic memory allocation, the kernel matrix is pre-allocated at compile-time with maximum capacity \(M\). When only \(n < M\) observations are present, the inactive rows and columns are padded with an identity block, preserving positive-definiteness and allowing Gaussian elimination with the full \(M \times M\) system. The resulting alpha vector \(\boldsymbol{\alpha} = (\mathbf{K} + \sigma_n^2 \mathbf{I})^{-1} \mathbf{y}\) has zeros in the inactive entries.
| Case | Time per iteration | Space | Notes |
|---|---|---|---|
| Best | \(O(M^3)\) | \(O(M^2 + NP)\) | Dominated by GP solve |
| Average | \(O(M^3 + C \cdot P)\) | \(O(M^2 + NP)\) | \(C\) candidates, \(P\) parameters |
| Worst | \(O(M^3 + C \cdot P)\) | \(O(M^2 + NP)\) | Same as average |
Where \(M\) = maximum observations, \(C\) = candidate count, \(P\) = parameter count, \(N\) = current number of observations.
All memory is pre-allocated at compile-time on the stack — no dynamic allocation required.
Example: minimize \(f(x) = (x - 0.5)^2\) on \(x \in [0, 1]\).
An optimizer adjusts a model’s parameter vector \(\theta \in \mathbb{R}^P\) to minimize a loss function \(\mathcal{L}(\theta)\). The simplest and most fundamental strategy is gradient descent: repeatedly step in the direction of steepest descent:
\[\theta_{t+1} = \theta_t - \eta \, \nabla_\theta \mathcal{L}(\theta_t)\]
where \(\eta\) is the learning rate — the single most important hyper-parameter in neural network training.
This library provides a batch gradient descent optimizer with a fixed learning rate and maximum iteration count, suitable for small embedded models where training data fits in memory and simplicity is paramount.
The gradient \(\nabla_\theta \mathcal{L}\) is obtained via back-propagation. Each component \(\frac{\partial \mathcal{L}}{\partial \theta_i}\) tells how much the loss changes when \(\theta_i\) is perturbed by a small amount. The gradient points uphill; subtracting it moves downhill.
For a convex loss with Lipschitz-continuous gradient (\(L\)-smooth):
\[\mathcal{L}(\theta_T) - \mathcal{L}(\theta^*) \le \frac{\|\theta_0 - \theta^*\|^2}{2 \eta T}\]
provided \(\eta < \frac{1}{L}\). This gives a convergence rate of \(O(1/T)\).
For non-convex losses (typical in neural networks), gradient descent converges to a stationary point (\(\|\nabla \mathcal{L}\| \approx 0\)) which may be a local minimum or saddle point — not necessarily the global minimum.
| \(\eta\) | Behavior |
|---|---|
| Too small (\(\eta \ll 1/L\)) | Slow convergence, many iterations wasted |
| Optimal (\(\eta \approx 1/L\)) | Fastest reliable convergence |
| Too large (\(\eta > 2/L\)) | Oscillation and divergence |
graph LR
subgraph "Learning Rate Effect"
A["η too small<br/>crawls to minimum"]
B["η just right<br/>smooth convergence"]
C["η too large<br/>oscillates / diverges"]
end| Operation | Time | Space |
|---|---|---|
| One gradient evaluation | \(O(P)\) per parameter | \(O(P)\) gradient vector |
| One parameter update | \(O(P)\) | In-place |
| Total training (T iterations) | \(O(T \cdot P)\) | \(O(P)\) working memory |
Memory overhead is minimal: only the current parameter vector, the gradient vector, and the optimizer result. No momentum buffers or second-moment estimates.
Setup: 2 parameters, \(\theta_0 = [2.0, -1.0]^T\), \(\eta = 0.1\), loss \(\mathcal{L}(\theta) = \theta_1^2 + \theta_2^2\) (quadratic bowl).
Iteration 1:
| Step | Computation | Result |
|---|---|---|
| Gradient | \(\nabla \mathcal{L} = [2\theta_1, 2\theta_2]\) | \([4.0, -2.0]\) |
| Update | \(\theta_1 = \theta_0 - 0.1 \cdot \nabla\mathcal{L}\) | \([1.6, -0.8]\) |
| Loss | \(\mathcal{L}(\theta_1) = 1.6^2 + 0.8^2\) | \(3.20\) |
Iteration 2:
| Step | Computation | Result |
|---|---|---|
| Gradient | \([3.2, -1.6]\) | |
| Update | \([1.28, -0.64]\) | |
| Loss | \(2.048\) |
Pattern: Each iteration multiplies the loss by \((1 - 2\eta)^2 = 0.64\). After 20 iterations, \(\mathcal{L} \approx 0.0003\).
Convergence trace:
graph LR
I0["θ₀ = (2, -1)<br/>ℒ = 5.0"] --> I1["θ₁ = (1.6, -0.8)<br/>ℒ = 3.2"]
I1 --> I2["θ₂ = (1.28, -0.64)<br/>ℒ = 2.05"]
I2 --> Idots["⋯"]
Idots --> I20["θ₂₀ ≈ (0.01, -0.005)<br/>ℒ ≈ 0.0003"]maxIterations — always check
result.finalCost to assess convergence quality.| Variant | Key Difference |
|---|---|
| SGD (Stochastic Gradient Descent) | Uses a random mini-batch per iteration; noise helps escape local minima |
| SGD with Momentum | Accumulates a velocity: \(v_{t+1} = \mu v_t - \eta \nabla\mathcal{L}\); smooths out oscillations |
| Nesterov Momentum | Evaluates gradient at the lookahead position \(\theta + \mu v\); faster convergence |
| AdaGrad | Per-parameter adaptive learning rate based on accumulated squared gradients |
| RMSProp | Exponentially decaying average of squared gradients; handles non-stationary objectives |
| Adam | Combines momentum and RMSProp with bias correction; most popular adaptive method |
graph TD
Opt["Optimizer<br/>(Gradient Descent)"]
Loss["Loss Functions"]
Model["Model"]
Reg["Regularization"]
LR["Linear Regression"]
Loss -->|"∇ℒ"| Opt
Model -->|"θ, ∇θ"| Opt
Reg -->|"penalty gradient"| Loss
Opt -.->|"analytical solution at η→∞, 1 step"| LR| Component | Relationship |
|---|---|
| Loss Functions | Provides the Cost() and Gradient() the
optimizer calls each iteration |
| Model | Passes initial parameters to the optimizer and receives optimized parameters back |
| Regularization | Adds a penalty gradient to \(\nabla\mathcal{L}\), biasing the optimizer toward simpler models |
| Linear Regression | For MSE on a linear model, gradient descent converges to the same solution as the normal equation |
On-device neural network training and online system identification require an optimiser that accepts one externally computed gradient per call and maintains its own state between calls. Unlike batch optimisers that own the full objective, a step optimiser separates the gradient source from the update rule, enabling training loops where back-propagation runs elsewhere.
Two industry-standard rules are provided: Stochastic Gradient Descent (SGD) with optional momentum or Nesterov lookahead, and the Adam (Adaptive Moment Estimation) optimiser with bias correction.
Vanilla SGD applies the raw gradient:
\[\theta_{t+1} = \theta_t - \eta \, g_t\]
where \(g_t = \nabla_\theta \mathcal{L}(\theta_t)\) and \(\eta\) is the learning rate.
Momentum accumulates a velocity \(v\), smoothing oscillations and accelerating progress in low-curvature directions:
\[v_{t+1} = \beta \, v_t + g_t, \qquad \theta_{t+1} = \theta_t - \eta \, v_{t+1}\]
with \(\beta \in [0, 1)\) the momentum coefficient (typically \(0.9\)).
Nesterov momentum incorporates a lookahead correction, replacing the plain velocity step with:
\[\theta_{t+1} = \theta_t - \eta \, \bigl(g_t + \beta \, v_{t+1}\bigr)\]
This form evaluates the effective update at the anticipated next position, yielding faster convergence on smooth convex objectives.
Adam maintains exponential moving averages of the gradient (first moment \(m\)) and the squared gradient (second moment \(v\)):
\[m_{t+1} = \beta_1 \, m_t + (1 - \beta_1) \, g_t\]
\[v_{t+1} = \beta_2 \, v_t + (1 - \beta_2) \, g_t^2 \quad (\text{element-wise})\]
Both estimates are biased toward zero at initialisation. Bias correction removes this bias:
\[\hat{m}_{t+1} = \frac{m_{t+1}}{1 - \beta_1^{t+1}}, \qquad \hat{v}_{t+1} = \frac{v_{t+1}}{1 - \beta_2^{t+1}}\]
The parameter update normalises the corrected first moment by the square root of the corrected second moment, providing a per-parameter adaptive step size:
\[\theta_{t+1} = \theta_t - \eta \, \frac{\hat{m}_{t+1}}{\sqrt{\hat{v}_{t+1}} + \varepsilon}\]
Typical hyper-parameters: \(\eta = 10^{-3}\), \(\beta_1 = 0.9\), \(\beta_2 = 0.999\), \(\varepsilon = 10^{-8}\).
| Algorithm | Time per step | Extra state (floats) | Notes |
|---|---|---|---|
| SGD | \(O(N)\) | \(N\) | One velocity vector (equals the gradient when momentum is 0) |
| Adam | \(O(N)\) | \(2N + 2\) | Two moment vectors plus the running powers \(\beta_1^t\), \(\beta_2^t\) |
\(N\) is the number of parameters. All operations are in-place; no heap allocation is required.
SGD with momentum on \(\mathcal{L}(\theta) = \frac{1}{2}\|\theta\|^2\), \(N=1\), \(\theta_0 = 1\), \(\eta = 0.1\), \(\beta = 0.9\):
| \(t\) | \(g_t = \theta_t\) | \(v_t = 0.9 v_{t-1} + g_t\) | \(\theta_{t+1} = \theta_t - 0.1 v_t\) |
|---|---|---|---|
| 1 | 1.000 | 1.000 | 0.900 |
| 2 | 0.900 | 1.800 | 0.720 |
| 3 | 0.720 | 2.340 | 0.486 |
Adam on the same objective, \(\beta_1 = 0.9\), \(\beta_2 = 0.999\), \(\varepsilon = 10^{-8}\), \(\theta_0 = 0\), \(g_1 = 1\):
| Quantity | Value |
|---|---|
| \(m_1\) | \(0.1\) |
| \(v_1\) | \(0.001\) |
| \(\hat{m}_1\) | \(1.0\) |
| \(\hat{v}_1\) | \(1.0\) |
| \(\theta_1\) | \(-\eta \approx -0.001\) |
Bias correction is the critical step: without it, \(m_1/\sqrt{v_1} \approx 3.16\), giving a first step roughly \(\sqrt{1000}\) times larger than the bias-corrected value.
float.Reset() is
called, the velocity (SGD) or moment estimates (Adam) return to zero.
The first step after a reset behaves identically to starting from
scratch, so the bias-correction denominator for Adam also restarts from
\(t=1\).| Variant | Change from base |
|---|---|
| AdaGrad | Non-decaying sum of squared gradients (no \(\beta_2\) decay); aggressive learning rate shrinkage |
| RMSProp | Adam without first-moment tracking; lacks bias correction |
| AdamW | Decoupled weight-decay applied directly to \(\theta\) before the gradient step |
| AMSGrad | Replaces \(\hat{v}\) with the running maximum to guarantee monotone effective step-size |
| Component | Relationship |
|---|---|
| Gradient Descent | Batch counterpart; shares the learning-rate update rule but recomputes the objective each time |
| LMS Adaptive Filter | Equivalent to online SGD for a linear regression model under MSE loss |
| Regularization | Adds a penalty gradient to \(g_t\); compatible with any step optimiser |
Inverse kinematics, forward kinematics of parallel robots, sensor and hand-eye calibration and scan matching all ask the same question: which state makes a set of nonlinear residuals as small as possible? The problems are small, solved every control cycle or at commissioning, and must finish within a hard iteration budget without heap allocation.
Levenberg–Marquardt answers it by repeated linearization. Near the current guess the residuals behave like a linear function of a small correction, and Gauss–Newton solves that linear problem exactly, converging quadratically when a zero residual is reachable. Far from the solution, or near a singular Jacobian, the exact step can be huge or undefined. A damping term penalizes large corrections and trades speed for robustness.
The state need not be a plain vector: an angle wraps, an orientation lives on a manifold, a joint stops at a limit. The solver therefore never adds the correction itself. The problem applies it through a retraction, so the state never leaves its valid set. One careful solver replaces a hand-rolled Newton loop in every module.
\[ \min_{x}\; \tfrac12\,\|r(x)\|^2, \qquad r(x) \in \mathbb{R}^{R} \]
The state \(x\) may lie on a manifold or within bounds and has \(P\) local degrees of freedom. The problem supplies three things:
A damping policy supplies \(\lambda^2 \ge 0\) from the current Jacobian. A constant policy is classic Levenberg–Marquardt, and \(\lambda = 0\) is Gauss–Newton. A problem-aware schedule, such as zero away from singularities, plugs in through the same interface.
Linearizing \(r(x \oplus \delta) \approx r + J\delta\) and penalizing the correction gives
\[ \min_{\delta}\; \tfrac12\|r + J\delta\|^2 + \tfrac12\lambda^2\|\delta\|^2 \;\Longrightarrow\; (J^\top J + \lambda^2 I)\,\delta = -J^\top r . \]
The push-through identity \((J^\top J + \lambda^2 I)^{-1}J^\top = J^\top (JJ^\top + \lambda^2 I)^{-1}\) gives an equivalent form, so the solver always factors the smaller system:
| Shape | Condition | System | Step |
|---|---|---|---|
| Tall | \(R > P\) | \(P \times P\), parameter space | \(\delta = -(J^\top J + \lambda^2 I)^{-1}J^\top r\) |
| Wide or square | \(R \le P\) | \(R \times R\), residual space | \(\delta = -J^\top (JJ^\top + \lambda^2 I)^{-1} r\) |
For \(\lambda = 0\) and full row rank, the wide form is the minimum-norm solution of \(J\delta = -r\): the shortest correction that cancels the linearized residual. The system is solved by LU decomposition with partial pivoting, never by an explicit inverse. If the factorization finds a pivot below a relative \(10^{-6}\) (for example \(\lambda = 0\) with a rank-deficient \(J\)), the solve ends unconverged at the current state instead of dividing by zero. For \(\lambda^2 > 0\) the matrix is symmetric positive definite.
With tolerance \(\varepsilon\), optional step cap \(\Delta\) and iteration budget \(N\):
After \(N\) iterations the residual is recomputed at the returned state, which is converged only if it is within tolerance. The reported residual always belongs to the returned state.
The damped step vanishes exactly when \(J^\top r = 0\). For a tall problem this is a stationary point of the cost. For a wide problem of full row rank it forces \(r = 0\). Damping changes the path, not the destination: convergence drops from quadratic to linear, but a zero-residual fixed point stays a fixed point. A non-zero final residual therefore means an unreachable target, a singularity or an active bound.
| Item | Cost | Justification |
|---|---|---|
| Normal matrix | \(O(R\,P\,m)\) | \(J^\top J\) costs \(R P^2\), \(JJ^\top\) costs \(P R^2\); \(m = \min(R, P)\) |
| Factorization and solve | \(O(m^3)\) | LU of an \(m \times m\) matrix dominates the \(O(m^2)\) triangular solves |
| Whole solve | at most \(N\) iterations | each adds one residual and one Jacobian from the problem; \(N + 1\) residual evaluations in all, the last at the returned state |
| Memory | one \(R \times P\) Jacobian, one \(m \times m\) system | stack only; no heap, no recursion |
Rosenbrock residuals. \(r = (10(y - x^2),\; 1 - x)\), \(J = \begin{pmatrix} -20x & 10 \\ -1 & 0 \end{pmatrix}\), zero residual at \((1, 1)\). Start at \((-1.2, 1)\) with \(\lambda = 0\); \(R = P = 2\), so the step is solved in residual space.
| Iteration | State | \(r\) | \(J\) | Step \(\delta\) | New state |
|---|---|---|---|---|---|
| 1 | \((-1.2, 1)\) | \((-4.4, 2.2)\), \(\|r\| = 4.919\) | \(\begin{pmatrix} 24 & 10 \\ -1 & 0 \end{pmatrix}\) | \((2.2, -4.84)\) | \((1, -3.84)\) |
| 2 | \((1, -3.84)\) | \((-48.4, 0)\), \(\|r\| = 48.4\) | \(\begin{pmatrix} -20 & 10 \\ -1 & 0 \end{pmatrix}\) | \((0, 4.84)\) | \((1, 1)\) |
The second residual is linear in \(x\), so step 1 zeroes it exactly. The first is then linear in \(y\), so step 2 finishes exactly. The residual norm rises from \(4.9\) to \(48.4\) on the way: the iteration is not monotone. With a step cap \(\Delta = 1\), the first step of norm \(5.316540\) is scaled to unit length, giving \((-0.786197,\; 0.089634)\).
Angle on the unit circle. State \(\theta\), \(r = (\cos\theta, \sin\theta) - (\cos 2.5, \sin 2.5)\), \(J = (-\sin\theta,\; \cos\theta)^\top\). Here \(R = 2 > P = 1\) and \(J^\top J = 1\), so with \(\lambda = 0\) each step is \(\theta \leftarrow \theta - \sin(\theta - 2.5)\), wrapped into \((-\pi, \pi]\). Start at \(\theta = -2.5\), which is only \(1.28\) rad from the target across the \(\pm\pi\) seam.
| Iteration | \(\theta\) | \(\sin(\theta - 2.5)\) | Before wrap | After wrap |
|---|---|---|---|---|
| 1 | \(-2.500000\) | \(0.958924\) | \(-3.458924\) | \(2.824261\) |
| 2 | \(2.824261\) | \(0.318608\) | \(2.505653\) | \(2.505653\) |
| 3 | \(2.505653\) | \(0.005653\) | \(2.500000\) | \(2.500000\) |
The wrap keeps the state in its canonical range, so the answer is \(2.5\) and not \(2.5 - 2\pi\). The error falls \(0.324 \to 0.0057 \to 3\times10^{-8}\), faster than quadratic because of the circle’s symmetry.
Unreachable target. Target \((2, 0)\) lies outside the unit circle, with \(\lambda^2 = 1\). Then \(J^\top J + \lambda^2 = 2\) and \(\delta = -\tfrac12 J^\top r = -\sin\theta\), so \(\theta \leftarrow \theta - \sin\theta \to 0\). At \(\theta = 0\) the step vanishes (\(J^\top r = 0\)) while \(\|r\| = \|(1, 0) - (2, 0)\| = 1\). The solve exhausts its budget and reports not converged.
| Variant | Key difference |
|---|---|
| Gauss–Newton (\(\lambda = 0\)) | Quadratic convergence for zero-residual problems; fails on rank deficiency |
| Constant or problem-aware damping (this) | Fixed \(\lambda\) gives robust linear convergence; a Jacobian-dependent \(\lambda^2\) (zero away from singularities) keeps the fast rate where safe |
| Gain-ratio (Nielsen) adaptation | Raises or lowers \(\lambda\) from the ratio of actual to predicted cost reduction; needs a trial evaluation and accept/reject, a possible later variant |
| Algorithm | Relationship |
|---|---|
| LU Decomposition | Solves the damped normal system; its singularity report ends the solve unconverged |
| Damped Pseudo-Inverse | The same damped step in SVD form for one linear solve; avoids squaring the condition number at higher cost |
| SE(3) Transform | Supplies the manifold retraction for pose states |
| Angle Utilities | Supplies the wrapped retraction for angle states |
| Gradient Descent | First-order method for general costs; this method exploits the sum-of-squares structure through the Gauss–Newton curvature and needs far fewer iterations on small problems |
Regularization adds a penalty term \(\Omega(\theta)\) to the loss function that discourages overly complex models:
\[\mathcal{L}_{\text{total}} = \mathcal{L}(\hat{y}, y) + \lambda \, \Omega(\theta)\]
where \(\lambda > 0\) controls the strength of the penalty. Without regularization, a neural network with enough parameters can memorize the training data perfectly yet generalize poorly to new inputs — a phenomenon called overfitting. Regularization biases the optimizer toward simpler solutions that tend to generalize better.
\[\Omega_{L2}(\theta) = \frac{1}{2}\sum_{i=1}^{P} \theta_i^2 = \frac{1}{2}\|\theta\|_2^2\]
\[\frac{\partial \Omega_{L2}}{\partial \theta_i} = \theta_i\]
Effect on update rule:
\[\theta_{t+1} = \theta_t - \eta(\nabla_\theta \mathcal{L} + \lambda \theta_t) = (1 - \eta\lambda)\theta_t - \eta \nabla_\theta \mathcal{L}\]
The factor \((1 - \eta\lambda)\) shrinks weights toward zero each step — hence the name weight decay. Large weights are penalized quadratically, keeping the model smooth.
\[\Omega_{L1}(\theta) = \sum_{i=1}^{P} |\theta_i| = \|\theta\|_1\]
\[\frac{\partial \Omega_{L1}}{\partial \theta_i} = \mathrm{sign}(\theta_i)\]
Effect: L1 drives small weights exactly to zero, producing a sparse model. This is useful for feature selection — irrelevant connections are pruned automatically.
| Property | L1 | L2 |
|---|---|---|
| Penalty shape | Diamond (corners at axes) | Sphere |
| Sparsity | Promotes exact zeros | Shrinks toward zero but rarely reaches it |
| Gradient at \(\theta_i = 0\) | Undefined (sub-gradient) | Zero |
| Best for | Feature selection, sparse models | General-purpose weight control |
The regularized loss can be viewed as constrained optimization:
\[\min_\theta \mathcal{L}(\theta) \quad \text{subject to} \quad \Omega(\theta) \le c\]
where \(c\) is determined by \(\lambda\). L1 constrains \(\theta\) to a diamond, so the optimal point tends to lie at a corner (sparse). L2 constrains to a sphere, so the optimal point balances all dimensions.
| Operation | Time | Space |
|---|---|---|
| \(\Omega_{L2}(\theta)\) | \(O(P)\) | \(O(1)\) |
| \(\nabla \Omega_{L2}\) | \(O(P)\) | \(O(P)\) |
| \(\Omega_{L1}(\theta)\) | \(O(P)\) | \(O(1)\) |
| \(\nabla \Omega_{L1}\) | \(O(P)\) | \(O(P)\) |
Regularization adds negligible computational cost — one pass over the parameter vector per training iteration.
Scenario: 3 parameters, \(\theta = [0.8, -0.3, 0.5]\), \(\lambda = 0.1\).
L2 Regularization:
| Step | Computation | Result |
|---|---|---|
| Penalty | \(\frac{1}{2}(0.64 + 0.09 + 0.25) = 0.49\) | \(\Omega_{L2} = 0.49\) |
| Gradient | \([0.8, -0.3, 0.5]\) | Added to \(\nabla\mathcal{L}\) |
| Contribution to loss | \(0.1 \times 0.49 = 0.049\) |
L1 Regularization:
| Step | Computation | Result |
|---|---|---|
| Penalty | \(0.8 + 0.3 + 0.5 = 1.6\) | \(\Omega_{L1} = 1.6\) |
| Gradient | \([1, -1, 1]\) | Added to \(\nabla\mathcal{L}\) |
| Contribution to loss | \(0.1 \times 1.6 = 0.16\) |
After several L1 updates (\(\eta = 0.1\), \(\lambda = 0.1\)): the \(\theta_2 = -0.3\) component, already small, is driven to exactly zero. The model effectively prunes that connection.
| Variant | Key Difference |
|---|---|
| Elastic Net | \(\Omega = \alpha \|\theta\|_1 + (1-\alpha)\|\theta\|_2^2\); combines L1 sparsity with L2 smoothness |
| Dropout | Randomly zeroes activations during training; implicit ensemble regularization |
| Early stopping | Halts training before overfitting; regularization without modifying the loss |
| Data augmentation | Expands the training set with transformed copies; reduces overfitting by increasing data diversity |
| Spectral normalization | Constrains the spectral norm of weight matrices; stabilizes GAN training |
| Weight clipping | Hard constraint: \(\left\|\theta_i\right\| \le c\); used in Wasserstein GANs |
graph TD
Reg["Regularization"]
Loss["Loss Functions"]
Opt["Optimizer"]
Model["Model"]
LR["Linear Regression"]
Reg -->|"λ Ω(θ) added to ℒ"| Loss
Loss --> Opt
Opt --> Model
Reg -.->|"L2 + MSE = Ridge regression"| LR| Component | Relationship |
|---|---|
| Loss Functions | Regularization is a penalty added to the loss: \(\mathcal{L}_{\text{total}} = \mathcal{L} + \lambda\Omega\) |
| Optimizer | Receives the combined gradient \(\nabla\mathcal{L} + \lambda\nabla\Omega\) |
| Linear Regression | L2-regularized MSE with a linear model is Ridge regression; L1 is Lasso |
Trigonometric functions, magnitudes, and vector rotations are
fundamental operations in motor control, radar processing, and
navigation. On microcontrollers without a floating-point unit or
hardware multiplier, library implementations of sin,
cos, atan2, and hypot require
expensive software-emulated multiplications. This limits their use in
hard real-time loops where deterministic execution time is
mandatory.
CORDIC (COordinate Rotation DIgital Computer) solves this by expressing any planar rotation as a sum of progressively smaller elementary rotations, each of which requires only a bit-shift and an addition. The result is a trig engine that runs on shift-add hardware with a fixed, data-independent cycle count — exactly what a deterministic real-time loop demands.
A rotation by angle \(\theta\) in two dimensions transforms a vector \((x, y)\) to
\[x' = x\cos\theta - y\sin\theta, \quad y' = y\cos\theta + x\sin\theta.\]
Factoring out \(\cos\theta\) gives
\[x' = \cos\theta\,(x - y\tan\theta), \quad y' = \cos\theta\,(y + x\tan\theta).\]
When \(\tan\theta_i = \pm 2^{-i}\), the multiplication by \(\tan\theta_i\) becomes a right-shift by \(i\) bits. The angles \(\theta_i = \arctan(2^{-i})\) form the CORDIC angle table.
Starting from \((x_0, y_0, z_0) = (K, 0, \theta)\), each iteration steers the residual angle \(z\) toward zero:
\[x_{i+1} = x_i - \sigma_i \, 2^{-i} y_i\] \[y_{i+1} = y_i + \sigma_i \, 2^{-i} x_i\] \[z_{i+1} = z_i - \sigma_i \, \theta_i\]
where \(\sigma_i = \text{sign}(z_i)\). After \(N\) iterations, \(x_N \approx \cos\theta\) and \(y_N \approx \sin\theta\).
Starting from \((x_0, y_0, z_0) = (x, y, 0)\), each iteration steers \(y\) toward zero:
\[\sigma_i = -\text{sign}(y_i)\]
After \(N\) iterations, \(z_N \approx \arctan(y/x)\) and \(x_N \approx \|(x, y)\| / K\).
Each elementary rotation stretches the vector length by \(\sqrt{1 + 2^{-2i}}\). The accumulated gain over \(N\) iterations is
\[A_N = \prod_{i=0}^{N-1} \sqrt{1 + 2^{-2i}}.\]
The constant \(K = 1/A_N \approx 0.6073\) compensates for this growth. In rotation mode the initial \(x\) is pre-scaled by \(K\); in vectoring mode the final \(x\) is multiplied by \(K\).
The convergence domain is \(|z| \leq \sum_{i=0}^{N-1} \arctan(2^{-i})\). For \(N = 16\) this exceeds \(\pi/2\), so inputs outside \([-\pi/2, \pi/2]\) must be range-reduced by shifting the angle by \(\pm\pi\) and inverting the output signs. Vectoring mode uses quadrant detection on the signs of \(x\) and \(y\) to handle the full \([-\pi, \pi]\) range.
| Metric | Value |
|---|---|
| Time | \(O(N)\) — exactly \(N\) shift-add steps per call |
| Space | \(O(N)\) — angle table in ROM; \(O(1)\) working registers |
| Cycle count | Fixed, data-independent — no branch on input value |
One additional bit of precision is gained per iteration. \(N = 16\) yields approximately 16-bit accuracy; \(N = 20\) reaches the limits of single-precision float.
Compute \(\sin(\pi/6) = 0.5\) with \(N = 4\) for brevity (gain \(K_4 \approx 0.6352\)).
| \(i\) | \(\theta_i\) | \(\sigma_i\) | \(x_i\) | \(y_i\) | \(z_i\) |
|---|---|---|---|---|---|
| — | — | — | 0.6352 | 0.0000 | 0.5236 |
| 0 | 0.7854 | +1 | 0.6352 | 0.6352 | −0.2618 |
| 1 | 0.4636 | −1 | 0.7940 | 0.3176 | 0.2018 |
| 2 | 0.2450 | +1 | 0.7147 | 0.5122 | −0.0432 |
| 3 | 0.1244 | −1 | 0.8425 | 0.4248 | 0.0812 |
After iteration 3: \(y_4 \approx 0.43\), improving toward 0.5 as \(N\) grows.
The input to rotation mode must lie within the convergence domain after range reduction. Angles at exactly \(\pm\pi/2\) sit at the edge of the domain and may accumulate an extra half-ulp error.
In vectoring mode, \((x, y) = (0, 0)\) is degenerate; by convention the angle is returned as zero rather than causing a division-by-zero or NaN.
The shift \(2^{-i}\) eventually underflows in floating-point for large \(i\); iterations beyond \(\lfloor -\log_2(\epsilon) \rfloor\) contribute nothing and can be capped without loss of accuracy.
Hyperbolic CORDIC replaces the elementary angle
table with \(\tanh^{-1}(2^{-i})\) and
handles sinh, cosh, exp, and
ln.
Linear CORDIC uses shifts alone (no rotation) to implement multiply and divide.
Double-rotation trick repeats certain iterations to extend the convergence domain to \((-\pi, \pi]\) without a range-reduction step.
sin/cos at carrier frequency.atan2 for heading on
heading-constrained MCUs.TrigonometricFunctions provides a table-lookup
alternative with lower iteration count but higher ROM usage for the same
precision. Quaternion consumes CORDIC-generated
sin/cos for axis-angle conversions. On targets
with an FPU the standard library usually outperforms CORDIC; the
advantage is exclusive to multiply-poor hardware.
Three-dimensional attitude representation is a fundamental requirement in robotics, aerospace, and wearable sensing. Euler angles are intuitive but suffer from gimbal lock — a singularity that collapses three degrees of freedom into two whenever one angle reaches ±90°. Rotation matrices avoid this but carry nine words of state and require orthogonality re-enforcement.
A unit quaternion encodes the same rotation in four words, composes orientations with sixteen multiply-adds, and is free of singularities. Every modern AHRS filter — Madgwick, Mahony, Extended Kalman — stores attitude as a unit quaternion precisely because of this combination of compactness, numerical stability, and algebraic closure.
A quaternion is a hypercomplex number of the form
\[q = w + x\mathbf{i} + y\mathbf{j} + z\mathbf{k}\]
where \(w, x, y, z \in \mathbb{R}\) and the basis elements satisfy
\[\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{ijk} = -1.\]
A unit quaternion (\(\|q\| = 1\)) encodes a rotation by angle \(\theta\) about unit axis \(\hat{n}\) as
\[q = \left(\cos\frac{\theta}{2},\; \hat{n}\sin\frac{\theta}{2}\right).\]
Composition of two rotations \(q_a\) then \(q_b\) is
\[q_a \otimes q_b = \begin{pmatrix} w_a w_b - x_a x_b - y_a y_b - z_a z_b \\ w_a x_b + x_a w_b + y_a z_b - z_a y_b \\ w_a y_b - x_a z_b + y_a w_b + z_a x_b \\ w_a z_b + x_a y_b - y_a x_b + z_a w_b \end{pmatrix}.\]
This product is non-commutative: \(q_a \otimes q_b \neq q_b \otimes q_a\) in general.
A pure quaternion \(p = (0, \mathbf{v})\) is rotated by
\[\mathbf{v}' = q \otimes p \otimes q^{-1}.\]
For unit \(q\) this simplifies (Rodrigues cross-product form) to
\[\mathbf{v}' = \mathbf{v} + 2w\,(\mathbf{u} \times \mathbf{v}) + 2\,\mathbf{u} \times (\mathbf{u} \times \mathbf{v}),\]
where \(\mathbf{u} = (x, y, z)\). This costs 15 multiply-adds vs 9 for a pre-built rotation matrix, making it preferable when rotating one vector.
For any quaternion \(q^* = (w, -x, -y, -z)\). For a unit quaternion \(q^{-1} = q^*\).
\[R(q) = \begin{pmatrix} 1-2(y^2+z^2) & 2(xy-wz) & 2(xz+wy) \\ 2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx) \\ 2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) \end{pmatrix}.\]
Converting from unit quaternion to roll \(\phi\), pitch \(\theta\), yaw \(\psi\):
\[\phi = \mathrm{atan2}(2(wx+yz),\; 1-2(x^2+y^2))\] \[\theta = \arcsin(2(wy-zx))\] \[\psi = \mathrm{atan2}(2(wz+xy),\; 1-2(y^2+z^2))\]
At \(\theta = \pm 90°\) the \(\phi\) and \(\psi\) axes align (gimbal lock); the formula still returns a bounded value but the decomposition is no longer unique.
Spherical Linear Interpolation between unit quaternions \(q_0\) and \(q_1\) at fraction \(t \in [0,1]\):
\[\mathrm{Slerp}(q_0, q_1, t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}\,q_0 + \frac{\sin(t\Omega)}{\sin\Omega}\,q_1,\]
where \(\cos\Omega = q_0 \cdot q_1\). When \(\Omega \approx 0\) (nearly parallel quaternions) the formula degenerates; a normalized linear interpolation (nlerp) is substituted.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Hamilton product | O(1) | O(1) | 16 multiply-adds, scalar only |
| Vector rotate | O(1) | O(1) | 15 multiply-adds via cross-product |
| To rotation matrix | O(1) | O(1) | 9 elements, 16 multiplications |
| From rotation matrix | O(1) | O(1) | Branch on largest diagonal |
| SLERP | O(1) | O(1) | 1 acos + 2 sin + scalar blends |
| Euler conversion | O(1) | O(1) | 2 atan2 + 1 asin |
All operations are stack-only with no heap allocation.
Rotating \(\hat{x} = (1,0,0)\) by 90° about \(\hat{z}\):
Geometry3D (RotationAboutAxis,
CrossProduct) — provides the rotation matrix and vector
primitives reused by quaternion conversions.operator* / Normalize on
every sample.acos/sin for fixed-point axis-angle
conversions on cores without an FPU.Linear algebra computations — solvers, Kalman filter covariance updates, regression — depend on the numerical health of the matrices involved. Norms formalise the notion of “size” for matrices and vectors and are the building blocks of every conditioning, stability, and error-bound estimate in the library. They are also the cheapest such quantities to compute: a single pass over the entries with no allocation, suitable for real-time embedded paths.
Vector normalisation, closely related, produces the unit-length direction of a vector and is a recurring primitive in geometry, attitude estimation, and gradient methods.
For vectors \(\mathbf{a}, \mathbf{b} \in \mathbb{R}^n\), the dot product is \(\mathbf{a}\cdot\mathbf{b} = \sum_{i=1}^{n} a_i b_i\). The Euclidean (L2) norm is the square root of the self dot product:
\[\|\mathbf{v}\|_2 = \sqrt{\mathbf{v}\cdot\mathbf{v}} = \sqrt{\sum_{i=1}^{n} v_i^2}\]
VectorNorm is implemented in terms of
DotProduct. The unit vector \(\hat{\mathbf{v}} = \mathbf{v} /
\|\mathbf{v}\|_2\) satisfies \(\|\hat{\mathbf{v}}\|_2 = 1\). Normalisation
is undefined when \(\|\mathbf{v}\|_2 =
0\) and must be guarded.
Frobenius norm — treats the matrix as a flattened vector:
\[\|A\|_F = \sqrt{\sum_{i=1}^{m}\sum_{j=1}^{n} a_{ij}^2}\]
It is rotationally invariant under unitary transformations and cheap to compute.
1-norm (maximum absolute column sum):
\[\|A\|_1 = \max_{1 \le j \le n} \sum_{i=1}^{m} |a_{ij}|\]
Infinity norm (maximum absolute row sum):
\[\|A\|_\infty = \max_{1 \le i \le m} \sum_{j=1}^{n} |a_{ij}|\]
The 1-norm and infinity-norm are dual: \(\|A\|_\infty = \|A^\top\|_1\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| DotProduct | \(O(n)\) | \(O(1)\) | Single pass; VectorNorm builds on it |
| FrobeniusNorm | \(O(mn)\) | \(O(1)\) | Single pass, no allocation |
| OneNorm | \(O(mn)\) | \(O(1)\) | Column-wise sum, running max |
| InfinityNorm | \(O(mn)\) | \(O(1)\) | Row-wise sum, running max |
| VectorNorm | \(O(n)\) | \(O(1)\) | Single pass |
| Normalize | \(O(n)\) | \(O(n)\) | Output vector on stack |
Matrix \(A = \begin{bmatrix}3 & 1 \\ 1 & 2\end{bmatrix}\):
Vector \(\mathbf{v} = [3,\, 4]^\top\): \(\|\mathbf{v}\|_2 = 5\), and \(\hat{\mathbf{v}} = [0.6,\, 0.8]^\top\).
Zero vector normalisation — dividing by \(\|\mathbf{v}\|_2 = 0\) is undefined. The implementation returns an empty optional for near-zero norms.
Fast-math semantics — -ffast-math may
reorder floating-point operations. The norms are sums of non-negative
values, so reordering does not change the sign of the result, but
catastrophic cancellation can still occur for near-zero off-diagonal
entries.
Non-square matrices — FrobeniusNorm, OneNorm, and InfinityNorm apply to any \(m \times n\) matrix.
The spectral norm (largest singular value, \(\|A\|_2\)) is the tightest but requires an SVD — \(O(N^3)\) with a large constant, unsuitable for embedded real-time paths. The 1-norm and infinity-norm are cheap upper bounds used throughout the library instead.
General p-norms and weighted norms generalise the vector case; the L2 norm is the only one currently exposed because it is the natural quantity for geometric and least-squares work.
solvers::ConditionNumber).The 1-norm is the norm used by solvers::ConditionNumber
for its \(\|A\|\cdot\|A^{-1}\|\)
estimate. The math::Matrix type provides the storage and
transpose the norms operate on. Norm-based residual tests appear in
solvers and optimization.
A Householder reflector is the workhorse of numerically stable dense
linear algebra. A single reflector mirrors a vector onto a coordinate
axis, zeroing every entry below a chosen pivot in one orthogonal step.
Chaining reflectors triangularizes a matrix (QR), bidiagonalizes it
(SVD), or tridiagonalizes a symmetric matrix (eigensolvers).
HouseholderVector computes the reflector for one
sub-column; it is the shared primitive those factorizations call.
For a vector \(x\), the Householder reflector is the orthogonal matrix
\[H = I - \beta\, v v^\top\]
chosen so that \(Hx\) is zero below the pivot. With \(\sigma = \sum_{i>\text{start}} x_i^2\) and \(\|x\| = \sqrt{x_{\text{start}}^2 + \sigma}\), the reflector maps \(x_{\text{start}} \mapsto \mp\|x\|\). The pivot sign is chosen as \(-\mathrm{sign}(x_{\text{start}})\|x\|\) to avoid cancellation:
\[v_{\text{start}} = 1, \quad v_i = x_i / v_0, \quad \beta = \frac{2 v_0^2}{\sigma + v_0^2}\]
\(H\) is symmetric and orthogonal (\(H = H^\top = H^{-1}\)), so applying it is backward stable.
| Operation | Time | Space | Notes |
|---|---|---|---|
| HouseholderVector | \(O(n)\) | \(O(1)\) | Builds v (stored implicit unit pivot) |
| Apply \(H\) to a vector | \(O(n)\) | \(O(1)\) | x - β·v·(vᵀx) — never form H |
For \(x = [4, 3, 0, 0]^\top\), pivot
start = 0: \(\sigma = 9\),
\(\|x\| = 5\). Since \(x_0 > 0\), \(v_0 = -\sigma/(x_0 + \|x\|) = -1\), giving
\(\beta = 1\) and \(v = [1, -3, 0, 0]^\top\). Reflecting yields
\(Hx = [-5, 0, 0, 0]^\top\) — the
sub-column collapsed onto the axis.
Already-zero sub-column — when \(\sigma \approx 0\) there is nothing to zero; the routine returns \(\beta = 0\) (identity reflector) and callers skip the update.
Never form H explicitly — apply it as
x − β·v·(vᵀx) to keep the cost \(O(n)\) per column instead of \(O(n^2)\).
Float-only —
static_assert(std::is_floating_point_v<T>); the
sign-fixing and normalisation assume real floating-point arithmetic.
Givens rotations (math::GivensRotation) achieve the same
zeroing one entry at a time, preferable for sparse or streaming updates.
Complex Householder reflectors extend this to unitary
triangularization.
A.Used by solvers::QrDecomposition; complements
math::GivensRotation and
math::SolveUpperTriangular. Operates on
math::Vector.
A Givens rotation zeros a single matrix entry by rotating two rows in
a plane, leaving all others untouched. Because it touches only two rows,
it is the tool of choice for incremental linear algebra:
streaming a new row into an existing QR factor, sparse
triangularization, and the implicit-shift sweeps of QR/SVD
eigen-iterations. ComputeGivens derives the rotation
coefficients; ApplyGivens applies them to a scalar
pair.
Given two scalars \(a\) (the value to keep) and \(b\) (the value to zero), the rotation
\[G = \begin{bmatrix} c & s \\ -s & c \end{bmatrix}, \quad c = \frac{a}{r},\; s = \frac{b}{r},\; r = \sqrt{a^2 + b^2}\]
is orthogonal (\(c^2 + s^2 = 1\)) and satisfies
\[G \begin{bmatrix} a \\ b \end{bmatrix} = \begin{bmatrix} r \\ 0 \end{bmatrix}\]
Applied across a pair of rows, it zeros the target entry while preserving Euclidean length.
| Operation | Time | Space | Notes |
|---|---|---|---|
| ComputeGivens | \(O(1)\) | \(O(1)\) | One sqrt, two divides |
| ApplyGivens | \(O(1)\) | \(O(1)\) | Rotates one scalar pair |
| Rotate two rows | \(O(n)\) | \(O(1)\) | ApplyGivens across n columns |
ComputeGivens(3, 4): \(r =
5\), \(c = 0.6\), \(s = 0.8\). Applying to \((x, y) = (3, 4)\): \(x' = 0.6\cdot 3 + 0.8\cdot 4 = 5\),
\(y' = -0.8\cdot 3 + 0.6\cdot 4 =
0\) — the second component is annihilated and the norm is
preserved.
Degenerate pair — when \(a
= b = 0\) the rotation is undefined; ComputeGivens
returns the identity \((c, s) = (1,
0)\) so applying it is a safe no-op.
Float-only —
static_assert(std::is_floating_point_v<T>).
Householder reflectors (math::HouseholderTransform) zero
an entire sub-column at once and are cheaper for dense factorization;
Givens wins when only one entry (or one streamed row) changes.
Fast/“square-root-free” Givens variants trade the sqrt for
extra bookkeeping.
R
(QrDecomposition::GivensUpdateRow).Used by solvers::QrDecomposition; complements
math::HouseholderTransform.
Nearly every dense linear solver ends with a triangular system.
Gaussian elimination reduces Ax = b to an upper-triangular
Ux = c; QR factorization solves least squares through
Rx = Qᵀb; Cholesky solves two triangular systems
back-to-back. SolveUpperTriangular is the shared
back-substitution kernel these routines call so the algorithm lives in
exactly one place.
Given an upper-triangular matrix \(R \in \mathbb{R}^{n \times n}\) (entries below the diagonal ignored) and a right-hand side \(c\), back-substitution solves \(Rx = c\) bottom-up:
\[x_i = \frac{1}{r_{ii}}\left(c_i - \sum_{j=i+1}^{n} r_{ij}\, x_j\right), \quad i = n, n-1, \dots, 1\]
Each unknown depends only on those already computed, so a single reverse sweep suffices.
| Operation | Time | Space | Notes |
|---|---|---|---|
| SolveUpperTriangular | \(O(n^2)\) | \(O(n)\) | One reverse sweep, stack output |
For \(R = \begin{bmatrix}2 & -1 & 3 \\ 0 & 4 & 1 \\ 0 & 0 & 5\end{bmatrix}\), \(c = [10,\, -5,\, 15]^\top\):
Singular / near-zero pivot — a zero diagonal entry
makes the system unsolvable. The routine asserts
|r_{ii}| > 0 via really_assert; callers
(GaussianElimination, QrDecomposition) detect
rank deficiency before reaching it.
Generic on T — the kernel is templated
on any supported numeric type (float, Q15,
Q31) because Gaussian elimination is instantiated for all
three; it uses math::ToFloat only for the pivot
assertion.
A lower-triangular forward-substitution is the mirror image (top-down sweep) and can be added when Cholesky/LU forward solves need it. Block triangular solves generalise this to matrix right-hand sides.
Rx = Qᵀb.Shared by solvers::GaussianElimination and
solvers::QrDecomposition. Operates on
math::Matrix / math::Vector.
The Cholesky decomposition factors a symmetric positive-definite matrix \(A\) into the product \(A = LL^T\), where \(L\) is a lower triangular matrix with positive diagonal entries. This is the matrix equivalent of taking a “square root.”
It is approximately twice as fast as LU decomposition for symmetric positive-definite systems and is numerically stable without pivoting. In this library, it is used primarily by the Unscented Kalman Filter to generate sigma points from the state covariance matrix.
Given \(A \in \mathbb{R}^{n \times n}\) symmetric positive-definite, compute \(L\) such that \(A = LL^T\):
\[L_{jj} = \sqrt{A_{jj} - \sum_{k=0}^{j-1} L_{jk}^2}\]
\[L_{ij} = \frac{1}{L_{jj}} \left( A_{ij} - \sum_{k=0}^{j-1} L_{ik} L_{jk} \right), \quad i > j\]
| Operation | Time | Space | Notes |
|---|---|---|---|
| Decomposition | \(O(n^3/6)\) | \(O(n^2)\) | Half the cost of general LU decomposition |
Input: \(A = \begin{bmatrix} 4 & 2 \\ 2 & 5 \end{bmatrix}\) (symmetric positive-definite).
Compute \(L\) column by column:
| Step | Formula | Value |
|---|---|---|
| \(L_{00}\) | \(\sqrt{A_{00}} = \sqrt{4}\) | \(2\) |
| \(L_{10}\) | \(A_{10} / L_{00} = 2 / 2\) | \(1\) |
| \(L_{11}\) | \(\sqrt{A_{11} - L_{10}^2} = \sqrt{5 - 1}\) | \(2\) |
Result:
\[L = \begin{bmatrix} 2 & 0 \\ 1 & 2 \end{bmatrix}\]
Verify: \(LL^T = \begin{bmatrix} 2 & 0 \\ 1 & 2 \end{bmatrix} \begin{bmatrix} 2 & 1 \\ 0 & 2 \end{bmatrix} = \begin{bmatrix} 4 & 2 \\ 2 & 5 \end{bmatrix} = A\) ✓
sqrt is expensive or imprecise.| Algorithm | Relationship |
|---|---|
| Unscented Kalman Filter | Uses Factor to generate sigma points from the
covariance matrix |
| Consistency Metrics | Uses Solve for the SPD covariance solve behind
NEES/NIS |
| Gaussian Elimination | General-purpose alternative; does not exploit symmetry |
Small structural matrix utilities that are needed across the library but do not belong to any single algorithm:
Symmetrize enforces exact symmetry on
a matrix that should be symmetric in theory but drifts under
floating-point round-off.CongruenceTransform computes the
quadratic form A·M·Aᵀ — the single most common shape in
covariance-propagating code (Kalman predict F·P·Fᵀ,
innovation covariance H·P·Hᵀ, Joseph update
(I−KH)·P·(I−KH)ᵀ, etc.).Both are recurring needs in Kalman filters, EM parameter updates, and Riccati/Lyapunov solvers.
Any square matrix decomposes into a symmetric and a skew-symmetric part:
\[M = \underbrace{\tfrac{1}{2}(M + M^\top)}_{\text{symmetric}} + \underbrace{\tfrac{1}{2}(M - M^\top)}_{\text{skew}}\]
Symmetrize returns the symmetric part \(\tfrac{1}{2}(M + M^\top)\). It is the
orthogonal projection (in the Frobenius inner product) of \(M\) onto the subspace of symmetric
matrices, so it is the closest symmetric matrix to \(M\). Applying it to an already-symmetric
matrix is a no-op (idempotent).
If \(M\) is symmetric,
CongruenceTransform returns a symmetric result exactly (in
exact arithmetic): \((AMA^\top)^\top = A
M^\top A^\top = A M A^\top\). This makes it the natural building
block for propagating a covariance \(P\) through a linear map \(A\): \(P \mapsto
A P A^\top\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Symmetrize | \(O(n^2)\) | \(O(n^2)\) | One transpose, add, scale |
| CongruenceTransform | \(O(n^2 m + n m^2)\) | \(O(nm)\) | For \(A \in \mathbb{R}^{n\times m}\), two matrix products |
Symmetrize — for \(M = \begin{bmatrix}1 & 3 \\ -1 & 2\end{bmatrix}\): \(M^\top = \begin{bmatrix}1 & -1 \\ 3 & 2\end{bmatrix}\), so \(\tfrac{1}{2}(M + M^\top) = \begin{bmatrix}1 & 1 \\ 1 & 2\end{bmatrix}\) — off-diagonals averaged, diagonal unchanged.
CongruenceTransform — with \(A \in \mathbb{R}^{n\times m}\) and symmetric \(M \in \mathbb{R}^{m\times m}\), the result \(A M A^\top \in \mathbb{R}^{n\times n}\) is the covariance of \(A x\) when \(x\) has covariance \(M\).
Symmetrize is not a fix for indefiniteness — it
removes the skew part but does not make a matrix positive-definite;
covariance code typically also adds a small diagonal jitter
(+ εI) separately.
CongruenceTransform association — the implementation
evaluates (A·M)·Aᵀ, matching the left-associative
operator*; results are bit-identical to hand-written
A * M * A.Transpose(). Under fast-math the
symmetry of the output can still carry tiny round-off asymmetry — follow
with Symmetrize when exact symmetry is required.
Float-only — both are
static_assert(std::is_floating_point_v<T>).
The skew-symmetric part \(\tfrac{1}{2}(M -
M^\top)\) is Symmetrize’s companion. The transposed
congruence \(A^\top M A\) (used by
DiscreteAlgebraicRiccatiEquation) is obtained by passing
A.Transpose().
F·P·Fᵀ,
H·P·Hᵀ, Joseph form (I−KH)·P·(I−KH)ᵀ + K·R·Kᵀ
across the KF/EKF/UKF/smoother family.Q, R, P after asymmetric matrix
products (estimators::ExpectationMaximization).Operate on math::Matrix /
math::SquareMatrix. Consumed by the
filters::active Kalman family and
estimators::ExpectationMaximization;
CongruenceTransform pairs naturally with
Symmetrize for covariance-positivity hygiene.
Matrix and vector dimensions must be strictly positive; a zero-length parameter vector is not representable. Element access on an empty fixed-size array has no valid index, so allowing it would trade a compile-time error for silent undefined behaviour.
A component without trainable parameters (for example a pooling or pure activation stage) should report the absence of parameters explicitly — an optional parameter vector that is empty, or an interface that has no parameter accessor at all — rather than returning a zero-length vector.
When a control system or filter receives a step input, its output traces a transient trajectory before settling at the final value. Quantifying that trajectory with standardised scalar metrics — rise time, settling time, percent overshoot, peak time, and steady-state error — is the primary acceptance test for any closed-loop design. These metrics translate the raw sample sequence into the language of control specifications, allowing automated pass/fail decisions without manual inspection of time-domain plots.
Let \(y[k]\), \(k = 0, \ldots, N-1\) be the sampled step response and \(y_{ss}\) the steady-state value. The sample period is \(\Delta t\).
Rise Time \(T_r\)
The elapsed time for the response to travel from 10 % to 90 % of steady state:
\[T_r = (k_{90} - k_{10})\,\Delta t\]
where \(k_{10} = \min\{k : y[k] \ge 0.1\,y_{ss}\}\) and \(k_{90} = \min\{k \ge k_{10} : y[k] \ge 0.9\,y_{ss}\}\).
Settling Time \(T_s\)
The first time after which the response remains permanently inside the band \([(1-\delta)y_{ss},\,(1+\delta)y_{ss}]\) (typically \(\delta = 0.02\)):
\[T_s = (k^* + 1)\,\Delta t, \quad k^* = \max\{k : |y[k] - y_{ss}| > \delta\,|y_{ss}|\}\]
Percent Overshoot \(\%OS\)
\[\%OS = 100\,\frac{y_{\max} - y_{ss}}{y_{ss}}, \quad y_{\max} = \max_k y[k]\]
For an underdamped second-order system with damping ratio \(\zeta\):
\[\%OS = 100\,\exp\!\left(-\frac{\pi\zeta}{\sqrt{1-\zeta^2}}\right)\]
Peak Time \(T_p\)
\[T_p = k_p\,\Delta t, \quad k_p = \arg\max_k y[k]\]
For a continuous underdamped second-order system with natural frequency \(\omega_n\):
\[T_p = \frac{\pi}{\omega_n\sqrt{1-\zeta^2}}\]
Steady-State Error \(e_{ss}\)
\[e_{ss} = r - \bar{y}_{\text{tail}}\]
where \(r\) is the reference (command) value and \(\bar{y}_{\text{tail}}\) is the mean of the final quarter of the response buffer, providing a robust estimate of the achieved steady state.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(N)\) | \(O(1)\) | Single forward pass; no auxiliary storage |
Each metric requires at most one traversal of the \(N\)-element vector. The tail-mean for steady-state error adds a constant-fraction second scan of the same data — still \(O(N)\) total.
Consider a 10-sample ramp to \(y_{ss} = 1\) followed by a constant plateau (N = 20):
k: 0 1 2 3 4 5 6 7 8 9 10 11 …
y: 0 .1 .2 .3 .4 .5 .6 .7 .8 .9 1 1 …Zero steady state. Division by \(y_{ss}\) in percent overshoot is guarded; the function returns zero when \(y_{ss} = 0\) to avoid a NaN.
Non-monotone ramp. If the response crosses 90 % before 10 % (e.g., DC offset or wrong initial condition), \(k_{10}\) may be found after the first 90 % crossing. The implementation returns the first pair that satisfies the threshold order.
Oscillatory settling. Settling time is defined as the last time the trajectory leaves the band, not the first time it enters it. Repeated crossings near the boundary extend the metric correctly.
Finite buffer. With a bounded vector of length \(N\), if the response has not yet settled by
the final sample, SettlingTime returns \(N\,\Delta t\) and RiseTime
returns \((N-1)\,\Delta t\) as
conservative bounds.
Tail-mean length. Using the last \(\lfloor N/4 \rfloor + 1\) samples for the steady-state estimate assumes the transient has decayed to within numerical noise by that point. Poorly chosen \(N\) relative to the system time constant degrades the estimate.
Mean function on a
sub-range.The matrix exponential is the fundamental solution operator for linear time-invariant (LTI) systems. Given a square matrix \(A\), the matrix exponential \(e^A\) maps initial conditions of the ODE \(\dot{x} = Ax\) to their exact solution at unit time: \(x(1) = e^A x(0)\). In embedded control, it appears as the exact discretisation of a continuous-time state-space model — turning the pair \((A_c, B_c)\) into the discrete \((A_d, B_d)\) that an MCU actually executes. Accurate, bounded computation of \(e^A\) is therefore a prerequisite for correct control deployment.
For a square matrix \(A \in \mathbb{R}^{n \times n}\), the matrix exponential is defined by the power series
\[e^A = \sum_{k=0}^{\infty} \frac{A^k}{k!} = I + A + \frac{A^2}{2!} + \frac{A^3}{3!} + \cdots\]
The series converges for every finite matrix and every field of characteristic zero.
Truncating the series directly is unreliable for large \(\|A\|\) because the intermediate terms grow into a large “hump” before cancelling — a phenomenon of catastrophic cancellation. The scaling-and-squaring algorithm avoids this by exploiting the identity
\[e^A = \left(e^{A/2^s}\right)^{2^s}\]
Choose \(s = \max\!\left(0, \lceil \log_2 \|A\|_\infty \rceil\right)\) so that \(\|A/2^s\|_\infty \leq 1\), compute \(e^{A/2^s}\) accurately on the shrunken argument, then recover \(e^A\) by repeated squaring.
For the scaled matrix \(\hat{A} = A/2^s\), the exponential is approximated by the diagonal \((q, q)\) Padé rational function
\[e^{\hat{A}} \approx R_q(\hat{A}) = D_q(\hat{A})^{-1} N_q(\hat{A})\]
where \(N_q\) and \(D_q\) are matrix polynomials whose scalar counterparts are the numerator and denominator of the \((q,q)\) Padé approximant to \(e^x\). The coefficients \(c_k\) satisfy
\[c_k = \frac{(2q - k)!\, q!}{(2q)!\, k!\, (q-k)!}, \quad k = 0, 1, \ldots, q\]
For \(q = 6\) the coefficients are \(c_0 = 1\), \(c_1 = \tfrac{1}{2}\), \(c_2 = \tfrac{5}{44}\), \(c_3 = \tfrac{1}{66}\), \(c_4 = \tfrac{1}{792}\), \(c_5 = \tfrac{1}{15840}\), \(c_6 = \tfrac{1}{665280}\).
The even/odd split halves the number of matrix multiplications:
\[V = c_0 I + c_2 \hat{A}^2 + c_4 \hat{A}^4 + c_6 \hat{A}^6\] \[U = \hat{A}\!\left(c_1 I + c_3 \hat{A}^2 + c_5 \hat{A}^4\right)\] \[N_q = V + U, \quad D_q = V - U\]
The ratio \(D_q^{-1} N_q\) is evaluated by solving the linear system \(D_q X = N_q\) column-by-column using LU factorisation with partial pivoting, never forming \(D_q^{-1}\) explicitly.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Infinity-norm | \(O(n^2)\) | \(O(1)\) | Determines scaling exponent \(s\) |
| Matrix powers \(A^2, A^4, A^6\) | \(3\,O(n^3)\) | \(3 n^2\) | Even/odd split; three multiplications total |
| Polynomial evaluation | \(O(n^3)\) | \(2 n^2\) | Horner-style accumulation of \(U\) and \(V\) |
| LU solve (\(D_q X = N_q\)) | \(O(n^3)\) | \(n^2\) | One factorisation, \(n\) substitution passes |
| Squaring (\(s\) steps) | \(s\,O(n^3)\) | \(n^2\) | At most \(\lceil \log_2 \|A\|_\infty \rceil\) steps |
| Total | \(O((6+s)n^3)\) | \(O(n^2)\) | Stack-only; no heap |
Consider \(A = \begin{bmatrix}0 & -1 \\ 1 & 0\end{bmatrix}\), a rotation generator with \(\theta = 1\).
Catastrophic cancellation without scaling: naive Padé on a large argument produces entries that nearly cancel, magnifying rounding error. The scaling step ensures \(\|\hat{A}\|_\infty \leq 1\) before the rational approximation is applied.
Nilpotent matrices: the series terminates in finitely many terms. Scaling-and-squaring handles this transparently; the Padé approximant reduces to a truncated polynomial.
Stiff systems (large negative eigenvalues): the result is bounded because \(e^{\lambda}\) with \(\lambda \ll 0\) is near zero. Floating-point underflow may drive these entries to zero; this is physically correct and numerically harmless.
Singular denominator: \(D_q\) is singular only if \(e^A\) has a pole, which the matrix exponential never does (\(A\) finite \(\Rightarrow\) \(e^A\) invertible). For the Padé denominator this means near-singularity can occur only at pathological arguments; the LU pivoting detects and gracefully handles it in practice.
Scaling exponent overflow: for a matrix with entries \(\approx 10^{38}\) the exponent \(s\) would be \(\approx 126\), requiring 126 squarings. This is accepted behaviour; the algorithm remains correct but slow.
Higher-order Padé: orders 8, 10, or 13 reduce the required scaling and improve accuracy for modest \(\|A\|\). Higham’s 2005 algorithm chooses the order adaptively. Order 6 is a reasonable default for embedded float arithmetic.
Schur decomposition pre-conditioning: computing \(e^A = Q e^T Q^\mathsf{T}\) (with \(T\) upper-triangular Schur form) avoids the hump phenomenon entirely and permits reuse of the Schur factors. The additional cost is the Schur decomposition itself (\(O(n^3)\)) and is not justified for small embedded matrices.
Taylor series with Horner evaluation: accurate only for \(\|A\| \ll 1\); the scaling step achieves exactly this, making scaling-and-squaring a superset.
ContinuousToDiscrete): \(A_d =
e^{A_c \Delta t}\).TriangularSolve (SolveUnitLowerTriangular,
SolveUpperTriangular) performs the back-substitution steps
of the LU solve inside SolvePade.
MatrixNorms::InfinityNorm computes the scaling exponent.
ContinuousToDiscrete (roadmap item 30) is the primary
consumer of this algorithm.
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.
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.
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.
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.
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.
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.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(n^2)\) | \(O(n^2)\) | Dominated by back-substitution in GE |
| Average | \(O(n^3)\) | \(O(n^2)\) | Gaussian elimination with partial pivoting |
| Worst | \(O(n^3)\) | \(O(n^2)\) | All pivots require row swaps |
All storage is stack-allocated; \(n\) is bounded at compile time by
Dim.
Consider a 2-D state (\(n = 2\)) with error \(\varepsilon = [1, 2]^{\mathsf{T}}\) and covariance \(P = \mathrm{diag}(1, 4)\).
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.
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.
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 covariance being
symmetric positive-definite, the linear solve reuses
math::CholeskyDecomposition::Solve (factorisation plus
forward/back substitution), keeping the metric a dependency-free
math primitive.
Angles live on a circle, not a line: a heading of \(179^\circ\) and one of \(-179^\circ\) are \(2^\circ\) apart, not \(358^\circ\). Controllers that subtract angles naively command a full turn the wrong way, integrators that never wrap lose precision as the value grows, and encoders or IMUs report wrapped values that an estimator must turn back into a continuous signal. These bugs recur in every module that touches angles, so the library provides one stateless, tested set of helpers with one convention: wrap into a canonical interval, take the shortest signed difference, and unwrap a wrapped measurement.
Two angles are equivalent when they differ by a multiple of \(2\pi\). Wrapping picks the representative inside a fixed half-open interval of length \(2\pi\):
\[\operatorname{wrap}_{(-\pi,\pi]}(a) = r - \pi,\;\; r \in (0, 2\pi],\; r \equiv a + \pi \pmod{2\pi} \qquad \operatorname{wrap}_{[0,2\pi)}(a) = a \bmod 2\pi\]
The floating-point remainder (fmod) is truncated — it keeps the sign of the dividend. The implementation therefore shifts the angle so the interval starts at zero, takes the remainder \(r \in (-2\pi, 2\pi)\), adds \(2\pi\) if \(r\) is not positive (signed case) or is negative (unsigned case), and shifts back.
Convention (normative): the signed interval is \((-\pi, \pi]\); both \(+\pi\) and \(-\pi\) map to \(+\pi\), so a half turn has exactly one representation. The unsigned interval is \([0, 2\pi)\) and never returns \(2\pi\).
For target \(\theta_t\) and current \(\theta_c\):
\[\Delta(\theta_t, \theta_c) = \operatorname{wrap}_{(-\pi,\pi]}(\theta_t - \theta_c)\]
Its magnitude is the shortest arc and its sign is the direction of travel from \(\theta_c\) to \(\theta_t\).
Given the previous continuous value \(u_{k-1}\) and a new wrapped sample \(w_k\):
\[u_k = u_{k-1} + \Delta(w_k, u_{k-1})\]
This recovers the true continuous angle as long as consecutive true samples differ by less than \(\pi\); otherwise the step is aliased by a multiple of \(2\pi\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Wrap to \((-\pi,\pi]\) | \(O(1)\) | \(O(1)\) | One fmod, one compare, a few adds |
| Wrap to \([0,2\pi)\) | \(O(1)\) | \(O(1)\) | One fmod, two compares, one conditional add |
| Shortest difference | \(O(1)\) | \(O(1)\) | One subtraction plus one signed wrap |
| Unwrap | \(O(1)\) | \(O(1)\) | One shortest difference plus one add; no state |
Cost does not depend on the magnitude of the input: the remainder is a single operation, not a loop subtracting \(2\pi\).
Shortest difference, target \(170^\circ\), current \(-170^\circ\): plain difference \(= 340^\circ\). Shift: \(340 + 180 = 520\). Remainder mod \(360\): \(160\) (positive, no correction). Shift back: \(160 - 180 = -20^\circ\). The short way is \(20^\circ\) in the negative direction, not \(340^\circ\) forward.
Wrap, \(7\pi/3\): shift: \(7\pi/3 + \pi = 10\pi/3\). Remainder mod \(2\pi\): \(10\pi/3 - 2\pi = 4\pi/3\). Shift back: \(4\pi/3 - \pi = \pi/3\). The unsigned wrap gives the same \(\pi/3\).
Half turn: \(a = -\pi\) shifts to \(0\); the remainder is \(0\), which is not positive, so \(2\pi\) is added and the result is \(2\pi - \pi = +\pi\).
Unwrap, previous \(358^\circ\), new sample \(2^\circ\): \(\Delta = \operatorname{wrap}(2 - 358) = \operatorname{wrap}(-356) = +4^\circ\), so the continuous value is \(362^\circ\).
Precision degrades with \(|a|\) — the remainder itself is exact, but \(2\pi\) is not representable, so each full turn removed carries the representation error of \(2\pi\) (about \(1.7\times10^{-7}\) rad in single precision) and the total error grows like \(|a|\,\varepsilon\). Wrap at the source, every integration step, rather than after long accumulation. An unwrapped signal is unbounded by design, so its own resolution shrinks as it grows.
Aliasing in unwrap — if the true angle moves by \(\pi\) or more between samples, the direction is ambiguous and the result is off by a multiple of \(2\pi\). Sample faster than half a turn per period.
Half-turn asymmetry — because \(\pm\pi\) both map to \(+\pi\), the shortest difference is not antisymmetric at exactly a half turn: swapping the arguments gives \(+\pi\) again, not \(-\pi\).
Rounding at the unsigned boundary — a tiny negative remainder plus \(2\pi\) can round to exactly \(2\pi\); the unsigned wrap detects this and returns \(0\), so the result stays strictly inside \([0, 2\pi)\).
Non-finite input — infinities and NaN have no meaningful remainder; inputs must be finite.
Float-only — floating-point types only, enforced at compile time; no fixed-point variants.
Degrees or turns as the unit only rescale the period (\(360\) or \(1\) instead of \(2\pi\)); the unit-turn form avoids the inexact \(2\pi\) entirely. Other canonical intervals, such as \([-\pi, \pi)\), differ only in which half-turn representative is kept. For rotations in 3D, quaternions avoid the wrap problem altogether, at the cost of a double cover (\(q\) and \(-q\)).
Used by the complementary filter in its heading mode. Related to the quaternion rotation representation, which has no wrap discontinuity.
A tridiagonal system has nonzeros only on the main diagonal and its two neighbours. Spline interpolation, implicit heat/diffusion steps and several smoothing problems all reduce to such systems. A dense solver wastes cubic time and quadratic memory on zeros; the Thomas algorithm is Gaussian elimination that skips every zero, so it runs in linear time using three short bands.
The elimination sweep depends only on the matrix, so it is performed once and reused: each new right-hand side costs two short sweeps and no division. This fits the common case where the matrix depends on fixed geometry (for example knot spacing) while the right-hand side changes per channel or per tick.
Let \(A \in \mathbb{R}^{n \times n}\) be tridiagonal with sub-diagonal \(a_k = A_{k,k-1}\) (\(a_0\) unused), diagonal \(b_k\) and super-diagonal \(c_k = A_{k,k+1}\) (\(c_{n-1}\) unused). The system \(Ax = d\) reads
\[a_k x_{k-1} + b_k x_k + c_k x_{k+1} = d_k, \quad k = 0, \dots, n-1\]
Eliminating \(a_k\) with the already-reduced row above leaves an upper bidiagonal system with unit diagonal. Define the pivots and modified super-diagonal
\[p_0 = b_0, \qquad p_k = b_k - a_k\, c'_{k-1}, \qquad c'_k = \frac{c_k}{p_k}\]
The reciprocals \(1/p_k\) are stored, so \(c'_k = c_k \cdot (1/p_k)\). This is the factorization \(A = LU\) with \(L\) lower bidiagonal (diagonal \(p_k\), sub-diagonal \(a_k\)) and \(U\) unit upper bidiagonal (super-diagonal \(c'_k\)).
\[y_0 = \frac{d_0}{p_0}, \qquad y_k = \frac{d_k - a_k\, y_{k-1}}{p_k}\]
\[x_{n-1} = y_{n-1}, \qquad x_k = y_k - c'_k\, x_{k+1}, \quad k = n-2, \dots, 0\]
Division by \(p_k\) is replaced by multiplication with the stored reciprocal.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Factorization | \(O(n)\) | \(O(n)\) | \(n\) divisions, one multiply-subtract per row |
| Solve (per RHS) | \(O(n)\) | \(O(n)\) | About \(3n\) multiply-adds, no division |
| Dense LU, for contrast | \(O(n^3)\) | \(O(n^2)\) | Operates on all the zeros |
Storage is three bands of fixed capacity; nothing is allocated dynamically. Factoring once and solving \(m\) right-hand sides costs \(O(n + mn)\) with all divisions paid up front.
Take \(A = \operatorname{tridiag}(-1, 2, -1)\) with \(n = 4\), so \(a_k = c_k = -1\), \(b_k = 2\).
| \(k\) | \(p_k\) | \(1/p_k\) | \(c'_k\) |
|---|---|---|---|
| 0 | \(2\) | \(1/2\) | \(-1/2\) |
| 1 | \(2 - (-1)(-1/2) = 3/2\) | \(2/3\) | \(-2/3\) |
| 2 | \(2 - (-1)(-2/3) = 4/3\) | \(3/4\) | \(-3/4\) |
| 3 | \(2 - (-1)(-3/4) = 5/4\) | \(4/5\) | unused |
Right-hand side \(d = (1, 0, 0, 1)\): forward \(y = (1/2,\ 1/3,\ 1/4,\ 1)\); back substitution gives \(x_3 = 1\), \(x_2 = 1/4 + \tfrac34 = 1\), \(x_1 = 1/3 + \tfrac23 = 1\), \(x_0 = 1/2 + 1/2 = 1\), so \(x = (1, 1, 1, 1)\).
Right-hand side \(d = (1, 2, 3, 4)\) reuses the same table: forward \(y = (1/2,\ 5/3,\ 7/2,\ 6)\); back substitution gives \(x_3 = 6\), \(x_2 = 7/2 + 9/2 = 8\), \(x_1 = 5/3 + 16/3 = 7\), \(x_0 = 1/2 + 7/2 = 4\), so \(x = (4, 7, 8, 6)\).
Verify: row 1 of \(Ax\) is \(-4 + 14 - 8 = 2\) ✓ and row 3 is \(-8 + 12 = 4\) ✓.
| Algorithm | Relationship |
|---|---|
| LU Decomposition | General fallback with partial pivoting; \(O(n^3)\) and dense |
| Triangular Solve | Same forward/back substitution idea, applied to full triangular factors |
| Levinson–Durbin | Another structured solver, for Toeplitz systems |
| Cubic spline interpolation | Main consumer; plans the second derivatives by solving one tridiagonal system |
Waypoint lists (robot teach-in programs, CNC paths, planner output), calibration tables and resampled signals all need a smooth curve through given samples. A single high-order polynomial through many points rings between them; a piecewise-linear curve has corners that excite mechanics. A cubic spline joins one cubic per interval so that value, slope and curvature are continuous (\(C^2\)), and it stays well behaved however many knots are used.
Several channels (the joints of a robot, the axes of a path, the columns of a table) share the same abscissae. They are fitted together: the linear system depends only on the knot spacing, so it is factored once and every channel reuses the factors. Fitting is a single linear-time pass on fixed storage; evaluation is a binary search plus a cubic, cheap and deterministic.
Knots \(x_0 < x_1 < \dots < x_{K-1}\) with spans \(h_k = x_{k+1} - x_k\) and values \(y_k\) (one component per channel; channels are independent). The unknowns are the knot second derivatives \(M_k = y''(x_k)\). On interval \(k\), with \(\tau = x - x_k \in [0, h_k]\),
\[y(\tau) = y_k + \beta_k\,\tau + \tfrac{M_k}{2}\,\tau^2 + d_k\,\tau^3, \qquad d_k = \frac{M_{k+1} - M_k}{6\,h_k}, \qquad \beta_k = \frac{y_{k+1} - y_k}{h_k} - \frac{h_k\,(2M_k + M_{k+1})}{6}\]
The cubic hits both knot values and both knot curvatures, so value is \(C^0\) and curvature is \(C^2\) by construction. Only slope continuity remains to be imposed.
Equating the slope at the end of interval \(k-1\), \(\beta_{k-1} + h_{k-1}(M_{k-1} + M_k)/2\), with \(\beta_k\) and writing \(s_k = (y_{k+1} - y_k)/h_k\) gives, for \(k = 1, \dots, K-2\),
\[h_{k-1} M_{k-1} + 2(h_{k-1} + h_k)\,M_k + h_k M_{k+1} = 6\,(s_k - s_{k-1})\]
Each row couples a knot to its two neighbours only, so the system is tridiagonal with \(K-2\) rows. Two end conditions supply the missing two.
| Condition | First row | Last row |
|---|---|---|
| Clamped (end slopes \(\sigma_0\), \(\sigma_1\)) | \(2h_0 M_0 + h_0 M_1 = 6\,(s_0 - \sigma_0)\) | \(h_{K-2} M_{K-2} + 2h_{K-2} M_{K-1} = 6\,(\sigma_1 - s_{K-2})\) |
| Natural | \(M_0 = 0\) | \(M_{K-1} = 0\) |
Clamping fixes the end slopes and leaves the end second derivatives free; natural ends do the reverse. Default slopes are zero.
Every interior row has diagonal \(2(h_{k-1} + h_k)\) against off-diagonal sum \(h_{k-1} + h_k\); clamped end rows have \(2h\) against \(h\); natural rows have \(1\) against \(0\). All rows are strictly diagonally dominant, so the matrix is nonsingular and the Thomas algorithm is stable without pivoting. The matrix contains only spans and the end-condition kind, never values or slopes: one factorization serves all channels, and only the right-hand sides differ.
Clamp \(x\) to \([x_0, x_{K-1}]\), find the interval by binary search, then use Horner’s scheme per channel:
\[y = y_k + \tau\,\bigl(\beta_k + \tau\,(\tfrac{M_k}{2} + \tau\,d_k)\bigr), \quad y' = \beta_k + \tau\,(M_k + 3 d_k \tau), \quad y'' = M_k + 6 d_k \tau\]
An abscissa exactly on an interior knot selects the interval starting there; the last knot selects the final interval. Both choices give the same result because the spline is \(C^2\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Factorization | \(O(K)\) | three bands | Once per fit, shared by all channels |
| Fit | \(O(D\,K)\) | \(K_{\max}(2 + 2D)\) scalars | \(D\) channels, each one right-hand side build and one solve of \(O(K)\); no iteration |
| Evaluate | \(O(\log K + D)\) | \(O(1)\) | Binary search, then three Horner polynomials per channel |
Storage holds the knots, their reciprocal spans, the values and the second derivatives: \(K_{\max}(2 + 2D)\) scalars, where \(K_{\max}\) is the compile-time capacity, plus the three bands of the solver. All of it is bounded and on the stack or inside the object; no heap.
Knots \((0, 1, 2)\), values \((0, 1, 0)\), so \(h_0 = h_1 = 1\) and \(s_0 = 1\), \(s_1 = -1\).
Clamped, zero end slopes. The system is \[\begin{pmatrix} 2 & 1 & 0 \\ 1 & 4 & 1 \\ 0 & 1 & 2 \end{pmatrix} M = \begin{pmatrix} 6 \\ -12 \\ 6 \end{pmatrix}\]
| \(k\) | Pivot \(p_k\) | \(c'_k\) | Forward \(y_k\) | Back \(M_k\) |
|---|---|---|---|---|
| 0 | \(2\) | \(1/2\) | \(3\) | \(3 + 3 = 6\) |
| 1 | \(4 - 1/2 = 7/2\) | \(2/7\) | \(-30/7\) | \(-30/7 - 12/7 = -6\) |
| 2 | \(2 - 2/7 = 12/7\) | unused | \(6\) | \(6\) |
So \(M = (6, -6, 6)\). Interval 0 has \(\beta_0 = 1 - (12 - 6)/6 = 0\) and \(d_0 = -2\), giving \(y(\tau) = 3\tau^2 - 2\tau^3\). Hence \(y(0.5) = 0.75 - 0.25 = 0.5\), \(y'(0.5) = 3 - 1.5 = 1.5\) and \(y''(0.5) = 0\). The curve leaves and enters every knot flat, like a smooth step.
Natural. The end rows reduce to \(M_0 = M_2 = 0\), so the middle row is \(4M_1 = -12\) and \(M = (0, -3, 0)\). Interval 0 has \(\beta_0 = 1 + 3/6 = 1.5\) and \(d_0 = -0.5\), giving \(y(\tau) = 1.5\tau - 0.5\tau^3\). Hence \(y(0.5) = 0.75 - 0.0625 = 0.6875\) and \(y'(0.5) = 1.125\). The end slope \(y'(0) = 1.5\) is free, and the curvature is zero there.
Random fits are checked for interpolation of every knot, for continuity of first and second derivative across each interior knot, and for the end conditions (prescribed slopes when clamped, zero curvature when natural). Continuity holds to about \(10^{-13}\) in double precision and to the single-precision rounding level in float.
| Algorithm | Relationship |
|---|---|
| Tridiagonal Solver | Solves the second-derivative system; its factorization is reused across channels |
| Polynomial Profile | The two-knot clamped case is the same cubic Hermite polynomial; the spline generalizes it to many knots with \(C^2\) joins |
| Polynomial fitting | Least-squares fit instead of interpolation |
Every pose-level algorithm — manipulator kinematics and Jacobians, pose inverse kinematics, Cartesian interpolation, task-space control, drone navigation, visual odometry — manipulates the same few objects: a rigid transform, a velocity-like 6-vector (a twist), a force-like 6-vector (a wrench), and the maps between a constant twist and the finite motion it produces. Defining them once, with one ordering convention, prevents the silent sign and ordering mismatches that appear when independently written modules exchange 6-vectors.
A transform is a rotation plus a translation: twelve scalars, composed with a few \(3\times3\) products and inverted in closed form, with no matrix solve. A twist is re-expressed in another frame by the adjoint: rotate both parts, then add the lever-arm effect of the frame offset to the linear part. Wrenches follow the dual rule so that power is the same in every frame. The exponential turns “rotate about a line while sliding along it” into a finite motion; the logarithm recovers that motion, which is exactly what pose-error feedback needs.
A transform \(T = (R, p)\) with \(R \in SO(3)\) (\(R^T R = I\), \(\det R = +1\)) and \(p \in \mathbb{R}^3\) maps coordinates in frame B to frame A, \(x_A = R\,x_B + p\). Its homogeneous form is
\[T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix}.\]
\[T_1 T_2 = (R_1 R_2,\; R_1 p_2 + p_1), \qquad T^{-1} = (R^T,\; -R^T p), \qquad T\cdot x = R x + p.\]
The composition applies \(T_2\) first. The inverse follows from \(R^{-1} = R^T\). A free direction is only rotated, \(x \mapsto R x\); the translation is ignored.
All 6-vectors are ordered linear part first: twists \(V = (v;\, \omega)\) and wrenches \(W = (f;\, n)\). This matches a geometric Jacobian with linear rows first and the tangent ordering of Sophus and manif. Lynch & Park and Murray et al. use \((\omega;\, v)\); the two conventions differ by a block permutation, so any formula from those texts converts by swapping the blocks (for the adjoint below, the rotation blocks stay on the diagonal and the \([p]_\times R\) block moves to the lower left).
Let \([a]_\times\) be the skew matrix with \([a]_\times b = a \times b\). A twist expressed in frame B is re-expressed in frame A by
\[V_A = \mathrm{Ad}_T V_B, \qquad \mathrm{Ad}_T = \begin{pmatrix} R & [p]_\times R \\ 0 & R \end{pmatrix}, \qquad \text{i.e. } v' = R v + p \times R\omega,\;\; \omega' = R\omega.\]
The wrench map is the power-consistent dual, \(\mathrm{Ad}_T^{-T}\):
\[f' = R f, \qquad n' = R n + p \times R f.\]
Power is frame invariant because the cross terms cancel by the antisymmetry of the scalar triple product:
\[W' \cdot V' = f \cdot v + n \cdot \omega + \underbrace{Rf \cdot (p \times R\omega) + (p \times Rf) \cdot R\omega}_{=\,0} = W \cdot V.\]
The adjoint is a group homomorphism, \(\mathrm{Ad}_{AB} = \mathrm{Ad}_A\,\mathrm{Ad}_B\), and \(\mathrm{Ad}_{T^{-1}} = \mathrm{Ad}_T^{-1}\).
A twist \(\xi = (v;\, \omega)\) held for \(\theta\) produces the finite motion \(e^{[\xi]\theta}\) with \([\xi] = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix}\). It has the block form \(\begin{pmatrix} e^{K\theta} & G v \\ 0 & 1 \end{pmatrix}\) with \(K = [\omega]_\times\) and \(G = \int_0^\theta e^{Ks}\,ds\): Rodrigues’ formula for the rotation, integrated term by term for the translation. Without normalising \(\omega\), and with \(\varphi = \theta\,\|\omega\|\),
\[R = I + \theta\,a_1\,K + \theta^2 a_2\,K^2, \qquad p = \theta v + \theta^2 a_2\,K v + \theta^3 a_3\,K^2 v,\]
\[a_1 = \frac{\sin\varphi}{\varphi}, \qquad a_2 = \frac{1-\cos\varphi}{\varphi^2} = \frac{2\sin^2(\varphi/2)}{\varphi^2}, \qquad a_3 = \frac{\varphi - \sin\varphi}{\varphi^3}.\]
Below \(\varphi = 0.1\) the coefficients switch to short Taylor series,
\[a_1 \approx 1 - \tfrac{\varphi^2}{6} + \tfrac{\varphi^4}{120}, \qquad a_2 \approx \tfrac12 - \tfrac{\varphi^2}{24} + \tfrac{\varphi^4}{720}, \qquad a_3 \approx \tfrac16 - \tfrac{\varphi^2}{120} + \tfrac{\varphi^4}{5040},\]
so there is no singular branch at \(\omega = 0\) (pure translation falls out as \(R = I\), \(p = \theta v\)) and no \(0/0\) or cancellation for small \(\varphi\). Because \(\theta\) scales the whole twist, a non-unit \(\omega\) needs no normalisation.
Re-expressing a twist in another frame generates the same motion conjugated by that frame:
\[\mathrm{Exp}(\mathrm{Ad}_A\,\xi,\, t) = A\,\mathrm{Exp}(\xi,\, t)\,A^{-1}.\]
Let \(s = \tfrac12\,\mathrm{vee}(R - R^T) = \sin\varphi\,\hat\omega\) and \(\cos\varphi = \tfrac12(\operatorname{tr}R - 1)\). The angle is
\[\varphi = \mathrm{atan2}\big(\|s\|,\; \cos\varphi\big),\]
which is accurate near \(0\) and \(\pi\), unlike \(\arccos\).
\[\hat\omega_i \hat\omega_j = \frac{\tfrac12(R_{ij} + R_{ji}) - \delta_{ij}\cos\varphi}{1 - \cos\varphi}.\]
The pivot component \(i\) is the largest diagonal entry, so \(\hat\omega_i^2 \ge \tfrac13\) and the divisions are benign; \(\hat\omega_i = \sqrt{\hat\omega_i^2}\), the other components follow from the off-diagonal products, and the sign is taken from \(s_i\). At exactly \(\pi\) either sign is valid. The result satisfies \(\|\omega\| \le \pi\): rotations are re-expressed the short way.
With \(\omega = \mathrm{Log}(R)\) and \(\varphi = \|\omega\|\), the inverse of the integral \(G\) is
\[v = \Big(I - \tfrac12[\omega]_\times + c(\varphi)\,[\omega]_\times^2\Big)\,p, \qquad c(\varphi) = \frac{1 - \tfrac{\varphi}{2}\cot\tfrac{\varphi}{2}}{\varphi^2} \approx \tfrac1{12} + \tfrac{\varphi^2}{720} + \tfrac{\varphi^4}{30240} \;\;(\varphi < 0.1).\]
The result is \((v;\, \omega)\) with \(\theta = 1\), so \(\mathrm{Exp}(\mathrm{Log}(T), 1) = T\). The coefficient stays finite on \([0, \pi]\), reaching \(1/\pi^2\) at \(\varphi = \pi\).
With target \((R_t, p_t)\) and current \((R_c, p_c)\), the base-frame error is
\[e = \big(p_t - p_c;\;\; \mathrm{Log}(R_t R_c^T)\big),\]
the translation difference stacked on the rotation vector that carries the current orientation onto the target. It has the linear-first ordering of a geometric Jacobian, and \(\|e_\omega\| \le \pi\) always points the short way round.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Compose | O(1) | O(1) | one \(3\times3\) product, one matrix–vector product |
| Inverse | O(1) | O(1) | transpose and one matrix–vector product; no solve |
| Apply / rotate | O(1) | O(1) | one matrix–vector product (plus 3 additions for apply) |
| Twist / wrench transform | O(1) | O(1) | two matrix–vector products and one cross product, no \(6\times6\) formed |
| Adjoint | O(1) | O(1) | \(36\) scalars, one extra \(3\times3\) product; built only on request |
| Exp | O(1) | O(1) | two sines of \(\varphi\) and \(\varphi/2\), none below \(\varphi = 0.1\) |
| Log | O(1) | O(1) | one atan2; a half-angle sine/cosine pair above \(\varphi = 0.1\); one square root for large angles |
A transform holds \(12\) scalars; the \(6\times6\) adjoint appears only when requested. All operations are stack-only: no heap, no recursion, constant work per call.
Apply. \(R = R_z(\pi/2)\), \(p = (1,0,0)\): \(R\,(1,0,0) = (0,1,0)\), plus \(p\) gives \((1,1,0)\).
Exp of a screw. Rotation about \(\hat z\) through the point \((1,0,0)\): \(\omega = \hat z\), \(v = -\hat z \times (1,0,0) = (0,-1,0)\), \(\theta = \pi/2\).
| Step | Value |
|---|---|
| \(\varphi = \theta\|\omega\|\) | \(\pi/2 \ge 0.1\), direct forms |
| Coefficients | \(a_1 = 2/\pi\), \(a_2 = 4/\pi^2\), \(a_3 = (\pi/2 - 1)/(\pi/2)^3\) |
| Vectors | \(Kv = \hat z \times v = (1,0,0)\), \(K^2 v = (0,1,0)\) |
| \(R\) | \(\theta a_1 = 1\), \(\theta^2 a_2 = 1\): \(R = I + K + K^2 = R_z(\pi/2)\) |
| \(p\) | \(\tfrac{\pi}{2}(0,-1,0) + (1,0,0) + (\tfrac{\pi}{2} - 1)(0,1,0) = (1,-1,0)\) |
Check: the origin sits at \((-1,0,0)\) relative to the axis; a quarter turn takes it to \((0,-1,0)\), and adding the axis point gives \((1,-1,0)\).
Log of that transform. \(s = (0,0,1)\), \(\cos\varphi = 0\), so \(\varphi = \pi/2\) and \(\omega = (0,0,\pi/2)\). Then \(\omega \times p = (\tfrac\pi2, \tfrac\pi2, 0)\), \(\omega \times (\omega \times p) = (-\tfrac{\pi^2}{4}, \tfrac{\pi^2}{4}, 0)\), and \(c(\pi/2)\,\pi^2/4 = 1 - \pi/4\), so
\[v = (1 - \tfrac\pi4 - (1 - \tfrac\pi4),\; -1 - \tfrac\pi4 + (1 - \tfrac\pi4),\; 0) = (0, -\tfrac\pi2, 0).\]
The result \(\big((0,-\tfrac\pi2,0);\,(0,0,\tfrac\pi2)\big)\) is \(\theta\xi\), as expected.
Pose error, short way round. Target rotated \(+190^\circ\) about \(\hat z\) relative to the current pose: \(\cos\varphi = \cos 190^\circ = -0.9848\), \(\|s\| = \sin 170^\circ = 0.1736\), so \(\varphi = 170^\circ = 2.967\) rad \(> \pi/2\). The symmetric part gives \(\hat\omega_z^2 = (1 + 0.9848)/1.9848 = 1\), and \(s_z = \sin 190^\circ < 0\) selects \(-\hat z\). The rotation error is \(-170^\circ\cdot\hat z\), not \(+190^\circ\).
| Variant | Key difference |
|---|---|
| Angular-first ordering \((\omega;\, v)\) | Same algebra with blocks permuted; used by Lynch & Park and Murray et al. |
| Body vs. spatial twist | Body twists are \(T^{-1}\dot T\), spatial twists \(\dot T\,T^{-1}\); related by the adjoint |
| Quaternion rotation part | Four scalars and cheap renormalisation; the rotation vector is the vector part scaled by \(\varphi/\sin(\varphi/2)\) |
| Dual quaternions | Rotation and translation in one algebra; screw interpolation |
| Similarity group | Adds a uniform scale for monocular visual odometry |
A polynomial profile is a closed-form time law that drives one axis from a start state to a goal state over a fixed duration \(T\). A cubic matches endpoint position and velocity; a quintic additionally matches endpoint acceleration, so the motion has no acceleration step at either end.
Point-to-point moves are the bread and butter of servo motion: robot joints, CNC and pick-and-place axes, motor position loops. The coefficients come from a handful of algebraic expressions, solved once. Each servo tick is then a fixed number of multiply-adds, with no iteration, no lookup table and a deterministic cycle count.
Let \(t \in [0, T]\), \(\Delta q = q_f - q_0\), and let \(\alpha_0, \alpha_f\) be the start and end accelerations. A polynomial of degree \(n\) has \(n+1\) coefficients, so a cubic satisfies four conditions and a quintic six:
\[q(0) = q_0,\quad q(T) = q_f,\quad \dot{q}(0) = v_0,\quad \dot{q}(T) = v_f \qquad \text{(quintic also: } \ddot{q}(0) = \alpha_0,\ \ddot{q}(T) = \alpha_f\text{)}\]
The conditions are linear in the coefficients \(a_k\) of \(q(t) = \sum_k a_k t^k\) and the time basis is fixed, so the solution is a plug-in formula rather than a runtime matrix solve.
The start conditions give \(a_0 = q_0\) and \(a_1 = v_0\). The end conditions leave a \(2\times 2\) system with the closed-form solution
\[a_2 = \frac{3\,\Delta q - (2 v_0 + v_f)\,T}{T^2}, \qquad a_3 = \frac{-2\,\Delta q + (v_0 + v_f)\,T}{T^3}\]
The acceleration is linear in \(t\) and the jerk is the constant \(6 a_3\).
The start conditions give \(a_0 = q_0\), \(a_1 = v_0\), \(a_2 = \alpha_0 / 2\). The end conditions give
\[a_3 = \frac{20\,\Delta q - (8 v_f + 12 v_0)\,T - (3\alpha_0 - \alpha_f)\,T^2}{2\,T^3}\]
\[a_4 = \frac{-30\,\Delta q + (14 v_f + 16 v_0)\,T + (3\alpha_0 - 2\alpha_f)\,T^2}{2\,T^4}\]
\[a_5 = \frac{12\,\Delta q - 6 (v_f + v_0)\,T - (\alpha_0 - \alpha_f)\,T^2}{2\,T^5}\]
The acceleration is a cubic matching \(\alpha_0\) and \(\alpha_f\), hence continuous; the jerk is a quadratic.
Position, velocity, acceleration and jerk are four Horner evaluations of the polynomial and its three derivatives:
\[q = a_0 + t\big(a_1 + t(a_2 + t(a_3 + t(a_4 + t\,a_5)))\big), \qquad \dddot{q} = 6 a_3 + t\,(24 a_4 + 60 a_5\, t)\]
The cubic is the same evaluation with \(a_4 = a_5 = 0\). The time is first clamped to \([0, T]\).
With zero end velocities (and accelerations, for the quintic) the cubic peaks at \(1.5\,\Delta q / T\) in velocity and \(6\,\Delta q / T^2\) in acceleration; the quintic at \(1.875\,\Delta q / T\) and \(\tfrac{10}{\sqrt{3}}\,\Delta q / T^2 \approx 5.77\,\Delta q / T^2\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Coefficient solve | \(O(1)\) | \(O(1)\) | Closed-form expressions, once at construction or reset |
| Sample | \(O(\text{degree})\) | \(O(1)\) | Four Horner evaluations, at most 14 multiply-adds; no state change, so deterministic |
| Stored state | n/a | \(O(1)\) | Six coefficients, the duration and the degree; stack only, no heap |
Rest-to-rest cubic with \(q_0 = 0\), \(q_f = 1\), \(T = 2\), \(v_0 = v_f = 0\), so \(\Delta q = 1\) and \(a_0 = a_1 = 0\).
| Step | Computation | Result |
|---|---|---|
| \(a_2\) | \((3\cdot 1 - 0)/2^2\) | \(0.75\) |
| \(a_3\) | \((-2\cdot 1 + 0)/2^3\) | \(-0.25\) |
| Position | \(0.75\,t^2 - 0.25\,t^3 = 3(t/2)^2 - 2(t/2)^3\) | \(q(1) = 0.5\), \(q(2) = 1\) |
| Velocity | \(1.5\,t - 0.75\,t^2\) | \(v(1) = 0.75 = 1.5\,\Delta q / T\) (the peak) |
| Acceleration | \(1.5 - 1.5\,t\) | \(+1.5\) at \(t=0\), \(-1.5\) at \(t=2\) |
| Jerk | \(6 a_3\) | \(-1.5\), constant |
The position is the smoothstep of \(t/2\), symmetric about the midpoint, where the velocity peaks. The acceleration jumps from \(0\) at rest to \(1.5\) at the first instant and from \(-1.5\) back to \(0\) at the end.
| Variant | Key difference |
|---|---|
| Cubic (this) | Position and velocity matched; constant jerk; acceleration discontinuous at the ends |
| Quintic (this) | Adds acceleration matching; continuous acceleration at about \(25\%\) higher peak velocity for rest-to-rest |
| Path-parameter time law | A 0 to 1 instance maps time to a normalized path coordinate; with \(v_0 = v_f = 1\), \(T = 1\) the cubic collapses to \(s = t\) |
| Algorithm | Relationship |
|---|---|
| Trapezoidal velocity profile | Respects velocity and acceleration limits with a free duration; polynomials fix the duration and let the peaks follow |
| S-curve profile | Bounds jerk explicitly under limits; the quintic only smooths the endpoints |
| Cubic spline interpolation | Handles many via points with continuous acceleration; for two knots it reduces to this cubic |
A point-to-point move has to reach its goal as fast as the actuator allows without saturating the drive. The trapezoidal profile — linear segment with parabolic blends (LSPB) — does this with the simplest rule that respects both a velocity ceiling and an acceleration ceiling: accelerate at the maximum rate, coast at the maximum speed, decelerate at the maximum rate. Velocity plotted against time is a trapezoid; a move too short to reach the ceiling becomes a triangle.
From the limits and the distance alone, the plan is the minimum-time one. A second mode fixes the duration instead, which multi-axis synchronization needs: all axes must finish together, so the faster ones are slowed rather than the slowest sped up.
Rest-to-rest move of length \(d = |q_f - q_0|\) in direction \(\sigma = \pm 1\) (positive when \(q_f \ge q_0\)), with limits \(v_{max} > 0\) and \(a_{max} > 0\). The profile is planned for the magnitude and mirrored by \(\sigma\).
A ramp between rest and speed \(v\) takes \(v/a_{max}\) and covers \(v^2/(2a_{max})\), so the two ramps together consume the blend distance \(v^2/a_{max}\). Comparing \(d\) with the blend distance at \(v_{max}\) selects the shape:
\[ \begin{cases} d \ge v_{max}^2/a_{max}: & v = v_{max}, \quad t_a = v/a_{max}, \quad t_c = (d - v^2/a_{max})/v \quad\text{(trapezoid)} \\ d < v_{max}^2/a_{max}: & v = \sqrt{d\,a_{max}}, \quad t_a = v/a_{max}, \quad t_c = 0 \quad\text{(triangle)} \end{cases} \]
The total time is \(T = 2t_a + t_c\). The cases agree at \(d = v_{max}^2/a_{max}\), so \(T\) is continuous in \(d\).
With \(v\), \(t_a\), \(t_c\), \(T\) precomputed, the unsigned state is:
| Phase | Interval | Acceleration | Velocity | Position |
|---|---|---|---|---|
| Parabolic ramp-up | \(0 \le t < t_a\) | \(a_{max}\) | \(a_{max}\,t\) | \(\tfrac12 a_{max}\,t^2\) |
| Linear cruise | \(t_a \le t < t_a + t_c\) | \(0\) | \(v\) | \(v\,(t - t_a/2)\) |
| Parabolic ramp-down | \(t_a + t_c \le t < T\) | \(-a_{max}\) | \(a_{max}\,(T-t)\) | \(d - \tfrac12 a_{max}\,(T-t)^2\) |
Velocity, acceleration and the position offset are multiplied by \(\sigma\). At and beyond \(t = T\) the profile holds the goal at rest (velocity and acceleration zero); times before \(0\) are treated as \(t = 0\), holding the start with zero velocity. The reported jerk is zero in every phase.
The acceleration \(a_{max}\) is kept and the cruise speed is lowered until the move takes exactly \(T\). The ramps cover \(v\,t_a\) in total and the cruise covers \(v\,(T - 2t_a)\), so with \(t_a = v/a_{max}\)
\[ d = v\left(T - \frac{v}{a_{max}}\right) \;\Longleftrightarrow\; v^2 - a_{max}T\,v + a_{max}\,d = 0 . \]
The smaller root keeps \(t_c \ge 0\). Multiplying it by its conjugate gives a form with no subtraction of nearly equal numbers:
\[ v = \frac{a_{max}T - \sqrt{a_{max}^2T^2 - 4a_{max}d}}{2} = \frac{2\,a_{max}\,d}{a_{max}T + \sqrt{a_{max}^2T^2 - 4a_{max}d}} . \]
A real root exists iff \(a_{max}\,T^2 \ge 4d\). Equality gives a double root \(v = a_{max}T/2\) and a triangle: \(T\) is then the shortest duration achievable at this acceleration. Otherwise \(t_a = v/a_{max}\) and \(t_c = T - 2t_a\).
Let \(T_{sync}\) be the longest axis time and \(\lambda = T_{sync}/T_{axis} \ge 1\). Planning an axis again with \((v_{max}/\lambda,\; a_{max}/\lambda^2)\) leaves the blend distance \(v_{max}^2/a_{max}\) unchanged, so the same shape is selected, while \(t_a\) and \(t_c\) both scale by \(\lambda\): the duration is exactly \(\lambda\,T_{axis}\). Velocity is divided by \(\lambda\) and acceleration by \(\lambda^2\).
The fixed-duration plan is the alternative. It keeps \(a_{max}\) and only lowers the cruise speed: the smaller root decreases as \(T\) grows, so for \(T \ge T_{min}\) the cruise speed never exceeds \(v_{max}\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Minimum-time plan | \(O(1)\) | \(O(1)\) | One comparison, one square root (triangle only) |
| Fixed-duration plan | \(O(1)\) | \(O(1)\) | One square root, one division |
| Sample | \(O(1)\) | \(O(1)\) | One phase branch, a few multiply-adds; no state change |
Memory is a fixed handful of scalars: start, direction, distance, acceleration, peak speed and three phase times.
Trapezoid. \(d = 5\), \(v_{max} = 1\), \(a_{max} = 2\). The blend distance is \(1/2 = 0.5 \le 5\), so a cruise exists: \(t_a = 0.5\), \(t_c = (5 - 0.5)/1 = 4.5\), \(T = 2(0.5) + 4.5 = 5.5\).
| \(t\) | Phase | Position | Velocity | Acceleration |
|---|---|---|---|---|
| \(0.25\) | ramp-up | \(\tfrac12\cdot 2\cdot 0.25^2 = 0.0625\) | \(0.5\) | \(2\) |
| \(3\) | cruise | \(1\cdot(3 - 0.25) = 2.75\) | \(1\) | \(0\) |
| \(5.25\) | ramp-down | \(5 - \tfrac12\cdot 2\cdot 0.25^2 = 4.9375\) | \(0.5\) | \(-2\) |
| \(\ge 5.5\) | held | \(5\) | \(0\) | \(0\) |
The ramp-up ends at position \(0.25\), and the cruise formula gives the same value at \(t = 0.5\). The triangle threshold for these limits is \(d = 0.5\); for \(d = 0.25\), \(v = \sqrt{0.5} \approx 0.7071\) and \(T \approx 0.7071\), with no cruise.
Fixed duration. \(d = 4\), \(a_{max} = 1\), \(T = 5\): discriminant \(25 - 16 = 9\), \(v = 2\cdot 4/(5 + 3) = 1\), \(t_a = 1\), \(t_c = 3\); check \(1\cdot(5 - 1) = 4\). For \(d = 5\), \(a_{max} = 2\), \(T = 6\): discriminant \(144 - 40 = 104\), \(v = 20/(12 + \sqrt{104}) \approx 0.900980\).
The S-curve profile adds a jerk limit (seven segments) for continuous acceleration at the cost of a longer move. Polynomial profiles (cubic, quintic) are fixed-duration and smooth by construction but enforce no velocity or acceleration ceiling. Unequal acceleration and deceleration limits use the same construction with unequal ramp times.
The S-curve profile is the jerk-limited upgrade under the same velocity and acceleration limits. The polynomial profile is fixed-time and smooth but offers no limit guarantees. The saturation rate limiter slews a changing setpoint online, whereas the trapezoid plans a whole move offline.
A trapezoidal velocity profile bounds speed and acceleration, but acceleration jumps instantaneously at every corner. Those jumps are impulses in jerk: they excite structural resonances, wear gearing, and make payloads wobble or liquids spill. The S-curve (double-S) profile also bounds jerk, so acceleration ramps linearly in and out and is continuous, giving far less vibration than a trapezoid. The price is a slightly longer move.
For a rest-to-rest move the phase durations have a closed form: planning is a handful of arithmetic operations, with no iteration. Three limits drive it: a velocity ceiling \(v_{\max}\), an acceleration ceiling \(a_{\max}\) and a jerk ceiling \(j_{\max}\). Short moves never reach the ceilings; the corresponding phases shrink to zero length.
A rest-to-rest move of length \(h > 0\) is built from seven constant-jerk phases:
| Phase | Jerk | Acceleration | Duration |
|---|---|---|---|
| 1 | \(+j_{\max}\) | ramps \(0 \to a_{lim}\) | \(T_j\) |
| 2 | \(0\) | constant \(a_{lim}\) | \(T_a - 2T_j\) |
| 3 | \(-j_{\max}\) | ramps \(a_{lim} \to 0\) | \(T_j\) |
| 4 | \(0\) | zero, cruise at \(v_{lim}\) | \(T_v\) |
| 5 | \(-j_{\max}\) | ramps \(0 \to -a_{lim}\) | \(T_j\) |
| 6 | \(0\) | constant \(-a_{lim}\) | \(T_a - 2T_j\) |
| 7 | \(+j_{\max}\) | ramps \(-a_{lim} \to 0\) | \(T_j\) |
Phases 1–3 form the acceleration stage of length \(T_a\); phases 5–7 mirror it, so the profile is symmetric about its midpoint and the total time is \(T = 2T_a + T_v\). Integrating the stage gives the peaks and the distance:
\[a_{lim} = j_{\max} T_j, \qquad v_{lim} = a_{lim}\,(T_a - T_j), \qquad h = v_{lim}\,(T_a + T_v)\]
This is the case \(v_0 = v_1 = 0\) of Biagiotti and Melchiorri, §3.4. Each step assumes one more ceiling is unreachable.
Step 1 — assume \(v_{\max}\) is reached (\(v_{lim} = v_{\max}\)):
\[ \text{if } v_{\max} j_{\max} \ge a_{\max}^2:\; T_j = \frac{a_{\max}}{j_{\max}},\; T_a = T_j + \frac{v_{\max}}{a_{\max}} \qquad\text{else}\qquad T_j = \sqrt{\frac{v_{\max}}{j_{\max}}},\; T_a = 2T_j \]
\[T_v = \frac{h}{v_{\max}} - T_a\]
The first case has an acceleration plateau; the second has a pure triangular ramp with peak \(j_{\max}T_j < a_{\max}\). Here \(T = h/v_{\max} + T_a\), so the jerk limit costs exactly \(a_{\max}/j_{\max}\) over a trapezoid with the same \(v_{\max}, a_{\max}\).
Step 2 — if \(T_v < 0\), set \(T_v = 0\) and assume \(a_{\max}\) is still reached, \(T_j = a_{\max}/j_{\max}\). Then \(h = a_{\max} T_a (T_a - T_j)\) is a quadratic in \(T_a\):
\[\Delta = \frac{a_{\max}^4}{j_{\max}^2} + 4\,a_{\max} h, \qquad T_a = \frac{a_{\max}^2/j_{\max} + \sqrt{\Delta}}{2\,a_{\max}}\]
Step 3 — if \(T_a < 2T_j\), the plateau would have negative length, so \(a_{\max}\) is unreachable too. Both ramps are triangles, \(T_a = 2T_j\) and \(h = 2 j_{\max} T_j^3\):
\[T_j = \sqrt[3]{\frac{h}{2\,j_{\max}}}, \qquad T_a = 2T_j\]
Step 2 applies when \(h < v_{\max} T_a\), and step 3 when \(h < 2 a_{\max}^3 / j_{\max}^2\) (from \(T_a \ge 2T_j \iff \Delta \ge 9 a_{\max}^4/j_{\max}^2\)). Neighbouring branches agree on \(T\) at each threshold, so \(T\) is continuous in \(h\). No duration is ever negative: in step 1, \(T_a - 2T_j = v_{\max}/a_{\max} - a_{\max}/j_{\max} \ge 0\) is the branch condition itself; step 2 is guarded by the step-3 test; step 3 has \(T_a - 2T_j = 0\). Zero-length phases collapse out of the profile.
Within a phase of jerk \(j_i\) starting from state \((s_i, v_i, a_i)\), with local time \(\tau\):
\[a = a_i + j_i\tau, \qquad v = v_i + a_i\tau + \tfrac12 j_i\tau^2, \qquad s = s_i + v_i\tau + \tfrac12 a_i\tau^2 + \tfrac16 j_i\tau^3\]
The seven boundary states are integrated exactly once, at planning time, with these polynomials. Each sample then locates its phase and evaluates a cubic in time. At and beyond \(T\) the profile holds the goal at rest; before \(t = 0\) it holds the start. A negative move direction negates the offset, velocity, acceleration and jerk.
Replanning with \((v_{\max}/\lambda,\; a_{\max}/\lambda^2,\; j_{\max}/\lambda^3)\) stretches an axis by \(\lambda \ge 1\). Every planner term scales consistently (\(T_j \to \lambda T_j\), \(T_a \to \lambda T_a\), \(\Delta \to \lambda^{-2}\Delta\)) and the branch tests are invariant, so the duration scales by exactly \(\lambda\) in every branch. Axes stretched to a common \(T\) finish together.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Plan | \(O(1)\) | \(O(1)\) | At most one square root and one cube root, no iteration |
| Sample | \(O(1)\) | \(O(1)\) | Locate one of seven phases, evaluate one cubic |
Stored state is a few fixed-size tables of phase boundaries on the stack; no heap is used. Cost is independent of \(h\) and of the branch taken.
Limits \((v_{\max}, a_{\max}, j_{\max}) = (1, 2, 10)\), \(h = 0.3\).
Boundary states \((a, v, s)\) of the acceleration stage: at \(t = 0.2\), \((2,\, 0.2,\, 0.0133)\); at \(t = 0.3\), \((2,\, 0.4,\, 0.0433)\); at \(t = 0.5\), \((0,\, 0.6,\, 0.15)\). The mirrored stage ends at \(s = 0.3\), at rest. A sample at \(t = 0.4\) lies in phase 3 with \(\tau = 0.1\): \(a = 1\), \(v = 0.55\), \(s = 0.0917\).
| \((v_{\max}, a_{\max}, j_{\max})\) | \(h\) | Branch | \(T_j\) | \(T_a\) | \(T_v\) | \(T\) |
|---|---|---|---|---|---|---|
| (1, 2, 10) | 5 | 1, plateau | 0.2 | 0.7 | 4.3 | 5.7 |
| (1, 2, 1) | 5 | 1, triangle | 1 | 2 | 3 | 7 |
| (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 |
| (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 |
For \((1, 2, 10)\) the thresholds are \(h \ge 0.7\) for step 1 and \(h \ge 2a_{\max}^3/j_{\max}^2 = 0.16\) for step 2. The last row has \(a_{lim} = 1\) and \(v_{lim} = 0.1\).
| Algorithm | Relationship |
|---|---|
| Trapezoidal profile | Jerk-unbounded predecessor; shares the synchronization scheme |
| Quintic polynomial profile | Fixed duration and smooth, but limits are not enforced directly |
Gaussian elimination is the standard direct method for solving a system of linear equations \(Ax = b\). It transforms the coefficient matrix into upper-triangular form through systematic row operations, then solves via back-substitution.
It serves as the foundational linear solver in this library — used inside the normal equation solver for Linear Regression, inside the DARE iteration for LQR, and anywhere else a small dense linear system arises at runtime.
The implementation uses partial pivoting (selecting the largest-magnitude entry in each column as pivot) to improve numerical stability.
Solve \(Ax = b\) where \(A \in \mathbb{R}^{n \times n}\) is non-singular, \(b \in \mathbb{R}^n\).
For each column \(k = 0, \ldots, n-1\):
\[\ell_{ik} = \frac{A_{ik}}{A_{kk}}\] \[A_{ij} \leftarrow A_{ij} - \ell_{ik} \cdot A_{kj}, \quad j = k, \ldots, n-1\] \[b_i \leftarrow b_i - \ell_{ik} \cdot b_k\]
After all columns are processed, \(A\) is upper-triangular: \(Ux = b'\).
\[x_i = \frac{b'_i - \sum_{j=i+1}^{n-1} U_{ij} \, x_j}{U_{ii}}, \quad i = n-1, \ldots, 0\]
For \(AX = B\) where \(B\) has \(m\) columns, each column is solved independently.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Forward elimination | \(O(n^3)\) | \(O(n^2)\) | In-place on copies of \(A\) and \(b\) |
| Back-substitution | \(O(n^2)\) | \(O(n)\) | |
| Multi-column solve | \(O(n^3 + n^2 m)\) | \(O(n^2)\) | \(m\) back-substitutions |
Why \(O(n^3)\): The elimination has \(n\) stages; stage \(k\) performs \((n-k)^2\) multiplications. Summing: \(\sum_{k=1}^{n} k^2 \approx n^3/3\).
System:
\[\begin{bmatrix} 2 & 1 & -1 \\ -3 & -1 & 2 \\ -2 & 1 & 2 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix} = \begin{bmatrix} 8 \\ -11 \\ -3 \end{bmatrix}\]
Step 1 — Column 0: pivot selection
\(|A_{00}| = 2\), \(|A_{10}| = 3\) (largest), \(|A_{20}| = 2\). Swap rows 0 and 1:
\[\begin{bmatrix} -3 & -1 & 2 \\ 2 & 1 & -1 \\ -2 & 1 & 2 \end{bmatrix}, \quad b = \begin{bmatrix} -11 \\ 8 \\ -3 \end{bmatrix}\]
Step 2 — Eliminate below pivot
Step 3 — Column 1: pivot selection
\(|1/3| < |5/3|\). Swap rows 1 and 2.
Step 4 — Eliminate below:
\(\ell = (1/3)/(5/3) = 1/5\). Row 2 -= \((1/5)\) × Row 1 → \([0,\; 0,\; 1/5 \mid -1/5]\). Upper-triangular form reached.
Step 5 — Back-substitution:
\[x_3 = \frac{-1/5}{1/5} = -1, \qquad x_2 = \frac{13/3 - (2/3)(-1)}{5/3} = 3, \qquad x_1 = \frac{-11 - (-1)(3) - 2(-1)}{-3} = 2\]
Verification: \(2(2) + 1(3) + (-1)(-1) = 8\) ✓, \(\;-3(2) - 1(3) + 2(-1) = -11\) ✓, \(\;-2(2) + 1(3) + 2(-1) = -3\) ✓.
| Variant | Key Difference |
|---|---|
| Full pivoting | Pivots on both rows and columns; more stable but rarely needed in practice |
| LU factorization | Stores the \(L\) and \(U\) factors for reuse; solves multiple right-hand sides efficiently |
| Cholesky factorization | Specialized for symmetric positive-definite matrices; \(O(n^3/6)\) instead of \(O(n^3/3)\) |
| Gauss-Jordan elimination | Reduces to reduced row echelon form (identity matrix); used for matrix inversion |
| Iterative refinement | Solves once, then iteratively corrects the residual to improve accuracy |
graph LR
GE["Gaussian Elimination"]
LR["Linear Regression"]
DARE["DARE Solver"]
LQR["LQR Controller"]
LD["Levinson-Durbin"]
LR --> GE
DARE --> GE
LD -.->|"Toeplitz-specialized alternative"| GE| Algorithm | Relationship |
|---|---|
| Linear Regression | Uses Gaussian elimination to solve the normal equation |
| DARE Solver | Calls Gaussian elimination at each Riccati iteration |
| Levinson-Durbin | Specialized \(O(n^2)\) solver for Toeplitz systems; Gaussian elimination is the \(O(n^3)\) fallback |
Many problems in signal processing produce linear systems with Toeplitz structure — each descending diagonal contains the same value. Autocorrelation matrices, for example, are always Toeplitz and symmetric. General solvers like Gaussian elimination ignore this structure and cost \(O(n^3)\).
The Levinson-Durbin algorithm exploits the Toeplitz structure to solve such systems in \(O(n^2)\) time and \(O(n)\) auxiliary space. It works by solving an order-1 system first, then growing the solution to order 2, 3, …, up to \(n\), reusing previous solutions at each step via a recursion involving reflection coefficients.
This algorithm is central to AR model estimation (Yule-Walker) and linear prediction in speech processing.
A symmetric Toeplitz matrix and the system to solve:
\[\begin{bmatrix} r_0 & r_1 & r_2 & \cdots & r_{n-1} \\ r_1 & r_0 & r_1 & \cdots & r_{n-2} \\ r_2 & r_1 & r_0 & \cdots & r_{n-3} \\ \vdots & & & \ddots & \vdots \\ r_{n-1} & r_{n-2} & r_{n-3} & \cdots & r_0 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_n \end{bmatrix} = \begin{bmatrix} b_1 \\ b_2 \\ \vdots \\ b_n \end{bmatrix}\]
Define \(\varphi^{(k)}\) as the solution of the order-\(k\) system. The algorithm computes:
Reflection coefficient at order \(k\):
\[\mu_k = \frac{b_k - \sum_{j=0}^{k-1} \varphi_j^{(k-1)} \, r_{k-j}}{e_{k-1}}\]
Solution update:
\[\varphi_j^{(k)} = \varphi_j^{(k-1)} + \mu_k \, \varphi_{k-1-j}^{(k-1)}, \quad 0 \leq j < k\] \[\varphi_k^{(k)} = \mu_k\]
Prediction error update:
\[e_k = e_{k-1}(1 - \mu_k^2)\]
Starting from \(e_0 = r_0\) and \(\varphi_0^{(0)} = b_0 / r_0\).
If \(|\mu_k| < 1\) for all \(k\), the corresponding AR model is stable (all poles inside the unit circle) and the Toeplitz matrix is positive definite.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(n^2)\) | \(O(n)\) | Versus \(O(n^3)\) for general solvers |
Why \(O(n^2)\): At order \(k\), computing \(\mu_k\) costs \(O(k)\) and updating the solution costs \(O(k)\). Summing over \(k = 1, \ldots, n\): \(\sum k = n(n+1)/2 = O(n^2)\).
System: \(R x = b\) where \(r = [4, 2, 1]\), \(b = [2, 1, 0.5]\), \(n = 3\).
\[\begin{bmatrix} 4 & 2 & 1 \\ 2 & 4 & 2 \\ 1 & 2 & 4 \end{bmatrix} x = \begin{bmatrix} 2 \\ 1 \\ 0.5 \end{bmatrix}\]
Step 0 — Initialize:
\(e_0 = r_0 = 4\), \(\varphi_0^{(0)} = b_0 / r_0 = 2/4 = 0.5\)
Step 1 — Order 2:
\(\mu_1 = \frac{b_1 - \varphi_0^{(0)} \cdot r_1}{e_0} = \frac{1 - 0.5 \cdot 2}{4} = \frac{0}{4} = 0\)
\(\varphi_0^{(1)} = \varphi_0^{(0)} + 0 \cdot \varphi_0^{(0)} = 0.5\), \(\varphi_1^{(1)} = 0\)
\(e_1 = 4(1 - 0) = 4\)
Step 2 — Order 3:
\(\mu_2 = \frac{b_2 - (\varphi_0^{(1)} r_2 + \varphi_1^{(1)} r_1)}{e_1} = \frac{0.5 - (0.5 \cdot 1 + 0 \cdot 2)}{4} = \frac{0}{4} = 0\)
\(\varphi^{(2)} = [0.5, 0, 0]\)
Solution: \(x = [0.5, 0, 0]^T\)
Verification: \(4(0.5) + 2(0) + 1(0) = 2\) ✓, \(2(0.5) + 4(0) + 2(0) = 1\) ✓
| Variant | Key Difference |
|---|---|
| Split Levinson | Replaces the two-vector recursion with a single vector; slightly more efficient |
| Block Levinson | Solves block-Toeplitz systems (matrix entries instead of scalars) |
| Burg’s method | Computes reflection coefficients from data rather than autocovariances; often more accurate for short records |
| Schur algorithm | Computes reflection coefficients without forming the full solution; useful for VLSI implementations |
| Superfast Toeplitz solvers | \(O(n \log^2 n)\) algorithms based on FFT and divide-and-conquer |
graph LR
LD["Levinson-Durbin"]
YW["Yule-Walker"]
GE["Gaussian Elimination"]
PSD["Power Spectral Density"]
YW --> LD
GE -.->|"general alternative"| LD
LD -.->|"parametric PSD"| PSD| Algorithm | Relationship |
|---|---|
| Yule-Walker | Primary consumer — produces the Toeplitz system that Levinson-Durbin solves |
| Gaussian Elimination | General \(O(n^3)\) alternative that ignores Toeplitz structure |
| Power Spectral Density | Levinson-Durbin enables parametric PSD estimation as an alternative to Welch’s non-parametric method |
Given a polynomial \(P(z) = c_0 z^n + c_1 z^{n-1} + \cdots + c_n\), finding all roots (real and complex) is a fundamental problem in control theory, signal processing, and numerical analysis. Analytical formulas exist only for degree \(\leq 4\); beyond that, iterative methods are required.
The Durand-Kerner method (also known as the Weierstrass iteration) simultaneously refines approximations to all roots at once. Each root estimate is updated using Newton’s method, but instead of computing the derivative of \(P\), the other root estimates are used to factor out known roots. This avoids polynomial deflation — a process that accumulates errors as roots are extracted sequentially.
The method is elegant, easy to implement, and works for polynomials with complex coefficients and complex roots.
Given current root estimates \(z_0, z_1, \ldots, z_{n-1}\), each is updated simultaneously:
\[z_r^{(k+1)} = z_r^{(k)} - \frac{P(z_r^{(k)})}{\displaystyle\prod_{j \neq r} \left(z_r^{(k)} - z_j^{(k)}\right)}\]
For a polynomial \(P(z) = c_0 \prod_{j=0}^{n-1}(z - z_j)\), the derivative is:
\[P'(z_r) = c_0 \prod_{j \neq r}(z_r - z_j)\]
So the Durand-Kerner update is exactly Newton’s: \(z_r - P(z_r)/P'(z_r)\), but with \(P'\) approximated using the current root estimates rather than computed from coefficients.
Roots are initialized on a circle of radius:
\[R = \left|\frac{c_n}{c_0}\right|^{1/n}\]
with angular positions offset by 0.4 radians to break symmetry. If \(R < 0.1\), it defaults to 1.0.
| Case | Time | Space | Notes |
|---|---|---|---|
| Per iteration | \(O(n^2)\) | \(O(n)\) | Evaluating \(P\) and the denominator product for all \(n\) roots |
| Total | \(O(n^2 k)\) | \(O(n)\) | \(k\) iterations until convergence (typically \(k \ll n\)) |
Why \(O(n^2)\): For each of the \(n\) roots, computing the denominator product requires multiplying \(n-1\) terms.
Polynomial: \(P(z) = z^3 - 6z^2 + 11z - 6 = (z-1)(z-2)(z-3)\)
Step 1 — Initialize on radius \(R = |{-6}/{1}|^{1/3} = 6^{1/3} \approx 1.817\):
Step 2 — Iteration 1 (for \(z_0\)):
Step 3 — Repeat for \(z_1\) and \(z_2\) (using the latest values).
Step 4 — Iterate until \(\max_r |z_r^{(k+1)} - z_r^{(k)}| < 10^{-6}\).
After ~15–20 iterations the roots converge to \(z \approx \{1, 2, 3\}\) (imaginary parts \(< 10^{-6}\)).
math::Complex<T> (from
numerical/math/ComplexNumber.hpp), so it is limited to
floating-point types (float, double).
Fixed-point types are not supported.| Variant | Key Difference |
|---|---|
| Aberth-Ehrlich method | Similar simultaneous iteration but includes a correction term that improves convergence for clustered roots |
| Jenkins-Traub | Sequential algorithm (one root at a time) with superior convergence for difficult polynomials; standard in numerical libraries |
| Companion matrix + eigenvalues | Converts to an eigenvalue problem; robust but \(O(n^3)\) |
| Laguerre’s method | Converges cubically for simple roots; sequential (extracts one root at a time, then deflates) |
| Muller’s method | Uses quadratic interpolation; works for non-polynomial equations too |
graph LR
DK["Durand-Kerner"]
RL["Root Locus"]
GE["Gaussian Elimination"]
RL --> DK
GE -.->|"companion matrix alternative"| DK| Algorithm | Relationship |
|---|---|
| Root Locus | Primary consumer — calls Durand-Kerner to find characteristic polynomial roots at each gain step |
| Gaussian Elimination | Alternative approach: form the companion matrix and compute eigenvalues (requires a different solver) |
The Discrete Algebraic Riccati Equation arises whenever you need to find an optimal trade-off between state regulation and control effort in a discrete-time linear system. It is the mathematical core of two fundamental algorithms:
The DARE finds a symmetric positive semi-definite matrix \(P\) that encodes the cost-to-go from any state. Once \(P\) is known, the optimal gain is a simple closed-form expression. This library solves the DARE via fixed-point iteration — simple to implement, numerically transparent, and well-suited to the small state dimensions typical in embedded control.
\[P = A^T P A - A^T P B \left(R + B^T P B\right)^{-1} B^T P A + Q\]
where: - \(A \in \mathbb{R}^{n \times n}\) — state transition matrix - \(B \in \mathbb{R}^{n \times m}\) — input matrix - \(Q \in \mathbb{R}^{n \times n}\) — state weighting matrix (\(Q \succeq 0\)) - \(R \in \mathbb{R}^{m \times m}\) — input weighting matrix (\(R \succ 0\)) - \(P \in \mathbb{R}^{n \times n}\) — the unknown (\(P \succeq 0\))
Starting from \(P_0 = Q\):
A unique stabilizing solution exists when: - \((A, B)\) is stabilizable (unstable modes are controllable). - \((A, C)\) is detectable (where \(Q = C^T C\)).
Under these conditions, the iteration converges monotonically: \(P_0 \preceq P_1 \preceq \cdots \preceq P^*\).
\[K = \left(R + B^T P B\right)^{-1} B^T P A\]
| Phase | Time | Space | Notes |
|---|---|---|---|
| Per iteration | \(O(n^3 + n^2 m + m^3)\) | \(O(n^2)\) | Matrix multiplications + one \(m \times m\) linear solve |
| Total solver | \(O(I(n^3 + m^3))\) | \(O(n^2)\) | \(I\) iterations (typically 10–30) |
Why \(O(n^3)\): Each iteration involves \(n \times n\) matrix products (\(A^T P A\)) and an \(m \times m\) linear solve (for \(S^{-1}\)). When \(m \ll n\), the matrix products dominate.
System: \(n=2\), \(m=1\), \(A = \begin{bmatrix}1 & 0.1\\0 & 1\end{bmatrix}\), \(B = \begin{bmatrix}0.005\\0.1\end{bmatrix}\), \(Q = I_2\), \(R = [1]\).
Iteration 0: \(P_0 = Q = I_2\)
Iterations 1–20: \(P\) grows monotonically until convergence. For this \(Q/R\) ratio, \(P^*\) stabilizes in ~15 iterations.
math::Tolerance<T>(), which adapts to both
float and fixed-point types.| Variant | Key Difference |
|---|---|
| Continuous ARE (CARE) | For continuous-time systems; different matrix equation |
| Newton’s method for DARE | Quadratic convergence but requires solving a Sylvester equation per iteration |
| Schur method | Eigendecomposition-based; finds the solution in one pass but requires \(O(n^3)\) eigenvalue computation |
| Doubling algorithm | \(O(\log I)\) convergence via matrix sign function; useful for large systems |
graph LR
DARE["DARE Solver"]
GE["Gaussian Elimination"]
LQR["LQR Controller"]
KF["Kalman Filter"]
GE --> DARE
DARE --> LQR
DARE --> KF| Algorithm | Relationship |
|---|---|
| Gaussian Elimination | Used at each iteration to solve the \(m \times m\) linear sub-system |
| LQR Controller | Direct consumer — extracts optimal gain \(K\) from the DARE solution matrix \(P\) |
| Kalman Filter | Dual problem — the steady-state Kalman gain is computed by a transposed DARE |
Ordinary differential equations of the form \(\dot{x} = f(x, u, t)\) arise throughout embedded control and dynamics — propagating plant models for prediction, running model-based observers, or performing hardware-in-the-loop simulation on the device itself.
RK4 (the classical fourth-order Runge-Kutta method) solves this problem with a fixed step size, producing deterministic, constant-work-per-tick execution. It is the natural choice for hard-real-time control loops.
Dormand-Prince RK45 is an embedded pair that computes both a 4th- and a 5th-order estimate from the same seven slope evaluations, then uses their difference as a cheap local error gauge. The step-size controller shrinks the step when the estimated error is too large and grows it when the solution is smooth — adapting accuracy to computational budget without user intervention. It is suited for offline simulation or hardware-in-the-loop testing where timing determinism is less critical than accuracy.
Given \(\dot{x} = f(x, u, t)\) with \(x(t_0) = x_0\), advance the state by one step from \(t\) to \(t + h\).
\[ k_1 = f(x_n, u, t_n) \] \[ k_2 = f\!\left(x_n + \tfrac{h}{2}k_1,\ u,\ t_n + \tfrac{h}{2}\right) \] \[ k_3 = f\!\left(x_n + \tfrac{h}{2}k_2,\ u,\ t_n + \tfrac{h}{2}\right) \] \[ k_4 = f\!\left(x_n + h\,k_3,\ u,\ t_n + h\right) \] \[ x_{n+1} = x_n + \frac{h}{6}\left(k_1 + 2k_2 + 2k_3 + k_4\right) \]
The weights \((1, 2, 2, 1)/6\) are derived by matching the Taylor series of the exact solution through fourth order. The local truncation error is \(O(h^5)\); the global error is \(O(h^4)\).
Dormand and Prince (1980) selected a 7-stage Butcher tableau whose 5th-order propagator \(y_5\) and 4th-order embedded propagator \(y_4\) share stages \(k_1, \ldots, k_6\), with \(k_7 = f(y_5, u, t+h)\) added only for the 4th-order correction and for FSAL reuse.
The 5th-order solution used to advance the state:
\[ y_5 = x_n + h\left(\frac{35}{384}k_1 + \frac{500}{1113}k_3 + \frac{125}{192}k_4 - \frac{2187}{6784}k_5 + \frac{11}{84}k_6\right) \]
The 4th-order embedded solution used only for error estimation:
\[ y_4 = x_n + h\left(\frac{5179}{57600}k_1 + \frac{7571}{16695}k_3 + \frac{393}{640}k_4 - \frac{92097}{339200}k_5 + \frac{187}{2100}k_6 + \frac{1}{40}k_7\right) \]
The mixed absolute/relative weighted RMS norm over all \(n_s\) state components:
\[ \text{err} = \sqrt{\frac{1}{n_s} \sum_{i=1}^{n_s} \left(\frac{(y_5 - y_4)_i}{\text{atol} + \text{rtol}\,|x_i|}\right)^2} \]
A step is accepted when \(\text{err} \le 1\). The next step size is:
\[ h_{\text{new}} = h \cdot \text{clamp}\!\left(0.9 \cdot \text{err}^{-1/5},\ 0.2,\ 5\right) \]
clamped further to \([h_{\min}, h_{\max}]\).
The 7th stage \(k_7 = f(y_5, u, t+h)\) equals the first stage of the next accepted step. Caching it reduces each accepted step from 7 to 6 function evaluations.
| Integrator | RHS evaluations per accepted step | State memory |
|---|---|---|
| RK4 (fixed) | 4 (always) | \(O(n_s)\) |
| Dormand-Prince | 6 (with FSAL), 7 on first step | \(O(n_s)\) |
All intermediate stage vectors are stack-allocated. No heap is used. The cost of one step is \(O(s \cdot n_s)\) where \(s\) is the stage count plus the cost of evaluating \(f\).
Scalar decay \(\dot{x} = -x\), \(x(0) = 1\), exact solution \(x(t) = e^{-t}\), \(h = 0.1\):
| Stage | Formula | Value |
|---|---|---|
| \(k_1\) | \(f(1, 0) = -1\) | \(-1\) |
| \(k_2\) | \(f(1 - 0.05, 0.05) = -0.95\) | \(-0.95\) |
| \(k_3\) | \(f(1 - 0.0475, 0.05) = -0.9525\) | \(-0.9525\) |
| \(k_4\) | \(f(1 - 0.09525, 0.1) = -0.90475\) | \(-0.90475\) |
| \(x_1\) | \(1 + (0.1/6)(-1 - 1.9 - 1.905 - 0.90475)\) | \(\approx 0.90484\) |
Exact: \(e^{-0.1} \approx 0.90484\). Agreement to six significant figures — consistent with \(O(h^5)\) local error.
Step; a zero step produces an unchanged state but wastes
evaluations.| Variant | Key Difference |
|---|---|
| Euler (1st order) | One stage; \(O(h)\) global error; useful only for rough prototyping |
| RK4 (this) | Four stages; \(O(h^4)\) global error; standard fixed-step workhorse |
| Dormand-Prince (this) | Seven stages; \(O(h^5)\) propagator with built-in \(O(h^4)\) error estimate |
| Bogacki-Shampine RK23 | Three-stage embedded pair; lower overhead for mildly stiff or smooth problems |
| Adams-Bashforth | Multi-step; reuses past evaluations; efficient but requires startup phase |
| Implicit RK / SDIRK | Solves a nonlinear system at each stage; suitable for stiff problems at the cost of a linear solve per step |
dynamics/) forward in time for prediction horizons in MPC
or trajectory planning.graph LR
RK["RK4 / Dormand-Prince"]
DYN["dynamics/ (Euler-Lagrange, Newton-Euler)"]
EKF["Extended Kalman Filter"]
MPC["MPC Controller"]
C2D["ContinuousToDiscrete"]
DYN --> RK
RK --> EKF
RK --> MPC
C2D -.->|"exact linear alternative"| RK| Algorithm | Relationship |
|---|---|
dynamics/ models |
Provide the right-hand side \(f(x, u, t)\) that RK integrates |
| Extended Kalman Filter | Uses RK to propagate the state prediction step between measurements |
| MPC Controller | Uses RK to simulate the plant over a prediction horizon |
| ContinuousToDiscrete | Exact matrix-exponential discretization — an alternative for linear, time-invariant systems |
The spectral radius of a square matrix is the largest magnitude among all its eigenvalues. In discrete-time dynamical systems, the spectral radius of the state-transition matrix is the single most important scalar quantity: if it is strictly less than one, every state trajectory converges to zero; if it equals or exceeds one, at least one mode grows without bound or fails to decay. Rather than reporting only a binary stable/unstable flag, exposing the spectral radius and the complementary stability margin (one minus the spectral radius) gives the designer a quantitative handle on how much gain or perturbation the system can tolerate before losing stability.
Given an \(n \times n\) matrix \(A\), the characteristic polynomial is
\[p(\lambda) = \det(\lambda I - A) = \lambda^n + c_{n-1}\lambda^{n-1} + \cdots + c_1\lambda + c_0.\]
The Faddeev–LeVerrier recurrence computes the coefficients \(c_k\) without determinant expansion. Define auxiliary matrices \(M_k\) by
\[M_1 = A, \quad c_{n-1} = -\mathrm{tr}(M_1),\] \[M_k = A\bigl(M_{k-1} + c_{n-k+1}I\bigr), \quad c_{n-k} = -\frac{1}{k}\mathrm{tr}(M_k), \quad k = 2, \ldots, n.\]
This recurrence requires only matrix-matrix multiplication, scalar multiplication of a matrix by the identity, and the trace, making it suitable for static (stack-allocated) computation without a heap.
Given the full set of eigenvalues \(\{\lambda_i\}_{i=1}^{n}\) (roots of the characteristic polynomial),
\[\rho(A) = \max_{i} |\lambda_i|.\]
The roots are found numerically with the Durand–Kerner simultaneous root-finding iteration, which converges for arbitrary polynomials and requires only a fixed-size array of complex iterates.
A matrix \(A\) is Schur-stable (the discrete-time analogue of Hurwitz stability) if and only if \(\rho(A) < 1\). Every eigenvalue then lies strictly inside the complex unit disk, guaranteeing exponential decay of all modes.
\[\delta(A) = 1 - \rho(A).\]
A positive margin indicates Schur stability; \(\delta = 0\) corresponds to marginal stability (an eigenvalue on the unit circle); \(\delta < 0\) signals instability. The margin quantifies how far \(\rho(A)\) is from the stability boundary.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(n^3)\) | \(O(n^2)\) | Faddeev–LeVerrier dominates; two \(n\times n\) temporaries on the stack |
| Average | \(O(n^3 + n \cdot I)\) | \(O(n^2 + n)\) | \(I\) Durand–Kerner iterations, typically \(\ll 200\) |
| Worst | \(O(n^3 + n \cdot I_{\max})\) | \(O(n^2 + n)\) | Near-repeated roots slow DK convergence |
The Faddeev–LeVerrier step executes \(n\) matrix multiplications each costing \(O(n^3)\), giving \(O(n^4)\) total; for the small dimensions typical of embedded control matrices (\(n \le 6\)) this is negligible.
Consider \(A = \begin{pmatrix} 0 & -1 \\ 1 & 0 \end{pmatrix}\) (90° rotation, \(\rho = 1\)).
Characteristic polynomial: \(\lambda^2 + 0\lambda + 1 = \lambda^2 + 1\). Roots: \(\pm i\), magnitudes both 1. \(\rho = 1\).
A matrix can be mathematically invertible yet catastrophically sensitive to perturbations in its entries or in the right-hand side of a linear system. This sensitivity, called ill-conditioning, is the root cause of accumulated floating-point error in solvers, regression, and Kalman-filter covariance updates. Measuring it before committing to a solve reveals whether the result can be trusted or whether regularisation, pivoting, or reformulation is warranted.
The condition number is the canonical scalar summary of
ill-conditioning. Because it requires the norm of the inverse, it
depends on a linear solver — which is why it lives in the
solvers layer, on top of the math norms,
rather than in math itself.
For an invertible square matrix \(A \in \mathbb{R}^{N \times N}\) and a chosen matrix norm \(\|\cdot\|\):
\[\kappa(A) = \|A\| \cdot \|A^{-1}\|\]
The condition number bounds the relative error amplification in the solution \(\mathbf{x}\) of \(A\mathbf{x} = \mathbf{b}\) due to a perturbation \(\delta\mathbf{b}\):
\[\frac{\|\delta\mathbf{x}\|}{\|\mathbf{x}\|} \le \kappa(A) \cdot \frac{\|\delta\mathbf{b}\|}{\|\mathbf{b}\|}\]
\(\kappa(A) \ge 1\) always; \(\kappa(A) = 1\) only for scalar multiples of orthogonal matrices. A singular matrix has \(\kappa(A) = \infty\). The estimate here uses the 1-norm.
The norm of \(A^{-1}\) is obtained without forming an explicit inverse as a first-class object: solving \(AX = I\) column by column with partial-pivoting Gaussian elimination yields the inverse columns, from which the 1-norm is accumulated. This mirrors the embedded convention of reusing the existing solver rather than adding a dedicated inversion routine.
| Operation | Time | Space | Notes |
|---|---|---|---|
| ConditionNumber | \(O(N^3)\) | \(O(N^2)\) | Dominated by the \(N\)-column solve of \(AX = I\) |
Matrix \(A = \begin{bmatrix}3 & 1 \\ 1 & 2\end{bmatrix}\), 1-norm condition number:
Singular matrices — a row of all zeros makes the system unsolvable. The singularity check fires before entering Gaussian elimination, returning an empty result rather than dividing by zero.
Near-singular matrices — the condition number can be astronomically large without any row being identically zero. Partial-pivoting elimination still completes, and the returned value reflects the ill-conditioning faithfully.
Norm choice — the 1-norm condition number is a practical upper bound on the spectral condition number \(\kappa_2\); it is cheaper by orders of magnitude because it avoids the SVD.
The spectral condition number (\(\kappa_2\), ratio of largest to smallest singular value) is the tightest but requires an SVD. The reciprocal condition number (\(\text{RCOND} = 1/\kappa\)) avoids overflow when \(\kappa\) is very large; LAPACK-style solvers return this form and compare against machine epsilon. For positive-definite systems, Cholesky factorisation supplies the same column-wise solves at half the cost.
Gaussian elimination with partial pivoting
(solvers::GaussianElimination) supplies the column-wise
solves of \(AX = I\). The matrix 1-norm
(math::OneNorm) supplies both factors of the product.
Cholesky decomposition (solvers::CholeskyDecomposition) is
the positive-definite alternative for the inverse solve.
The QR decomposition factors a matrix \(A\) into an orthogonal factor \(Q\) and an upper-triangular factor \(R\). It is the numerically robust foundation for least-squares regression, eigenvalue computation, and square-root filtering. In embedded systems, fitting an overdetermined model (more sensor measurements than unknowns) arises constantly in calibration, system identification, and sensor fusion. Solving the least-squares problem via QR avoids forming the normal equations \(A^\top A x = A^\top b\), whose condition number is the square of \(A\)’s — a critical advantage when working with reduced word lengths.
For an \(m \times n\) matrix \(A\) with \(m \geq n\), the thin QR factorization is
\[A = Q R,\]
where \(Q \in \mathbb{R}^{m \times n}\) has orthonormal columns (\(Q^\top Q = I_n\)) and \(R \in \mathbb{R}^{n \times n}\) is upper triangular.
A Householder reflector is a symmetric, orthogonal matrix of the form
\[H = I - \beta v v^\top, \quad \beta = \frac{2}{\|v\|^2},\]
chosen so that \(H x = \sigma e_1\) for a target vector \(x\), where \(\sigma = -\mathrm{sign}(x_1)\|x\|\). The sign convention \(\sigma = -\mathrm{sign}(x_1)\|x\|\) avoids catastrophic cancellation when computing \(v_1 = x_1 - \sigma\).
Applying \(k\) successive reflectors \(H_k, \ldots, H_1\) to \(A\) triangularizes it:
\[H_n \cdots H_1 A = R \implies A = H_1 \cdots H_n R = QR.\]
The reflectors are stored implicitly: below the diagonal of the working matrix, alongside a scalar \(\beta_k\) per column, consuming only \(O(mn)\) words.
A Givens rotation
\[G(i,j,\theta) = \begin{pmatrix} c & s \\ -s & c \end{pmatrix}, \quad c = \cos\theta,\ s = \sin\theta\]
zeros a single sub-diagonal entry by rotating two rows. When one new row streams in after an existing factorization, \(n\) plane rotations update \(R\) in \(O(n^2)\) flops — far cheaper than re-factorizing the augmented matrix from scratch.
Given the factorization \(A = QR\), the least-squares minimum-norm solution is
\[x = R^{-1} Q^\top b,\]
computed as: (1) multiply \(b \leftarrow Q^\top b\) by applying the stored reflectors in order without forming \(Q\) explicitly, then (2) back-substitute \(R x = (Q^\top b)_{1:n}\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(mn^2)\) | \(O(mn)\) | Householder on a well-conditioned dense \(A\) |
| Average | \(O(2mn^2 - \tfrac{2}{3}n^3)\) | \(O(mn)\) | Standard flop count from Golub & Van Loan |
| Worst | same as average (no pivoting) | \(O(mn)\) | Rank-deficient case detected but not re-ordered |
| Update | \(O(n^2)\) | \(O(n^2)\) | Single Givens row update; no re-factor |
All memory is stack-allocated via std::array; the factor
is stored in-place over the input.
Factor \(A = \begin{pmatrix} 12 & -51 \\ 6 & 167 \\ -4 & 24 \end{pmatrix}\) (first two columns of the Golub & Van Loan example).
Column 0 (\(k=0\)): Extract \(x = (12, 6, -4)^\top\), \(\|x\| = 14\). Choose \(\sigma = -14\) (sign rule), \(v_0 = 12 + 14 = 26\), \(v = (26, 6, -4)^\top / 26\), \(\beta = 2/\|v\|^2\). After applying \(H_0\): \(R_{00} = 14\), zeros below.
Column 1 (\(k=1\)): Repeat on the \(2 \times 1\) trailing sub-column.
The upper-triangular \(R\) then has diagonal magnitudes \(\{14, 175\}\) for this sub-matrix.
ApplyQtranspose avoids this for solves.SingularValueDecomposition (Golub-Kahan) and the symmetric
eigenvalue solver (Jacobi / tridiagonal QR).GaussianElimination’s upper-triangular solve.LinearRegression for ill-conditioned problems.Many embedded control and estimation algorithms reduce to solving a dense linear system \(A x = b\) for a general (non-symmetric) matrix \(A\). LU decomposition with partial pivoting factors \(A\) as \(P A = L U\), where \(P\) is a permutation, \(L\) is unit lower-triangular, and \(U\) is upper-triangular. Factoring \(A\) once and reusing \(L\) and \(U\) turns every subsequent solve into two cheap triangular sweeps — a critical advantage in loops where the same plant or observer matrix must be inverted against many right-hand sides at different time steps.
For an \(n \times n\) matrix \(A\), partial-pivoting LU factorization produces
\[P A = L U,\]
where: - \(P \in \{0,1\}^{n \times n}\) is a permutation matrix encoding row swaps, - \(L \in \mathbb{R}^{n \times n}\) is unit lower-triangular (\(L_{ii} = 1\), \(L_{ij} = 0\) for \(j > i\)), - \(U \in \mathbb{R}^{n \times n}\) is upper-triangular (\(U_{ij} = 0\) for \(i > j\)).
At step \(k\) of the outer loop (\(k = 0, \ldots, n-1\)):
\[A_{ij} \leftarrow A_{ij} - \ell_{ik} A_{kj}, \quad j > k.\]
After \(n - 1\) steps the working matrix holds \(U\) on and above the diagonal and the multipliers \(\ell_{ik}\) below it, packed together in a single \(n \times n\) array.
Given \(P A = L U\) and a right-hand side \(b\), the system \(A x = b\) becomes \(L U x = P b\). Solving in two passes:
\[L y = P b \quad \text{(forward substitution)},\] \[U x = y \quad \text{(back substitution)}.\]
Both passes cost \(O(n^2)\)
operations, and both reuse the shared
math::SolveUnitLowerTriangular /
math::SolveUpperTriangular kernels (from math/TriangularSolve.hpp)
applied directly to the packed factor — the same triangular-solve
building blocks that back QrDecomposition.
The factored form is exposed for reuse by other components:
L(), U(), and P() return the
individual factors (so callers can verify \(P
A = L U\) or build derived quantities), and
IsSingular() reports whether the last
Decompose aborted.
The determinant of \(A\) equals the product of \(U\)’s diagonal entries multiplied by the sign of \(P\):
\[\det(A) = \mathrm{sign}(P) \prod_{k=0}^{n-1} U_{kk}.\]
The sign flips each time rows are swapped; it starts at \(+1\) and is multiplied by \(-1\) per swap.
The inverse is formed by solving \(A X = I\) column by column — \(n\) back-and-forward substitution pairs — at a total cost of \(O(n^3)\).
| Operation | Time | Space | Notes |
|---|---|---|---|
Decompose |
\(O\!\left(\tfrac{2}{3}n^3\right)\) | \(O(n^2)\) | GEPP in-place; multipliers packed with \(U\) |
Solve |
\(O(n^2)\) | \(O(n)\) | Two triangular sweeps; reuses stored factors |
Determinant |
\(O(n)\) | \(O(1)\) | One pass over \(U\)’s diagonal |
Inverse |
\(O(n^3)\) | \(O(n^2)\) | \(n\) column solves; avoid when
Solve suffices |
All storage is stack-allocated; the packed LU matrix and pivot vector are bounded by \(N\).
Factor \(A = \begin{pmatrix} 2 & 1 & -1 \\ -3 & -1 & 2 \\ -2 & 1 & 2 \end{pmatrix}\).
Step \(k = 0\): Largest magnitude in column 0 is \(|-3| = 3\) at row 1. Swap rows 0 and 1. Working matrix: \(\begin{pmatrix} -3 & -1 & 2 \\ 2 & 1 & -1 \\ -2 & 1 & 2 \end{pmatrix}\). Multipliers: \(\ell_{10} = 2/(-3) = -2/3\), \(\ell_{20} = -2/(-3) = 2/3\). After elimination: \(U_{0\cdot} = (-3,-1,2)\); rows 1 and 2 updated.
Step \(k = 1\): Select pivot in column 1 from rows 1 onward. Possibly swap. Compute multiplier \(\ell_{21}\) and update \(U_{2\cdot}\).
Step \(k = 2\): Single diagonal entry remains; \(U_{22}\) is read directly.
The resulting \(L\) has the stored multipliers below the diagonal (with unit diagonal), and \(U\) sits on and above it.
float), the matrix is (numerically)
rank-deficient. The factorization aborts and returns false;
the caller must not invoke Solve or Inverse. A
relative (rather than absolute) tolerance is essential: on a truly
singular matrix the trailing pivot is a rounding-error residual whose
magnitude scales with the matrix entries and varies across
platforms/fast-math settings, so an absolute cutoff would
miss it.Inverse
calls Solve \(n\) times
and is \(O(n^3)\); prefer
Solve for a single right-hand side to avoid the extra \(O(n^3)\) work and the conditioning penalty
of computing the inverse explicitly.CholeskyDecomposition instead — it is roughly twice as fast
and requires no pivoting.GaussianElimination performs the same GEPP steps but
does not retain the factored form, making it a single-shot solver;
LuDecomposition amortises the \(O(n^3)\) cost over multiple
Solve calls.CholeskyDecomposition is a specialised \(O\!\left(\tfrac{1}{3}n^3\right)\)
factorization for symmetric positive-definite matrices — roughly \(2\times\) faster and without the overhead
of pivoting.QrDecomposition is preferred for least-squares
(overdetermined) problems; for square full-rank systems, LU is
faster.ConditionNumber can be estimated cheaply after
factorization by examining the ratio \(\max_k
|U_{kk}| / \min_k |U_{kk}|\).The Sylvester equation \(AX + XB = C\) and its symmetric specialisations — the continuous Lyapunov \(AX + XA^\top = -Q\) and the discrete Lyapunov \(AXA^\top - X = -Q\) — are the central matrix equations of stability analysis, gramian computation, and robust-control synthesis. A positive-definite Lyapunov solution is a formal certificate that the system \(\dot{x} = Ax\) is asymptotically stable; the same equations produce the controllability and observability gramians that measure how effectively a linear system can be driven and observed.
Given matrices \(A \in \mathbb{R}^{N \times N}\), \(B \in \mathbb{R}^{M \times M}\), and \(C \in \mathbb{R}^{N \times M}\), the Sylvester equation is
\[AX + XB = C,\]
with unknown \(X \in \mathbb{R}^{N \times M}\). The equation has a unique solution if and only if \(A\) and \(-B\) share no eigenvalue, equivalently \(\lambda_i(A) + \lambda_j(B) \neq 0\) for all \(i, j\).
Stacking \(X\) column-by-column into \(\mathrm{vec}(X) \in \mathbb{R}^{NM}\) and applying the identities \(\mathrm{vec}(AXI) = (I_M \otimes A)\mathrm{vec}(X)\) and \(\mathrm{vec}(IXB) = (B^\top \otimes I_N)\mathrm{vec}(X)\) converts the matrix equation into the standard linear system
\[\underbrace{(I_M \otimes A + B^\top \otimes I_N)}_{K \in \mathbb{R}^{NM \times NM}} \mathrm{vec}(X) = \mathrm{vec}(C).\]
The \((iN+k, jN+l)\) entry of \(K\) is \(\delta_{ij} A_{kl} + B_{ji} \delta_{kl}\). The system is solved by Gaussian elimination with partial pivoting; a near-zero pivot signals the unsolvable (degenerate eigenvalue) case.
Setting \(B = A^\top\) and \(C = -Q\) reduces the Sylvester equation to the continuous Lyapunov form
\[AX + XA^\top = -Q.\]
When \(A\) is Hurwitz (all eigenvalues in the open left half-plane) and \(Q\) is symmetric positive semi-definite, the unique solution \(X\) is symmetric positive definite.
The discrete counterpart
\[AXA^\top - X = -Q\]
is solved by the Kronecker system \((A \otimes A - I_{N^2})\mathrm{vec}(X) = -\mathrm{vec}(Q)\), with the \((iN+k, jN+l)\) entry of \(A \otimes A - I\) equal to \(A_{ij}A_{kl} - \delta_{ij}\delta_{kl}\). When \(A\) is Schur-stable (\(\rho(A) < 1\)) and \(Q \succeq 0\), the unique solution equals the controllability gramian \(X = \sum_{k=0}^{\infty} A^k Q (A^\top)^k\).
| Case | Time | Space | Notes |
|---|---|---|---|
| \(N{=}M{=}2\) | \(O(64)\) flops | \(O(NM)^2 = 16\) floats for K | 4×4 linear system |
| \(N{=}M{=}3\) | \(O(729)\) flops | 81 floats for K | 9×9 linear system |
| General \(N{=}M\) | \(O(N^6)\) flops | \(O(N^4)\) words | Gaussian elimination on the \(N^2 \times N^2\) Kronecker system |
| Alternative (Bartels–Stewart) | \(O(N^3)\) | \(O(N^2)\) | Requires Schur decomposition; preferred for \(N \geq 6\) |
For the embedded sizes (\(N \leq 5\)) targeted by this library the Kronecker approach is negligibly more expensive than Bartels–Stewart and avoids the full Schur machinery.
Solve \(AX + XB = C\) for the \(2 \times 2\) case \(A = \bigl[\begin{smallmatrix}-3&1\\0&-2\end{smallmatrix}\bigr]\), \(B = \bigl[\begin{smallmatrix}1&2\\0&1\end{smallmatrix}\bigr]\), \(C = I_2\).
Build \(K\) (the \(4 \times 4\) Kronecker matrix):
\[K = \begin{pmatrix}-2&1&0&0\\0&-1&0&0\\2&0&-2&1\\0&2&0&-1\end{pmatrix}.\]
Build \(\mathrm{vec}(C) = (1, 0, 0, 1)^\top\).
Gaussian elimination with partial pivoting produces \(X_{00} = -0.5\), \(X_{10} = 0\), \(X_{01} = -1\), \(X_{11} = -1\).
Verify: \(AX + XB = I_2\). ✓
false and leaves the solution
unchanged.SquareRootKalmanFilter).LuDecomposition solver.The singular value decomposition (SVD) factors any \(m \times n\) matrix \(A\) into two orthonormal rotation factors and a non-negative diagonal, revealing the fundamental geometric action of \(A\) as a stretch along \(k = \min(m, n)\) orthogonal directions. It is the most numerically stable matrix factorization available and exposes rank, condition number, and the pseudo-inverse directly from its output. In embedded systems, SVD powers robust least-squares solvers for sensor calibration, redundant-actuator control allocation, and low-rank model reduction — all applications where the matrix \(A\) may be ill-conditioned or rank-deficient and normal-equation approaches fail.
For \(A \in \mathbb{R}^{m \times n}\) and \(k = \min(m, n)\), the thin SVD is
\[A = U \Sigma V^\top,\]
where \(U \in \mathbb{R}^{m \times k}\) and \(V \in \mathbb{R}^{n \times k}\) have orthonormal columns (\(U^\top U = V^\top V = I_k\)) and \(\Sigma = \mathrm{diag}(\sigma_1, \ldots, \sigma_k)\) with \(\sigma_1 \geq \cdots \geq \sigma_k \geq 0\). For a tall or square matrix (\(m \geq n\)) \(V\) is square and orthogonal; for a wide matrix (\(m < n\)) \(U\) is. The scalars \(\sigma_i\) are the singular values of \(A\); the columns of \(U\) and \(V\) are the left and right singular vectors.
The Golub–Kahan bidiagonalization below runs on a tall matrix. A wide \(A\) is handled through its transpose: factor \(A^\top = U' \Sigma V'^\top\) (tall), then \(A = V' \Sigma U'^\top\), so \(U = V'\) and \(V = U'\). The singular values are shared, and no extra storage is needed beyond the tall factors.
The singular values satisfy \(\sigma_i^2 = \lambda_i(A^\top A)\), where \(\lambda_i\) denotes the \(i\)-th eigenvalue of \(A^\top A\). This ties the SVD to the symmetric eigenproblem and provides a cross-check: computing the eigenvalues of \(A^\top A\) via the Jacobi or tridiagonal-QR solver should yield the squared singular values.
The Moore-Penrose pseudo-inverse is
\[A^+ = V \Sigma^+ U^\top, \quad \Sigma^+_{ii} = \begin{cases} 1/\sigma_i & \sigma_i > \tau \\ 0 & \text{otherwise} \end{cases}\]
for a threshold \(\tau > 0\). Truncating small singular values regularises the solution against near-null-space components, avoiding division by near-zero values that arise from rank-deficiency or numerical noise.
\[\mathrm{rank}(A) = \#\{i : \sigma_i > \tau\}, \quad \kappa(A) = \sigma_1 / \sigma_k.\]
The condition number \(\kappa\) measures sensitivity: a system with \(\kappa \gg 1\) amplifies input perturbations by a factor of \(\kappa\) in the least-squares solution.
| Phase | Time | Space | Notes |
|---|---|---|---|
| Bidiagonalization | \(O(Mk^2 - k^3/3)\) | \(O(mn)\) | Two-sided Householder reflectors |
| QR sweeps | \(O(k^2)\) per iteration, \(O(k)\) total iterations | \(O(k^2)\) | Golub-Reinsch implicit-shift; converges fast |
| Pseudo-inverse | \(O(k\,mn)\) | \(O(nm)\) | Matrix triple product \(V \Sigma^+ U^\top\) |
| Total | \(O(Mk^2 + k^3)\) | \(O(mn)\) | Dominated by bidiagonalization |
Here \(M = \max(m, n)\) and \(k = \min(m, n)\): a wide matrix costs the same as its transpose.
All storage is stack-allocated; no heap is used. The bidiagonal form is stored implicitly in the working copy of \(A\) alongside the Householder reflector scalars.
Input: \(A = \begin{pmatrix} 3 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 2 \end{pmatrix}\) (diagonal matrix).
Phase 1 — Bidiagonalization: \(A\) is already diagonal (and hence upper-bidiagonal). All Householder reflectors are trivial (\(\beta = 0\)). The bidiagonal form retains \(B = A\); \(U = I_3\), \(V = I_3\).
Phase 2 — QR sweeps: The superdiagonal is already zero, so the sweep loop exits immediately.
Sign correction and sort: \(\sigma = (3, 1, 2)\) after reading the diagonal; the entry \(-1\) has a negative sign, so it is flipped to \(+1\) and the corresponding column of \(U\) is negated. After descending sort: \(\sigma = (3, 2, 1)\).
ConditionNumber() returns zero as a sentinel in this case
rather than dividing by zero.QrDecomposition provides the Householder reflectors
reused in the bidiagonalization phase.JacobiEigenSolver (symmetric eigenproblem) is an
alternative route to the same result via \(\sigma_i^2 = \lambda_i(A^\top A)\); the two
cross-check each other.LuDecomposition and GaussianElimination
solve square systems but cannot handle rank-deficient or overdetermined
cases; SVD supersedes them for those inputs.TotalLeastSquares (item 44) builds directly on SVD: the
solution lies in the right singular vector corresponding to the smallest
singular value of the augmented matrix \([A\
b]\).Many embedded estimation and analysis tasks reduce to the symmetric eigenvalue problem: given a real symmetric matrix \(A\), find scalars \(\lambda_k\) and orthonormal vectors \(v_k\) such that \(A v_k = \lambda_k v_k\). Covariance matrices, Gramians, inertia tensors, and modal-analysis stiffness matrices are all symmetric, and their eigen-decomposition exposes principal directions (PCA), energy distribution, conditioning, and vibration modes.
The cyclic Jacobi method is the oldest and one of the most robust ways to solve this problem. It repeatedly applies planar rotations that annihilate one off-diagonal entry at a time, gradually driving \(A\) toward a diagonal matrix whose entries are the eigenvalues. Its appeal for resource-constrained systems is threefold: it needs only a fixed-size working copy of the matrix (no heap), it produces the full set of eigenvalues and a fully orthonormal eigenvector basis, and it is exceptionally accurate for symmetric matrices — including the small relative eigenvalues that QR-based methods can lose.
A Jacobi rotation \(G(p, q, \theta)\) is an orthogonal matrix equal to the identity except in the \(2\times 2\) block on rows/columns \(p\) and \(q\):
\[ \begin{bmatrix} c & s \\ -s & c \end{bmatrix}, \qquad c = \cos\theta, \; s = \sin\theta . \]
Applying the similarity transform \(A' = G^\top A G\) leaves the spectrum unchanged (orthogonal similarity) but modifies only rows/columns \(p\) and \(q\).
The angle is chosen so that the off-diagonal entry \(a_{pq}\) becomes zero. Requiring \(a'_{pq} = 0\) gives
\[ (c^2 - s^2)\,a_{pq} + c\,s\,(a_{pp} - a_{qq}) = 0 . \]
Defining
\[ \theta = \frac{a_{qq} - a_{pp}}{2\,a_{pq}}, \qquad t = \frac{\operatorname{sign}(\theta)}{|\theta| + \sqrt{\theta^2 + 1}}, \qquad c = \frac{1}{\sqrt{t^2 + 1}}, \qquad s = t\,c , \]
selects the smaller root \(t = \tan\theta\). Choosing the smaller rotation angle keeps the transformation close to the identity, which is what guarantees numerical stability and monotone convergence.
Let \(\operatorname{off}(A) = \sqrt{\sum_{i \neq j} a_{ij}^2}\) be the Frobenius norm of the off-diagonal part. Each rotation that zeroes \(a_{pq}\) reduces \(\operatorname{off}(A)^2\) by exactly \(2\,a_{pq}^2\), since orthogonal similarity preserves the total Frobenius norm and the diagonal absorbs the removed mass. Thus \(\operatorname{off}(A)\) decreases monotonically toward zero.
Rather than searching for the largest off-diagonal entry each step (classical Jacobi, \(O(n^2)\) search per rotation), the cyclic variant sweeps every pair \((p, q)\) with \(p < q\) in fixed row-major order. A full sweep touches all \(n(n-1)/2\) pairs. Near convergence the method is quadratically convergent: the off-diagonal norm is roughly squared each sweep, so a handful of sweeps suffices even for ill-conditioned inputs.
Accumulating the rotations \(V \leftarrow V\,G\) starting from \(V = I\) yields the orthogonal matrix whose columns are the eigenvectors. On termination \(A\) is (numerically) diagonal with \(\lambda_k = a_{kk}\), and \(A \approx V \operatorname{diag}(\lambda) V^\top\).
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(n^3)\) | \(O(n^2)\) | Already near-diagonal; one sweep to confirm convergence |
| Average | \(O(n^3)\) | \(O(n^2)\) | Typically 6–10 sweeps; each sweep costs \(O(n^3)\) |
| Worst | \(O(S n^3)\) | \(O(n^2)\) | \(S\) sweeps capped by a fixed maximum; each rotation is \(O(n)\) |
Why \(O(n^3)\) per sweep: a sweep performs \(n(n-1)/2 = O(n^2)\) rotations, and each rotation updates two rows and two columns at \(O(n)\) cost. Space is a single \(n\times n\) working copy plus the \(n\times n\) eigenvector accumulator — both stack/static, no dynamic allocation.
Input:
\[ A = \begin{bmatrix} 2 & 1 \\ 1 & 2 \end{bmatrix} \]
Step 1 — Pick pivot \((p, q) = (0, 1)\), \(a_{pq} = 1\).
Step 2 — Compute angle. \(\theta = (a_{qq} - a_{pp}) / (2 a_{pq}) = (2 - 2)/2 = 0\), so \(t = 1\), \(c = s = 1/\sqrt{2} \approx 0.7071\) (a \(45^\circ\) rotation).
Step 3 — Rotate. The updated diagonal is
\[ a'_{pp} = c^2 a_{pp} - 2 s c\, a_{pq} + s^2 a_{qq} = 1, \qquad a'_{qq} = s^2 a_{pp} + 2 s c\, a_{pq} + c^2 a_{qq} = 3, \]
and \(a'_{pq} = 0\). The matrix is now diagonal.
Step 4 — Read results. Eigenvalues \(\{1, 3\}\); eigenvectors (columns of the accumulated rotation) \(\tfrac{1}{\sqrt 2}(1, -1)^\top\) and \(\tfrac{1}{\sqrt 2}(1, 1)^\top\). After sorting ascending, \(\lambda = (1, 3)\).
| Variant | Key Difference |
|---|---|
| Classical Jacobi | Zeroes the largest off-diagonal entry each step; fewer rotations but an \(O(n^2)\) search each time |
| Threshold Jacobi | Skips pivots below a per-sweep threshold, cheaper early sweeps on sparse-ish matrices |
| One-sided Jacobi | Applies rotations to a factor only; the basis of Jacobi SVD for tall matrices |
| QR / tridiagonal method | Reduces to tridiagonal form then iterates — faster asymptotically but less accurate on tiny eigenvalues |
graph LR
JAC["Jacobi Eigen"]
QR["QR Decomposition"]
CHOL["Cholesky"]
SVD["SVD (Golub-Kahan)"]
QR -.->|"shares Givens rotations"| JAC
JAC -->|"symmetric building block"| SVD
JAC -.->|"eigenvalue floor keeps SPD"| CHOL| Algorithm | Relationship |
|---|---|
| QR Decomposition | Both are built from orthogonal (Givens/Householder) transforms; QR underlies the alternative tridiagonal eigen-method |
| Cholesky Decomposition | Requires symmetric positive-definite input; Jacobi eigenvalues certify or restore definiteness |
| Spectral Radius | Returns only the dominant eigenvalue magnitude; Jacobi returns the full spectrum and vectors |
Mechanical plants (robot arms, pendulums, drones, a motor driving a load) are naturally second-order systems: \(\ddot{q} = a(q, \dot{q}, u)\), with acceleration computed from position, velocity and input. They are simulated before hardware exists, so controllers can be tested in model-in-the-loop runs or run on-target as observers.
A general-purpose integrator treats this as a first-order system and ignores its structure. Over a long run the simulated energy creeps up or down, so a lightly damped plant looks more (or less) stable than it really is. Symplectic integrators advance position and velocity in an interleaved order that respects the geometry of mechanics. For a conservative system the energy error stays bounded forever, at one or two acceleration evaluations per step — fewer than the four of RK4.
Two fixed-step methods are provided: semi-implicit (symplectic) Euler, first order and one evaluation, and velocity Verlet in kick–drift–kick form, second order and two evaluations.
State \((q, v)\) with \(v = \dot{q}\). The input \(u\) is held constant over the step (zero-order hold), and the step size \(h > 0\) is fixed.
\[ v^+ = v + h\,a(q, v, u), \qquad q^+ = q + h\,v^+ \]
The only change from explicit Euler is that the position update uses the new velocity. Local error is \(O(h^2)\), global error \(O(h)\).
\[ v_{1/2} = v + \tfrac{h}{2}\,a(q, v, u), \qquad q^+ = q + h\,v_{1/2}, \qquad v^+ = v_{1/2} + \tfrac{h}{2}\,a(q^+, v_{1/2}, u) \]
The step is symmetric in time, which cancels the odd error terms: global error \(O(h^2)\). For constant acceleration it is exact, since \(v^+ = v + h\,a\) and \(q^+ = q + h\,v + \tfrac{h^2}{2}a\). Semi-implicit Euler is not: it gives \(q^+ = q + h\,v + h^2 a\).
For a position-only force, \(a = -\nabla U(q)\), each method is a composition of shears (a velocity update depending on \(q\), a position update depending on \(v\)), each with unit Jacobian determinant. The step map therefore preserves phase-space area and is symplectic. Backward error analysis then shows the numerical trajectory is the exact trajectory of a nearby modified energy \(\tilde{H} = H + O(h^p)\), so the true energy oscillates around a constant instead of drifting.
For \(a = -\omega^2 q\) and \(\theta = h\omega\), both methods have a step matrix with determinant \(1\) and trace \(2 - \theta^2\). The eigenvalues stay on the unit circle, hence the trajectory is bounded, exactly when \(|2 - \theta^2| < 2\), i.e.
\[ h\,\omega < 2 . \]
The conserved modified energies and the resulting error of the true energy \(E = \tfrac12(v^2 + \omega^2 q^2)\) are:
| Method | Conserved quantity (\(\omega = 1\)) | Relative energy error |
|---|---|---|
| Semi-implicit Euler | \(\tfrac12\left(v^2 + q^2 - h\,q\,v\right)\) | oscillates, amplitude \(\approx h\omega/2\) |
| Velocity Verlet | \(\tfrac12\left(v^2 + (1 - \tfrac{\theta^2}{4})\,q^2\right)\) | oscillates, amplitude \(\approx (h\omega)^2/8\) |
With damping or Coriolis terms the system is not Hamiltonian, so neither method is exactly symplectic. Verlet evaluates the second kick at the half-step velocity \(v_{1/2}\), which keeps second-order accuracy. Semi-implicit Euler evaluates its single acceleration at the old velocity. Energy decay is then physical dissipation, not a numerical artifact.
| Method | Acceleration evaluations | Order | Arithmetic | Memory |
|---|---|---|---|---|
| Semi-implicit Euler | 1 | 1 | \(O(n)\) | two state copies |
| Velocity Verlet | 2 | 2 | \(O(n)\) | two state copies |
Here \(n\) is the number of generalized coordinates. The cost of a step is the cost of its evaluations plus a handful of vector operations; no heap is used. Each step is a pure function of its arguments; the second Verlet evaluation is not reused as the next step’s first. Every evaluation goes through a virtual call: fine for simulation and model-in-the-loop tests; for interrupt-service use, bind the concrete system type instead.
Unit oscillator \(a = -q\), \(h = 0.1\), start \((q, v) = (1, 0)\), true energy \(E = 0.5\).
Semi-implicit Euler
| Step | Velocity update | Position update | True \(E\) | Modified energy |
|---|---|---|---|---|
| 1 | \(0 + 0.1(-1) = -0.1\) | \(1 + 0.1(-0.1) = 0.99\) | \(0.4950\) | \(0.5000\) |
| 2 | \(-0.1 + 0.1(-0.99) = -0.199\) | \(0.99 + 0.1(-0.199) = 0.9701\) | \(0.4903\) | \(0.5000\) |
The true energy wanders (about \(1\%\) and \(2\%\) low), yet the modified energy is conserved exactly; the offset is \(\tfrac12 h\,q\,v\), which cannot grow beyond \(h/4\).
Velocity Verlet, one step: half kick \(v_{1/2} = 0 + 0.05(-1) = -0.05\); drift \(q^+ = 1 + 0.1(-0.05) = 0.995\); half kick with the new position \(v^+ = -0.05 + 0.05(-0.995) = -0.09975\). Exact values are \(\cos 0.1 = 0.995004\) and \(-\sin 0.1 = -0.099833\). The true energy is \(0.49999\), and the modified energy \(\tfrac12\left(v^2 + 0.9975\,q^2\right) = 0.49875\) equals its initial value.
| Variant | Key difference |
|---|---|
| Semi-implicit Euler, velocity Verlet (this) | Symplectic, orders 1 and 2; explicit Euler differs only in using the old velocity for the drift and its energy grows |
| Higher-order composition | Chains of Verlet steps with weights (Yoshida, Suzuki) reach orders 4, 6, … |
| Implicit midpoint / Gauss | Symplectic and implicit; suited to stiff or non-separable systems at the cost of a nonlinear solve |
| Algorithm | Relationship |
|---|---|
| Runge–Kutta 4 | Four evaluations per step, fourth order, general first-order systems; energy drifts slowly on conservative problems |
| Dormand–Prince | Adaptive step and error control for first-order systems; higher accuracy per step, no structure preservation |
Mapping desired outputs to inputs through a non-square or ill-conditioned matrix \(A\) is everywhere: joint rates from a tool velocity, motor commands from a desired thrust and torque, calibration corrections from residuals. The plain pseudo-inverse divides by every singular value, so near rank loss it explodes and commands huge, saturating inputs. Damping replaces the division by a gain that equals \(1/\sigma\) for strong directions and fades to zero for weak ones, trading a small, bounded accuracy loss for a bounded answer.
When there are more inputs than outputs, \(A\) has a null space: input motions it cannot feel. A null-space projector removes from any vector every component the matrix sees, so a secondary goal (stay away from limits, share load, save energy) can use the spare freedom without disturbing the primary one. Both tools come from one thin SVD, factored once per matrix.
For \(A \in \mathbb{R}^{m \times n}\), tall or wide, and \(k = \min(m, n)\), the thin SVD is \(A = U\Sigma V^\top\) with \(\sigma_0 \geq \cdots \geq \sigma_{k-1} \geq 0\), left vectors \(u_i \in \mathbb{R}^m\) and right vectors \(v_i \in \mathbb{R}^n\). With a relative tolerance \(\tau\) (default \(10^{-4}\)) the rank threshold and rank are
\[\theta = \tau\,\sigma_0, \qquad r = \#\{\,i : \sigma_i > \theta\,\}.\]
Since the values are sorted, the counted directions are the leading \(r\). The zero matrix has \(\sigma_0 = 0\), hence \(r = 0\).
Damped least squares (Tikhonov regularization) minimizes \(\|Ax - b\|^2 + \lambda^2\|x\|^2\). Its normal equations give
\[x = (A^\top A + \lambda^2 I)^{-1}A^\top b = A^\top (AA^\top + \lambda^2 I)^{-1} b,\]
and in the singular basis each direction is scaled by the gain
\[g(\sigma) = \begin{cases} \dfrac{\sigma}{\sigma^2 + \lambda^2} & \lambda > 0, \\[2mm] 1/\sigma & \lambda = 0,\ \sigma > \theta, \\[1mm] 0 & \lambda = 0,\ \sigma \leq \theta. \end{cases}\]
For \(\sigma \gg \lambda\) the gain is \(\approx 1/\sigma\); for \(\sigma \ll \lambda\) it is \(\approx \sigma/\lambda^2 \to 0\). Its maximum is at \(\sigma = \lambda\), so \(g \leq 1/(2\lambda)\) always. Without damping the threshold zeroes weak directions, so no division by zero occurs. The solution and the damped inverse are
\[x = \sum_{i=0}^{k-1} g(\sigma_i)\,(u_i^\top b)\,v_i, \qquad A^\dagger_\lambda = \sum_{i=0}^{k-1} g(\sigma_i)\,v_i u_i^\top.\]
Every term lies in the span of the \(v_i\), the row space of \(A\), so \(x\) has no null-space component: for \(\lambda = 0\) it is the minimum-norm solution.
\[P = I - \sum_{i<r} v_i v_i^\top, \qquad Px = x - \sum_{i<r}(v_i^\top x)\,v_i.\]
The second form applies the projection without forming \(P\): \(r\) inner products and \(r\) vector updates. The projector is deliberately undamped. Built from the damped inverse, \(I - A^\dagger_\lambda A\) leaves a residual in the primary task:
\[A\,(I - A^\dagger_\lambda A) = \sum_i \sigma_i \frac{\lambda^2}{\sigma_i^2 + \lambda^2}\,u_i v_i^\top = \lambda^2 (AA^\top + \lambda^2 I)^{-1} A \neq 0.\]
Along each singular direction its eigenvalue is \(\lambda^2/(\sigma_i^2 + \lambda^2)\), strictly between \(0\) and \(1\) for \(\sigma_i > 0\), so it is not idempotent either. The thresholded projector is exact: \(P^2 = P\), \(P^\top = P\), and
\[AP = \sum_{i \geq r} \sigma_i\,u_i v_i^\top, \qquad \sigma_i \leq \theta,\]
which is zero for exact rank loss and at rounding level for full rank.
For \(b \in \operatorname{range}(A)\) the residual is \(b - Ax = \sum_i \frac{\lambda^2}{\sigma_i^2 + \lambda^2}(u_i^\top b)\,u_i\), hence
\[\|Ax - b\| \leq \frac{\lambda^2}{\sigma_{\min}^2 + \lambda^2}\,\|b\| \leq \frac{\lambda^2}{\sigma_{\min}^2}\,\|b\|,\]
with \(\sigma_{\min}\) the smallest nonzero singular value. Robustness costs accuracy of order \(\lambda^2/\sigma_{\min}^2\). Near rank loss the threshold grows the null space: the lost direction is released to the projector by design, while the damped solution stays bounded.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Factor | \(O(Mk^2)\) | \(O(mk + nk)\) | One Golub–Kahan SVD; wide matrices through their transpose |
| Solve | \(O((m + n)\,k)\) | \(O(n)\) | \(k\) inner products of length \(m\), \(k\) updates of length \(n\); no division |
| Project | \(O(n\,r)\) | \(O(n)\) | \(r\) inner products and updates; \(P\) never formed |
| Explicit inverse, projector | \(O(mnk)\), \(O(n^2 r)\) | \(O(mn)\), \(O(n^2)\) | Diagnostics; sums of rank-one terms |
Here \(M = \max(m, n)\). The SVD factors are the only persistent storage: \(U\) is \(m \times k\), \(V\) is \(n \times k\), plus \(k\) singular values. No heap is used, and the factorization is paid once per matrix; later solves and projections are inner products only.
Input: \(A = \begin{pmatrix} 1 & 0 & 1 \\ 0 & 1 & 1 \end{pmatrix}\), \(b = (1, 2)\), \(\tau = 10^{-4}\).
Factor. \(AA^\top = \begin{pmatrix} 2 & 1 \\ 1 & 2 \end{pmatrix}\) has eigenvalues \(3\) and \(1\), so \(\sigma = (\sqrt{3}, 1)\). Then \(u_0 = (1, 1)/\sqrt{2}\), \(u_1 = (1, -1)/\sqrt{2}\), \(v_0 = (1, 1, 2)/\sqrt{6}\), \(v_1 = (1, -1, 0)/\sqrt{2}\) (signs are arbitrary). The threshold is \(\theta \approx 1.7 \times 10^{-4}\), so \(r = 2\); the null space is spanned by \((1, 1, -1)\).
Gains and projections at \(\lambda = 0.1\):
| \(i\) | \(\sigma_i\) | \(u_i^\top b\) | \(1/\sigma_i\) | \(g(\sigma_i)\) |
|---|---|---|---|---|
| 0 | \(1.732051\) | \(2.121320\) | \(0.577350\) | \(0.575432\) |
| 1 | \(1\) | \(-0.707107\) | \(1\) | \(0.990099\) |
Solution. Without damping, \(x = A^\dagger b = (0, 1, 1)\): it satisfies \(Ax = b\) and is orthogonal to the null space (\(0 + 1 - 1 = 0\)). With \(\lambda = 0.1\), \(x = (0.003289, 0.993388, 0.996678)\). The residual is \(b - Ax = \lambda^2(AA^\top + \lambda^2 I)^{-1}b\), of norm \(0.009934\), within the bound \(0.022139\) (the looser \(\lambda^2\|b\|/\sigma_{\min}^2\) gives \(0.022361\)).
Projector. With both singular directions counted,
\[P = I - v_0 v_0^\top - v_1 v_1^\top = \tfrac{1}{3}\begin{pmatrix} 1 & 1 & -1 \\ 1 & 1 & -1 \\ -1 & -1 & 1 \end{pmatrix}, \qquad \operatorname{tr} P = 1 = n - r.\]
Projecting \((1, 0, 0)\) gives \((1/3, 1/3, -1/3)\), which \(A\) maps to \(0\). Projecting the damped solution gives \(\approx 0\), since it lies in the row space.
Rank loss and check. For \(A = \begin{pmatrix} 1 & 1 & 0 \\ 2 & 2 & 0 \end{pmatrix}\) the singular values are \((\sqrt{10}, 0)\): \(r = 1\) and \(\operatorname{tr} P = 2\). Undamped, the zero direction gets zero gain, and \(A^\dagger(1, 2) = (0.5, 0.5, 0)\). The results were compared with the closed forms \((A^\top A + \lambda^2 I)^{-1}A^\top b\) (tall) and \(A^\top (AA^\top + \lambda^2 I)^{-1} b\) (wide), evaluated in double precision, on random \(2 \times 3\), \(6 \times 7\), \(7 \times 6\) and \(4 \times 4\) matrices: solutions agree to about \(10^{-5}\) and \(\|P^2 - P\|\) is about \(10^{-6}\).
Many embedded decisions are quadratic programs: distribute a wrench over cables that can only pull, allocate thrust to motors that saturate, track prioritised tasks under joint limits, keep model-predictive inputs inside bounds. Clipping the unconstrained answer, or running a fixed number of projection sweeps, is cheap but can be wrong at the edges, where several limits interact.
This solver finds the exact minimiser of a strictly convex quadratic cost under linear equality and inequality constraints, in a bounded number of steps and without dynamic memory. It is a dual active-set method: it starts at the unconstrained minimum, which violates some constraints, and repeatedly pulls the point towards the most violated one along the direction that raises the cost least. Every intermediate point is optimal for the constraints chosen so far, so no feasible starting point is needed, and a running orthogonal factorisation keeps each step at quadratic cost.
\[ \min_{x \in \mathbb{R}^n} \; \tfrac12 x^\top H x + g^\top x \quad \text{s.t.} \quad E x = e, \;\; C x \ge c, \]
with \(H\) symmetric positive definite. Strict convexity makes the minimiser unique whenever the feasible set is non-empty. Write the rows of \(E\) followed by the rows of \(C\) as normals \(n_i\) with bounds \(b_i\), $m = $ (number of equalities) \(+\) (number of inequalities). A bound \(\ell_k \le x_k \le h_k\) is two inequality rows: \(x_k \ge \ell_k\) and \(-x_k \ge -h_k\).
A point \(x\) is optimal exactly when there are multipliers with
\[ H x + g = E^\top \lambda + C^\top \mu, \qquad \mu \ge 0, \qquad \mu_i\,(n_i^\top x - b_i) = 0, \]
and \(x\) is feasible. Equality multipliers \(\lambda\) are free in sign; inequality multipliers \(\mu\) are non-negative and zero for inactive rows. The algorithm maintains stationarity and \(\mu \ge 0\) at every iteration and reaches feasibility only at the end: the cost of the iterate rises monotonically towards the optimum.
Factor \(H = L L^\top\) and let \(N\) collect the active normals. The change of variables \(y = L^\top x\) turns the cost into \(\tfrac12\|y\|^2 + (L^{-1}g)^\top y\) and each normal into \(L^{-1} n_i\). A QR factorisation of these transformed normals gives the stable pair
\[ J = L^{-\top} Q, \qquad J^\top N = \begin{bmatrix} R \\ 0 \end{bmatrix}, \qquad J^\top H J = I, \]
with \(Q\) orthogonal and \(R\) upper triangular, \(q \times q\) for \(q\) active constraints. Split \(J = [J_1\; J_2]\) at column \(q\). The columns of \(J_2\) satisfy \(N^\top J_2 = 0\): moving along them keeps every active constraint satisfied. Since \(J J^\top = H^{-1}\), products with \(J\) replace any explicit inverse of \(H\), which is what keeps the update numerically stable.
Let \(p\) be the constraint to add, \(n^+\) its normal oriented so that the current residual \(s = n^{+\top} x - b^+\) is negative. Define
\[ d = J^\top n^+ = \begin{bmatrix} d_1 \\ d_2 \end{bmatrix}, \qquad z = J_2 d_2, \qquad r = R^{-1} d_1 . \]
Then \(n^+ = N r + H z\) and \(n^{+\top} z = \|d_2\|^2\). Moving \(x \leftarrow x + t z\), \(u \leftarrow u - t r\) on the active multipliers and \(u_p \leftarrow u_p + t\) preserves stationarity, leaves the active constraints satisfied, and changes the residual of \(p\) by \(t\,\|d_2\|^2\). Two events can stop the step:
With \(t = \min(t_1, t_2)\) the multipliers and, if \(t_2\) is finite, \(x\) are updated. If \(t = t_2\), constraint \(p\) becomes active. If \(t = t_1\), the blocking constraint is dropped and the step repeats for the same \(p\) with a smaller active set. If \(t = \infty\), nothing bounds the multiplier of \(p\) and the constraints are contradictory: the problem is infeasible.
In exact arithmetic \(n^+\) depends on the active normals iff \(d_2 = 0\). In single precision \(d_2\) is a tiny nonzero vector, and an absolute test on \(\|z\|\) lets the step \(-s/(z^\top n^+)\) divide by a rounding artefact. The shipped solver therefore uses a relative test:
\[ \|d_2\| \le 10^{-5}\,\|d\| . \]
Since \(J\) transforms by an orthogonal \(Q\), the ratio \(\|d_2\|/\|d\|\) is the sine of the angle between the transformed new normal and the span of the active ones. A dependent constraint takes no full step; only a dual step with \(t_1\) remains, which is how degenerate active sets resolve.
Equalities are added first, in order; an equality whose residual is positive is added with its normal and bound negated, and its multiplier is reported with the original sign. Equalities are never dropped. Then, while some inactive inequality has residual below \(-\tau\,\|n_i\|\) (relative tolerance, default \(\tau = 10^{-6}\)), the one with the most negative raw residual is added. Each completed addition raises the cost, so in exact arithmetic no active set repeats; an outer cap (default \(2m + n\)) and a bound on drops per addition guard against rounding-induced cycling. Hitting either returns an iteration-limit status with the current finite \(x\), which is dual feasible but may violate constraints.
With \(n\) variables, \(m\) constraints, \(q \le n\) active, and \(K\) the iteration cap:
| Case | Time | Space | Notes |
|---|---|---|---|
| Setup | \(O(n^3)\) | \(O(n^2)\) | Cholesky, triangular inverse \(J = L^{-\top}\), start point \(x = -J J^\top g\) |
| Best | \(O(n^3 + mn)\) | \(O(n^2 + m)\) | Unconstrained minimum already feasible: one residual scan |
| Average | \(O(n^3 + k\,(n^2 + mn))\) | \(O(n^2 + m)\) | \(k\) additions, a few drops; per addition \(d\), \(z\), \(r\) and rotations cost \(O(n^2)\), the scan \(O(mn)\) |
| Worst | \(O(n^3 + K\,(n + m)\,n^2)\) | \(O(n^2 + m)\) | Cap reached; loose bound, each addition allows at most \(n + m + 1\) add/drop passes |
Storage is \(J\) and \(R\) (\(n \times n\) each), multipliers, signs and an active-set list of length at most \(n\); all on the stack, no heap.
Minimise \(\tfrac12\|x\|^2\) (\(H = I\), \(g = 0\)) subject to \(x_2 \ge 1\) and \(0.1\,x_1 + 0.5\,x_2 \ge 0.9\). Start at \(x = 0\) with \(J = I\); residuals are \(-1\) and \(-0.9\), so the first row is the most violated.
| Step | Action | \(t_1\) | \(t_2\) | Result |
|---|---|---|---|---|
| 1 | Add row 1: \(z = (0,1)\), no active set | \(\infty\) | \(1/1 = 1\) | \(x = (0, 1)\), \(u_1 = 1\), row 1 active |
| 2 | Add row 2: \(n^+ = (0.1, 0.5)\); component along row 1 is \(d_1 = 0.5\), orthogonal part \(z = (0.1, 0)\), \(r = 0.5\), residual \(-0.4\) | \(1/0.5 = 2\) | \(0.4/0.01 = 40\) | Partial step \(t = 2\): \(x = (0.2, 1)\), \(u_1 = 0\), \(u_2 = 2\); row 1 dropped |
| 3 | Repeat row 2 with empty active set: \(z = n^+\), residual \(-0.38\) | \(\infty\) | \(0.38/0.26 = 1.4615\) | \(x = (0.346154, 1.730769)\), \(u_2 = 3.461538\) |
No inactive row is violated (\(x_2 = 1.73 \ge 1\)), so the solver stops: \(x = 0.9\,(0.1, 0.5)/0.26\), the projection onto the second row, with multiplier \(0.9/0.26 = 3.461538\) and row 1 inactive.
Other reference points:
| Algorithm | Relationship |
|---|---|
| Cholesky decomposition | Supplies \(L\) with \(H = L L^\top\), and is the test for strict convexity |
| Triangular solve | Builds \(J = L^{-\top}\) once and back-substitutes \(R^{-1} d_1\) every pass |
| Givens rotation | Restores the triangular form of \(R\) when a constraint is added or dropped |
| QR decomposition | \(J\) and \(R\) are an incrementally updated QR of the active normals in the \(H^{-1}\) metric |
| Damped pseudo-inverse | The unconstrained counterpart: with no active constraint the answer is \(-H^{-1} g\); this solver adds exact limits |
| Model predictive control | Its input bounds are a direct use of the bound rows; the current fixed-count projected descent could be replaced by this solver |
Many embedded control plants are naturally cascaded: motor current drives torque, torque drives velocity, velocity drives position. Classical linear controllers treat the whole cascade as a single transfer function and tune gains empirically. Backstepping instead exploits the cascade structure directly, stabilising each integrator stage in sequence with a Lyapunov certificate attached at every step. The result is a constructive design recipe — not empirical tuning — that is provably stable by construction, which is essential for safety-critical electromechanical and flight control loops.
The controller targets plants in strict-feedback form:
\[\dot{x}_i = f_i(x_1,\ldots,x_i) + g_i(x_1,\ldots,x_i)\,x_{i+1}, \quad i = 1,\ldots,n-1\]
\[\dot{x}_n = f_n(x) + g_n(x)\,u\]
where \(x_i \in \mathbb{R}\) are scalar states, \(u \in \mathbb{R}\) is the real input, and \(g_i \neq 0\) everywhere in the operating region (controllability condition).
Let \(r(t)\) be the reference with known derivative \(\dot{r}\). Define the stage-1 error:
\[z_1 = x_1 - r\]
To make \(z_1\) converge, treat \(x_2\) as a virtual control and choose the desired value:
\[\alpha_1 = \frac{\dot{r} - f_1 - k_1 z_1}{g_1}\]
so that \(\dot{z}_1 = -k_1 z_1\) when \(x_2 = \alpha_1\). Since \(x_2\) cannot be instantaneously set, define the next error:
\[z_2 = x_2 - \alpha_1\]
Repeating down the chain, at stage \(i\) the virtual control satisfies:
\[\alpha_i = \frac{\dot{\alpha}_{i-1} - f_i - k_i z_i - g_{i-1} z_{i-1}}{g_i}\]
The Lyapunov cross term \(g_{i-1} z_{i-1}\) cancels the coupling between successive stages. At stage \(n\), \(\alpha_n\) is the actual input \(u\).
The aggregate Lyapunov function:
\[V = \frac{1}{2} \sum_{i=1}^{n} z_i^2\]
has time derivative:
\[\dot{V} = -\sum_{i=1}^{n} k_i z_i^2 < 0 \quad \forall\, z \neq 0\]
This guarantees uniform asymptotic stability of the origin \(z = 0\) (equivalently, \(x \to r\)) for all \(k_i > 0\).
Each \(\alpha_i\) depends on \(x\), the gains, and the reference; its time derivative \(\dot{\alpha}_i\) must be propagated analytically (chain rule) down the cascade. Numerical differentiation is excluded: noise amplification destroys the stability guarantee.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Construction | \(O(n)\) | \(O(n)\) | Gain validation only |
| ComputeControl | \(O(n)\) | \(O(n)\) | Single forward sweep; one divide per stage |
All storage is in fixed-size arrays on the stack. No heap allocation, no recursion.
Order-2 chain of integrators (\(f_i = 0\), \(g_i = 1\), \(r = 0\), \(\dot{r} = 0\)):
Nonlinear plants such as robot arms, quadrotors, and electromechanical drives are only well-controlled by a fixed linear gain over a narrow operating range. Feedback linearization resolves this by exploiting a known model of the plant’s nonlinearity to cancel it exactly in the closed loop, leaving an equivalent linear system — decoupled integrator chains — that a single outer-loop gain set can drive correctly across the full operating envelope. No gain scheduling, no lookup tables, no re-tuning when the operating point changes.
The technique applies to plants whose output \(y \in \mathbb{R}^m\) satisfies, after \(r\) differentiations,
\[y^{(r)} = a(x) + B(x)\, u\]
where \(x \in \mathbb{R}^n\) is the state, \(u \in \mathbb{R}^m\) is the input, \(a(x) \in \mathbb{R}^m\) is the drift term (known nonlinear dynamics), and \(B(x) \in \mathbb{R}^{m \times m}\) is the decoupling matrix (state-dependent input gain). The integer \(r\) is the relative degree. For mechanical systems (\(r = 2\)), written as \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = u\), the control-affine form has \(B(x) = M^{-1}(q)\) and \(a(x) = -M^{-1}(q)\bigl(C(q,\dot{q})\dot{q} + g(q)\bigr)\).
The inner law selects \(u\) so that the term \(a(x)\) is cancelled and the decoupling matrix is factored out:
\[u = B^{-1}(x)\,(v - a(x))\]
Substituting into the plant equation yields
\[y^{(r)} = a(x) + B(x)\,B^{-1}(x)\,(v - a(x)) = a(x) + (v - a(x)) = v\]
leaving pure integrator chains \(y^{(r)} = v\), provided \(B(x)\) is nonsingular.
With the plant reduced to integrators, a PD outer loop commands the virtual input:
\[v = y_d^{(r)} + K_d\,\dot{e} + K_p\, e, \quad e = y_d - y, \quad \dot{e} = \dot{y}_d - \dot{y}\]
The closed-loop error satisfies the linear ODE
\[e^{(r)} + K_d\,\dot{e} + K_p\, e = 0\]
whose eigenvalues are set by choosing \(K_p, K_d\). Critical damping per channel requires \(K_d = 2\sqrt{K_p}\).
Expanding yields the single expression evaluated on the hot path:
\[u = B^{-1}(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e - a(x)\bigr)\]
The law requires solving the linear system \(B(x)\,u = v - a(x)\) on the hot path; \(B(x)\) must be nonsingular.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Construction | \(O(m^2)\) | \(O(m^2)\) | Copy two gain matrices |
| ComputeInput | \(O(m^3)\) | \(O(m)\) extra | One linear solve dominates |
| Model query | \(O(m^2)\)–\(O(nm^2)\) | \(O(m^2)\) | Implementation-defined; injected model |
All storage is in fixed-size stack arrays; the law itself performs no heap allocation.
Consider a 2-DOF planar arm with \(m = 2\), \(K_p = 100 I\), \(K_d = 20 I\), and at one instant:
The gravity-like drift \(a(x)\) is added directly; the outer PD term drives position error to zero.
Real hardware changes over time: motor resistance drifts with temperature, payload mass varies, actuators age. A fixed controller tuned at the factory cannot maintain performance across this variability. Model Reference Adaptive Control (MRAC) addresses this by continuously adjusting its own gains online, using only the tracking error between the real plant and a designer-specified reference model, until the plant’s response matches the desired ideal. No knowledge of the exact plant parameters is needed — only the structure (the equations of motion) and the sign of the input gain.
This makes MRAC the canonical direct-adaptive scheme for the “known structure, unknown numbers” class of problems that appears throughout embedded control: a family of units sharing the same firmware but differing in parameter values, or a single unit whose parameters change slowly during operation.
Given a plant of the form
\[\dot{x} = a\,x + b\,u\]
where \(a\) and \(b\) are unknown constants with \(b \neq 0\) and \(\text{sgn}(b)\) known, the goal is to choose the control \(u\) so that \(x(t) \to x_m(t)\) as \(t \to \infty\), where \(x_m\) satisfies the reference model
\[\dot{x}_m = -a_m\,x_m + b_m\,r, \quad a_m > 0.\]
The ideal control law that would make the plant identical to the reference model is
\[u^* = \theta_x^*\,x + \theta_r^*\,r\]
where
\[\theta_x^* = \frac{-(a_m + a)}{b}, \qquad \theta_r^* = \frac{b_m}{b}.\]
Since \(a\) and \(b\) are unknown, \(\theta_x^*\) and \(\theta_r^*\) cannot be computed directly; instead they are estimated online.
Let \(\hat{\theta}_x\) and \(\hat{\theta}_r\) denote the current parameter estimates and define the parameter errors \(\tilde{\theta} = \hat{\theta} - \theta^*\). The tracking error \(e = x - x_m\) satisfies
\[\dot{e} = -a_m\,e + b\,(\tilde{\theta}_x\,x + \tilde{\theta}_r\,r).\]
The MIT rule minimises \(J = \tfrac{1}{2}e^2\) by gradient descent on the parameter space:
\[\dot{\hat{\theta}}_x = -\gamma\,\text{sgn}(b)\,e\,x, \qquad \dot{\hat{\theta}}_r = -\gamma\,\text{sgn}(b)\,e\,r.\]
This is a first-order gradient update. It converges when \(\gamma\) is small relative to the signal levels and the reference is persistently exciting. It has no global stability proof.
The Lyapunov method chooses the same update law but derives it from the Lyapunov function candidate
\[V(e, \tilde{\theta}) = \frac{e^2}{2} + \frac{|b|}{2\gamma}\left(\tilde{\theta}_x^2 + \tilde{\theta}_r^2\right).\]
Computing \(\dot{V}\) and choosing the parameter update laws to make \(\dot{V} \leq -a_m\,e^2 \leq 0\) yields precisely the same gradient form. The Lyapunov construction guarantees that \(e\) and \(\tilde{\theta}\) remain bounded for all \(t \geq 0\) and that \(e(t) \to 0\), even under large initial errors — a global stability guarantee that the pure MIT rule lacks.
The continuous-time update is approximated by forward Euler with step \(\Delta t\):
\[x_m[k+1] = x_m[k] + (A_m\,x_m[k] + B_m\,r[k])\,\Delta t\]
\[e[k] = x[k] - x_m[k+1]\]
\[u[k] = \hat{\theta}_x[k]\,x[k] + \hat{\theta}_r[k]\,r[k]\]
\[\hat{\theta}_x[k+1] = \hat{\theta}_x[k] - \gamma\,\text{sgn}(b)\,e[k]\,x[k]\,\Delta t\]
\[\hat{\theta}_r[k+1] = \hat{\theta}_r[k] - \gamma\,\text{sgn}(b)\,e[k]\,r[k]\,\Delta t\]
The same structure extends to the multi-input multi-output case using outer products of the error and regressor vectors, yielding matrix parameter estimates.
| Operation | Time | Space | Notes |
|---|---|---|---|
| ComputeControl | \(O(n^2 + nm)\) | \(O(1)\) working registers | Reference model step + two outer products + two matrix-vector products |
| Memory | \(O(n^2 + nm)\) static | Parameter matrices and reference state | All fixed-size; no heap allocation |
Here \(n\) = StateSize and \(m\) = InputSize. The dominant cost is the outer product update of the parameter matrices at each step.
First-order scalar example (\(n = m = 1\), \(a = -1.5\), \(b = 2\), \(a_m = 1\), \(b_m = 1\), \(\gamma = 1\), \(\Delta t = 0.01\)):
Active Disturbance Rejection Control addresses a fundamental tension in feedback design: high-performance control normally requires an accurate plant model, yet accurate models are expensive to identify and degrade with temperature, load, and wear. ADRC resolves this by treating everything beyond a known input gain — unmodeled dynamics, parameter variation, and external disturbances — as a single lumped signal called the total disturbance. An Extended State Observer (ESO) estimates this signal in real time, and the control law subtracts the estimate before issuing the command. What remains behaves like a clean chain of integrators that a simple PD law can regulate with textbook bandwidth.
The practical payoff on embedded hardware is significant: you need only one plant number (\(b_0\), the rough input gain) and two tuning dials. The controller then survives a bad model because any mismatch is absorbed into the disturbance estimate.
An \(n\)-th order SISO plant is written as the canonical integrator chain plus a total-disturbance term \(f\):
\[y^{(n)} = f(t, y, \dot{y}, \ldots, d) + b_0 u\]
where \(f\) captures unmodeled dynamics, nonlinearities, and external loads; \(b_0\) is a nominal input-gain estimate; and \(u\) is the control input.
Augmenting the \(n\) plant states with \(x_{n+1} = f\) yields an \((n+1)\)-dimensional system. The continuous ESO is a Luenberger-type observer driven by the output error:
\[\dot{\hat{x}}_i = \hat{x}_{i+1} + \beta_i (y - \hat{x}_1), \quad i = 1, \ldots, n\] \[\dot{\hat{x}}_{n+1} = \beta_{n+1} (y - \hat{x}_1)\]
with the convention \(\hat{x}_{n+1} = \hat{f}\) and \(\hat{x}_2\) through \(\hat{x}_n\) as derivative estimates.
The forward-Euler discretization used here is:
\[\hat{x}_i[k+1] = \hat{x}_i[k] + T_s \bigl(\beta_i \, e[k] + \hat{x}_{i+1}[k]\bigr), \quad e[k] = y[k] - \hat{x}_1[k]\]
with \(b_0 u[k-1]\) injected into the \((n)\)-th state to drive the highest derivative.
All observer poles are placed at \(-\omega_o\) (Gao’s bandwidth parameterization). The resulting gains follow the binomial expansion of \((\lambda + \omega_o)^{n+1}\):
\[\beta_i = \binom{n+1}{i} \omega_o^i, \quad i = 1, \ldots, n+1\]
All control poles are placed at \(-\omega_c\) via the expansion of \((\lambda + \omega_c)^n\):
\[k_i = \binom{n}{i} \omega_c^i, \quad i = 1, \ldots, n\]
For a second-order plant (\(n = 2\)):
\[\beta = [3\omega_o,\; 3\omega_o^2,\; \omega_o^3], \quad k = [\omega_c^2,\; 2\omega_c]\]
After disturbance estimation the control is:
\[u = \frac{u_0 - \hat{f}}{b_0}, \qquad u_0 = k_1(r - \hat{x}_1) - \sum_{i=2}^{n} k_i \hat{x}_i\]
Substituting into the plant equation and using \(\hat{f} \approx f\) gives the closed-loop residual \(y^{(n)} \approx u_0\), a pure integrator chain under a PD law — independent of the original plant dynamics.
| Case | Time | Space | Notes |
|---|---|---|---|
| Best | \(O(n)\) | \(O(n)\) | Linear sweep over \(n+1\) ESO states |
| Average | \(O(n)\) | \(O(n)\) | Same; gains precomputed at construction |
| Worst | \(O(n)\) | \(O(n)\) | No branching in the hot path |
Gains are computed once at construction from closed-form binomial
formulas in \(O(n)\) time. The
Compute hot path is a pair of \(O(n)\) loops with no dynamic
allocation.
Second-order plant (\(n=2\)), \(\omega_o = 30\), \(\omega_c = 6\), \(b_0 = 1\), \(T_s = 0.001\) s.
Observer gains: \(\beta_1 = 90\),
\(\beta_2 = 2700\), \(\beta_3 = 27000\).
Control gains: \(k_p = 36\), \(k_d = 12\).
At sample \(k\) with state \(\hat{x} = [\hat{y}, \hat{\dot{y}}, \hat{f}]\), measurement \(y[k]\), reference \(r\):
Compute(reference, measuredOutput, actualApplied)
overload.After a transient of roughly \(5/\omega_o \approx 0.17\) s the observer converges; the output tracks \(r\) with bandwidth \(\omega_c\).
Actuator saturation (anti-windup). When the
downstream actuator clips the commanded value, the ESO integrates the
wrong input and its disturbance estimate drifts, causing significant
overshoot. Close the anti-windup loop by feeding the actually-applied
signal back:
u_applied = clamp(Compute(r, y, u_applied_prev), lo, hi),
then pass u_applied as the third argument on the next call.
The two-argument Compute(reference, measuredOutput) is a
convenience overload that assumes no saturation (commanded = applied)
and is equivalent to passing the last commanded value.
ESO peaking. Large initial estimation errors drive high-magnitude corrections, temporarily saturating the actuator. Mitigation: initialize the observer near the first measurement, or schedule \(\omega_o\) upward from a low value during the first few samples.
Observer bandwidth vs. noise. Increasing \(\omega_o\) speeds convergence but amplifies measurement noise because \(\beta_3 = \omega_o^3\) grows cubically. A practical rule of thumb is \(\omega_o \in [3\omega_c, 10\omega_c]\).
\(b_0\) mismatch. The ESO is robust to moderate mismatch (factor of 2–3), but large errors shrink the stability margin. If \(b_0 \gg b_\text{true}\) the effective loop gain drops and response slows; if \(b_0 \ll b_\text{true}\) the loop gain rises and may oscillate.
Euler discretization stability and accuracy. The forward-Euler ESO is unconditionally unstable once \(\omega_o T_s\) exceeds the stability boundary of the \((n+1)\)-th order observer polynomial. For a first-order plant (\(n=1\)) this boundary is \(\omega_o T_s \approx 0.83\); higher orders have progressively tighter boundaries (approximately \(0.54\) for \(n=2\), \(0.40\) for \(n=3\)). The constructor enforces \(\omega_o T_s < 0.5/n\) as a conservative order-aware hard precondition (aborts if violated). Keeping \(\omega_o T_s \leq 0.1\) is further recommended for accuracy; beyond that a ZOH or bilinear discretization is preferred.
Integer overflow in gain computation. Binomial
coefficients are computed with integer arithmetic at compile time. For
large orders or very high bandwidths the intermediate product may exceed
std::size_t before the division; keep \(n \leq 5\) in practice.
Nonlinear ESO (NESO). Replace the linear correction \(\beta_i e\) with Han’s fal function to reduce peaking while preserving fast convergence.
Discrete ESO. Exact discretization of the observer (ZOH or pole-matched) improves accuracy when \(\omega_o T_s\) is not small.
Higher-order plants. The template parameter
Order generalizes the same bandwidth-parameterized
structure to \(n > 2\) — gains grow
binomially and the Compute loop extends automatically.
Multi-input / multi-output (MIMO). Each output channel runs an independent ADRC; cross-coupling is absorbed into the respective disturbance estimates.
Variable-structure control systems change their structure depending on the current system state. Sliding Mode Control (SMC) is the most widely used instantiation: the controller switches between two or more structures to drive the state onto a designer-specified manifold — the sliding surface — and keep it there. Once constrained to the surface, the closed-loop dynamics are governed entirely by the surface geometry, independent of the plant model or matched disturbances. This structural robustness makes SMC a preferred choice for motor drives, DC-DC power converters, and any embedded plant whose parameters drift or are poorly known.
The controller operates on a discrete-time linear plant
\[x_{k+1} = A x_k + B u_k, \quad x \in \mathbb{R}^n,\; u \in \mathbb{R}^m\]
where \(A \in \mathbb{R}^{n \times n}\) and \(B \in \mathbb{R}^{n \times m}\).
A linear sliding surface is defined by
\[s(x) = S x, \quad S \in \mathbb{R}^{m \times n}\]
The sliding manifold \(\{x : s(x) = 0\}\) is an \((n-m)\)-dimensional subspace. The matrix \(S\) is designed so the reduced-order dynamics on the manifold are stable and meet the desired closed-loop poles.
The equivalent control \(u_{eq}\) is the unique input that holds the state on \(s = 0\) (i.e., \(\dot{s} = 0\)) for the nominal plant:
\[u_{eq} = -(S B)^{-1} S A x\]
This exists if and only if \(S B\) is nonsingular, which is the relative-degree-one condition: each input channel must directly influence its corresponding sliding variable.
The switching term adds a robust push toward the surface:
\[u_{sw} = (S B)^{-1} K \mathrm{sat}(s/\phi)\]
where \(K \in \mathbb{R}^{m}\) is the per-channel switching gain and \(\phi > 0\) is the boundary-layer thickness. The saturation function
\[\mathrm{sat}(\sigma) = \begin{cases} \sigma & |\sigma| \le 1 \\ \mathrm{sign}(\sigma) & |\sigma| > 1 \end{cases}\]
replaces the discontinuous \(\mathrm{sign}(s)\) of ideal SMC with a continuous ramp inside \(|s| \le \phi\), eliminating infinite-bandwidth chattering while maintaining the reaching property.
\[u = u_{eq} - u_{sw} = -(S B)^{-1}\bigl[S A x + K\,\mathrm{sat}(s/\phi)\bigr]\]
The Lyapunov function \(V = \tfrac{1}{2} s^\top s\) satisfies \(\dot{V} < 0\) outside the boundary layer when \(K_i > |d_i|\) for each matched-disturbance channel \(d_i\). This guarantees finite-time arrival at \(|s| \le \phi\).
On the surface \(s = 0\), the state evolves according to the \((n-m)\)-dimensional reduced-order system. For a single-input system with \(S = [c_1, \ldots, c_{n-1}, 1]\), the sliding pole is determined by the characteristic polynomial of the first \((n-1)\) rows under the surface constraint.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Construction | \(O(m^3 + n m)\) | \(O(nm + m^2)\) | \((SB)^{-1}\) inversion once |
| ComputeControl | \(O(n^2 + m^2)\) | \(O(1)\) extra | \(S A x\) dominates for large \(n\) |
| Surface | \(O(nm)\) | \(O(1)\) extra | monitoring only |
All data is stored in fixed-size arrays; no heap allocation at any point.
Consider a double-integrator plant (\(n=2\), \(m=1\)):
\[A = \begin{bmatrix}0 & 1\\0 & 0\end{bmatrix}, \quad B = \begin{bmatrix}0\\1\end{bmatrix}\]
with surface \(S = [1\; 1]\), gain \(K = 2\), boundary layer \(\phi = 0.05\).
sat function is semantically identical to the saturation
block already in the library.Real plants never match their nominal models. External loads, friction, actuator nonlinearities, and parameter drift inject unmodeled energy into the loop. A Disturbance Observer (DOB) lumps all of these effects into a single equivalent disturbance signal, estimates it online, and subtracts it from the control input so the plant behaves as if it were the clean nominal model.
The key insight is that the DOB wraps around any existing controller without redesigning it. An engineer who has already tuned a PID or LQR for the nominal plant can bolt on a DOB and gain strong disturbance rejection without revisiting the nominal design. This makes DOBs especially attractive for embedded motion controllers — motor drives, precision stages, robotic joints — where the plant is moderately well-known but subject to load variations the nominal model ignores.
Let the true discrete-time plant be \(P(z)\) and the nominal model be \(P_n(z)\). The control input seen by the true plant is \(u_a = u + d\), where \(u\) is the commanded input and \(d\) is the lumped equivalent disturbance that captures model mismatch, external loads, and friction. The plant output is
\[y = P(z)\, u_a = P(z)(u + d).\]
If \(P_n^{-1}(z)\) is applied to \(y\), it reconstructs the effective input that the nominal plant would have needed to produce that output:
\[P_n^{-1}(z)\, y \approx u + d \quad \text{(if } P \approx P_n\text{)}.\]
Subtracting the actual commanded input \(u\) isolates the disturbance:
\[\hat{d} = P_n^{-1}(z)\, y - u.\]
The plant inverse \(P_n^{-1}(z)\) is generally improper (more zeros than poles) and amplifies high-frequency measurement noise. A low-pass Q-filter \(Q(z)\) is cascaded to make the combination \(Q(z)\,P_n^{-1}(z)\) proper and bandwidth-limited:
\[\hat{d} = Q(z)\,P_n^{-1}(z)\, y - Q(z)\, u.\]
The filter \(Q(z)\) must have relative degree at least equal to the relative degree of \(P_n(z)\) so the realization does not differentiate. Unity DC gain, \(Q(1) = 1\), is required for complete rejection of constant (step) disturbances.
The DOB corrects the nominal controller output \(c\) by subtracting the estimate:
\[u = c - \hat{d}.\]
The closed-loop system then sees an effective plant of \(P_n(z)\) inside the Q-filter bandwidth — the actual mismatch and disturbances are cancelled — and approaches the uncorrected nominal plant behaviour outside the bandwidth.
Let \(L(z) = Q(z)\,P_n^{-1}(z)\,P(z)\). The closed-loop sensitivity from disturbance \(d\) to output \(y\) is
\[S_d(z) = \frac{P(z)(1 - Q(z))}{1 + P(z)C(z)(1 - Q(z))}.\]
Inside the Q-filter passband (\(Q \approx 1\)): \(S_d \approx 0\) — the disturbance is rejected. Outside the passband (\(Q \approx 0\)): \(S_d\) equals the nominal sensitivity — the DOB is transparent.
Robust stability requires the complementary sensitivity of the inner DOB loop to satisfy
\[\left|Q(e^{j\omega})\,\Delta_m(e^{j\omega})\right| < 1 \quad \forall\, \omega,\]
where \(\Delta_m = (P - P_n)/P_n\) is the relative model uncertainty. Widening \(Q\) improves disturbance rejection but shrinks the robust-stability margin — this is the fundamental DOB trade-off.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Construct | \(O(N \cdot S^2)\) | \(O(S^2 + N)\) | DC gain simulation, \(S\) = StateSize, \(N\) = 512 |
| Compute | \(O(N_{\rm in})\) | \(O(1)\) extra | Per-channel biquad filter pair |
| Reset | \(O(N_{\rm in})\) | \(O(1)\) extra | Clears filter states |
All storage is fixed-size; no heap allocation occurs at any point in the lifecycle.
Consider a first-order discrete plant (\(n=1\), \(m=p=1\)) with \(a=0.9\), \(b=0.1\), \(c=1\), DC gain \(= b/(1-a) = 1\), and a second-order Butterworth Q-filter at 20 Hz (sample rate 1 kHz).
Steady-state with constant disturbance \(d = 0.5\), nominal command \(c = 0\):
Robust control guarantees performance under worst-case conditions rather than average ones. H∞ state-feedback synthesizes a linear gain that minimizes the largest possible ratio of output energy to disturbance energy across all bounded disturbances — the induced \(\mathcal{L}_2\) gain. This provable attenuation bound is what safety-critical and certification-driven embedded systems require: not a best-guess response, but a hard upper limit on how badly an unknown disturbance can degrade performance.
The design is framed around the generalized plant
\[ x_{k+1} = A x_k + B_1 w_k + B_2 u_k, \quad z_k = C_1 x_k + D_{12} u_k \]
where \(w \in \mathbb{R}^{n_w}\) is the exogenous disturbance, \(u \in \mathbb{R}^{n_u}\) is the control input, and \(z \in \mathbb{R}^{n_z}\) is the performance (error) output.
The controller objective is to find \(u = -K x\) such that
\[ \sup_{w \neq 0} \frac{\|z\|_2}{\|w\|_2} < \gamma \]
where \(\gamma > 0\) is the prescribed attenuation level. Finding the smallest feasible \(\gamma\) (the optimal \(\gamma^*\)) determines the best achievable robustness.
The H∞ state-feedback gain is derived from the discrete Riccati equation
\[ X = A^\top X A - A^\top X B \tilde{R}^{-1} B^\top X A + Q \]
with the augmented input matrix \(B = [B_2 \mid B_1]\) and the indefinite weight matrix
\[ \tilde{R} = \begin{pmatrix} I_{n_u} & 0 \\ 0 & -\gamma^2 I_{n_w} \end{pmatrix}. \]
The negative block encodes the adversarial role of the disturbance: the disturbance player maximizes while the control player minimizes. A positive-semidefinite stabilizing solution \(X \geq 0\) exists if and only if \(\gamma\) is above the optimal level \(\gamma^*\).
From the Riccati solution \(X\), the augmented gain is
\[ K_{\text{full}} = (\tilde{R} + B^\top X B)^{-1} B^\top X A. \]
Only the top \(n_u\) rows — the control block — form the feedback gain \(K\), and the closed-loop map is \(A_{\text{cl}} = A - B_2 K\).
A given \(\gamma\) is feasible when: 1. The GARE has a positive-semidefinite solution \(X \geq 0\). 2. The disturbance block \(-\gamma^2 I + B_1^\top X B_1 \prec 0\) is negative definite, confirming that the disturbance remains a genuine maximizer rather than a destabilizing force.
Neither condition holds for \(\gamma < \gamma^*\); both hold for \(\gamma > \gamma^*\). A standard bisection on \([\gamma_{\min}, \gamma_{\max}]\) converges to \(\gamma^*\) at a linear rate.
| Phase | Time Complexity | Space | Notes |
|---|---|---|---|
| Synthesize | \(O(\log((\gamma_{\max}-\gamma_{\min})/\epsilon) \cdot n^3)\) | \(O(n^2)\) | Dominated by iterative DARE solves |
| ComputeControl | \(O(n_u \cdot n)\) | \(O(1)\) | Single matrix-vector multiply |
Consider a 2-state discrete plant with one disturbance and one control input.
1e-3f) prevents premature acceptance of a diverged
iterate.A window function is a mathematical taper applied to a finite-length signal before spectral analysis. When we extract a segment of \(N\) samples from a continuous signal, we implicitly multiply by a rectangular window — a hard cut that introduces sharp discontinuities at the segment boundaries. In the frequency domain these discontinuities spread energy across all bins, an artefact called spectral leakage.
Window functions taper the segment smoothly toward zero at both ends, trading a small amount of frequency resolution (wider main lobe) for dramatically reduced leakage (lower side lobes). Choosing the right window is a fundamental step in every FFT-based pipeline.
A symmetric window of length \(N\) is a sequence \(w[n],\; n = 0, 1, \ldots, N-1\), designed so that \(w[0] \approx w[N-1] \approx 0\) (except for the rectangular case).
| Window | Definition | Main-Lobe Width | Peak Side Lobe |
|---|---|---|---|
| Rectangular | \(w[n] = 1\) | \(2/N\) | \(-13\) dB |
| Hamming | \(w[n] = 0.54 - 0.46\cos\!\left(\frac{2\pi n}{N-1}\right)\) | \(4/N\) | \(-43\) dB |
| Hann | \(w[n] = 0.5\left(1 - \cos\!\left(\frac{2\pi n}{N-1}\right)\right)\) | \(4/N\) | \(-32\) dB |
| Blackman | \(w[n] = 0.42 - 0.5\cos\!\left(\frac{2\pi n}{N-1}\right) + 0.08\cos\!\left(\frac{4\pi n}{N-1}\right)\) | \(6/N\) | \(-58\) dB |
In the frequency domain, windowing is convolution with the window’s Fourier transform \(W(f)\). Each spectral line of the signal is smeared by \(W(f)\). A narrower main lobe preserves resolution; lower side lobes suppress leakage.
The coherent gain \(CG\) of a window normalizes the amplitude after windowing:
\[CG = \frac{1}{N}\sum_{n=0}^{N-1} w[n]\]
For amplitude-accurate measurements, divide the windowed FFT output by \(CG\).
The processing gain \(PG\) compares the noise bandwidth of the window to the rectangular window:
\[PG = \frac{\left(\sum w[n]\right)^2}{N \sum w[n]^2}\]
| Window | Processing Gain |
|---|---|
| Rectangular | 1.00 |
| Hamming | 0.73 |
| Hann | 0.67 |
| Blackman | 0.57 |
| Operation | Time | Space |
|---|---|---|
| Generate \(N\) window coefficients | \(O(N)\) | \(O(N)\) |
| Apply window (element-wise multiply) | \(O(N)\) | \(O(1)\) extra |
| Pre-computed lookup | \(O(1)\) per sample | \(O(N)\) |
Window generation is dominated by trigonometric evaluations (\(\cos\)). For repeated use at a fixed \(N\), pre-compute and store the coefficients in a lookup table.
Scenario: Apply a Hann window to \(N = 8\) samples of a pure tone at bin 2.5 (non-integer frequency → leakage expected).
Step 1 — Generate the Hann window
| \(n\) | \(w[n] = 0.5(1 - \cos(2\pi n / 7))\) |
|---|---|
| 0 | 0.000 |
| 1 | 0.188 |
| 2 | 0.611 |
| 3 | 0.950 |
| 4 | 0.950 |
| 5 | 0.611 |
| 6 | 0.188 |
| 7 | 0.000 |
Step 2 — Element-wise multiply
\[x_w[n] = x[n] \cdot w[n]\]
The signal tapers to zero at both ends, removing the hard edges.
Step 3 — FFT of windowed signal
Compared to the un-windowed (rectangular) FFT: - The peak at bin 2.5 is slightly broader (main lobe widens from 2 to 4 bins). - The leakage floor drops from \(-13\) dB to \(-32\) dB.
graph LR
subgraph Before Windowing
R["Rectangular window<br/>Sharp edges → leakage"]
end
subgraph After Windowing
H["Hann window<br/>Smooth taper → low side lobes"]
end
R --> |"apply Hann"| H
H --> FFT["FFT → cleaner spectrum"]| Variant | Key Difference |
|---|---|
| Kaiser window | Parameterized by \(\beta\); allows continuous trade-off between main-lobe width and side-lobe level |
| Flat-top window | Near-zero amplitude error at the cost of very wide main lobe; used for calibration |
| Gaussian window | Smooth, no side-lobe discontinuities; parameterized by \(\sigma\) |
| Dolph–Chebyshev | Equiripple side lobes at a prescribed level; optimal for a given main-lobe width |
| DPSS (Slepian) | Maximum energy concentration in a given bandwidth; used in multi-taper spectral estimation |
graph TD
W["Window Functions"]
FFT["Fast Fourier Transform"]
PSD["Power Density Spectrum"]
DCT["Discrete Cosine Transform"]
W --> FFT
W --> PSD
W --> DCT| Algorithm | Relationship |
|---|---|
| FFT | Window is applied before the FFT to reduce spectral leakage |
| Power Density Spectrum | Built-in windowing per segment in Welch’s method |
| DCT | DCT’s implicit symmetry provides partial leakage suppression, but windowing still helps |