diff --git a/CHANGELOG.md b/CHANGELOG.md index e559905..e4d9a94 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,10 @@ See also: ## [UNRELEASED](https://github.com/quartiq/idsp/compare/v0.22.0...HEAD) - DATE +### Added + +* `kalman`: Scalar static Kalman filter and tooling + ## [0.22.0](https://github.com/quartiq/idsp/compare/v0.21.0...v0.22.0) - 2026-06-11 ### Changed diff --git a/dsp-fixedpoint/README.md b/dsp-fixedpoint/README.md index b55d18c..f6a2f7d 100644 --- a/dsp-fixedpoint/README.md +++ b/dsp-fixedpoint/README.md @@ -1,130 +1,82 @@ -# Fixed point primitives +# `dsp-fixedpoint` -`dsp-fixedpoint` provides small `no_std` fixed-point primitives with explicit -integer storage and widening accumulators. +`no_std` fixed-point arithmetic with explicit storage, accumulator, and +quantization boundaries. -## Model +## Type shape -`Q` stores a raw integer `T` and interprets it as scaled by `2^F`. +`Q` stores raw `T` bits scaled by `2^-F`; products accumulate in `A`. -- `T` is the storage type. -- `A` is the widened accumulator type used for intermediate results. -- `F` is the number of fractional bits. +- `T`: stored and signal value +- `A`: wide accumulator +- `F`: fractional bits -Type aliases cover the common signed, unsigned, and wrapping pairs: -`Q8/Q16/Q32/Q64`, `P8/P16/P32/P64`, `W8/W16/W32/W64`, and `V8/V16/V32/V64`. +Aliases include `Q32 = Q` and wrapping/unsigned variants. -## Construction - -There are two construction modes: +## Wide MAC, late quantization -- `from_int`, `from_f32`, and `from_f64` scale into the fixed-point domain. -- `from_bits` is the raw-representation constructor. +Put the coefficient on the left and the raw signal on the right: ```rust -use dsp_fixedpoint::Q8; - -let scaled = Q8::<4>::from_int(3); -let raw = Q8::<4>::from_bits(3 << 4); - -assert_eq!(scaled, raw); -assert_eq!(raw.into_bits(), 48); -``` +use dsp_fixedpoint::Q32; -## Operator semantics +type C = Q32<28>; +let a = [C::from_f32(0.25), C::from_f32(-0.5)]; +let x = [100_i32, 20]; -The crate keeps addition-like operators conservative and multiplication-like -operators efficient. `Q` means `Q` unless shown otherwise. +let mut acc = a[0] * x[0]; // Q +acc += a[1] * x[1]; // still wide +let y = acc.quantize(); // one shift and narrowing conversion -| Operation | Result | Notes | -| --- | --- | --- | -| `Q + Q`, `Q - Q`, `Q % Q` | `Q` | Requires the same `F`; use `.scale::()` explicitly when scales differ. | -| `Q * Q`, `Q / Q` | `Q` | Preserves the left-hand scale and quantizes the fixed-point result. | -| `Q * T` | `Q` | Widened raw-integer multiplication; same as `q.mul_wide(t)`. | -| `T * Q` | `T` | Applies `Q` as a gain to `T` and quantizes; same as `q.apply(t)`. | -| `Q / T` | `Q` | Raw integer division of the stored representation. | -| `T / Q` | `T` | Divides by `Q` as a fixed-point gain and quantizes. | -| `Q *= Q`, `Q /= Q` | `Q` | Assignment forms follow the left-hand scale. | -| `Q *= T`, `Q /= T` | `Q` | Assignment forms operate on the raw stored representation. | - -The mixed multiplication and division asymmetry is intentional: operand order -chooses whether the result remains wide or is quantized immediately. Use -`mul_wide()` and `apply()` when spelling that choice explicitly is clearer than -relying on operand order. - -## Scale restrictions - -`F` is an `i8`, but not every `i8` value is meaningful everywhere. - -- `Q::<_, _, -128>::DELTA` is rejected at compile time. -- `Q::<_, _, F>::one()` and `Q::<_, _, F>::ONE` are rejected at compile time - when the mathematical value `1` is not exactly representable by the storage - type and scale. - -```compile_fail -use dsp_fixedpoint::Q8; -use num_traits::One; - -let _ = Q8::<7>::one(); +assert_eq!(y, 15); ``` -## Serialization - -With `feature = "serde"`, `Q` serializes transparently as its raw representation. -For lossy scaled values, use `dsp_fixedpoint::serde::as_f32` or -`dsp_fixedpoint::serde::as_f64`. +The shape is `Q * T -> Q`. Accumulate products of that type, +then call `quantize()` once. Reversing the operands, `T * Q -> T`, quantizes each +product immediately. -```rust,ignore -# use serde::{Deserialize, Serialize}; -use dsp_fixedpoint::Q32; +## Construction -#[derive(Deserialize, Serialize)] -struct Config { - #[serde(with = "dsp_fixedpoint::serde::as_f64")] - gain: Q32<3>, -} -``` +`from_int` and `from_f32`/`from_f64` construct scaled values; `from_bits` +preserves raw bits. `FromRatio` constructs a coefficient from two raw values: -## Ecosystem Traits +```rust +use dsp_fixedpoint::{FromRatio, Q32}; -`Q` implements a small set of generic numeric traits where the semantics stay -clear: +let gain = Q32::<28>::from_ratio(1, 3); +assert!((gain.as_f64() - 1.0 / 3.0).abs() < Q32::<28>::DELTA as f64); +``` -- `Bounded` forwards to the raw storage bounds. -- `ToPrimitive`, `FromPrimitive`, and `NumCast` convert numeric values rather - than raw bits. -- `Signed` is implemented for signed `Q`/`W` families. -- `Hash` follows the raw representation. +For positive `F`, `FromRatio` widens the numerator before scaling and division. +The denominator must be nonzero and the result must fit `T`. -`Signed` inherits `One`, so the existing representability restriction still -applies: operations that need an exact `1`, such as `signum()`, are only usable -when `F >= 0`. +## Operators -## Formatting +| Operation | Result | Quantization | +| --- | --- | --- | +| `Q * T` | `Q` | none; wide product | +| `T * Q` | `T` | immediate | +| `Q * Q`, `Q / Q` | `Q` | immediate | +| `Q + Q`, `Q - Q` | `Q` | none; equal `F` required | +| `T / Q` | `T` | immediate | -Display shows decimal notation. `Binary/Octal/UpperHex/LowerHex` show dot -notation. +`mul_wide()` and `apply()` spell `Q * T` and `T * Q` explicitly. -```rust -use dsp_fixedpoint::Q8; +## Scale and conversion -assert_eq!( - format!("{:#b}", Q8::<3>::from_bits(0b01101001)), - "0b1101.001" -); -assert_eq!(format!("{:x}", Q8::<4>::from_bits(-0x14)), "-1.4"); -``` +Use `.scale::()` when changing fractional bits. Numeric traits convert the +represented value; `from_bits`/`into_bits` access the representation. -## `defmt` +`Q::<_, _, -128>::DELTA` is invalid. `One` is available only when `1` is exactly +representable by `T` and `F`. -With `feature = "defmt"`, `Q` implements `defmt::Format` and logs as a decimal -value using `f32`. This keeps the target-side implementation compact for -embedded use. Exact radix-dot formatting remains available through the standard -`Binary`, `Octal`, and hex format traits. +## Optional integrations -## `bytemuck` +- `serde`: transparent raw representation; `serde::as_f32`/`as_f64` for scaled + values. +- `defmt`: compact decimal logging through `f32`. +- `bytemuck`: `Pod`, `Zeroable`, and `TransparentWrapper` when the component + types permit them. -With `feature = "bytemuck"`, `Q` derives `Pod`, `Zeroable`, and -`TransparentWrapper`. That makes zero-copy buffer casts and wrapper conversions -available when the underlying storage and accumulator types also satisfy the -corresponding `bytemuck` bounds. +`Display` is decimal. Binary, octal, and hexadecimal formatting place the radix +point according to `F`. diff --git a/dsp-fixedpoint/src/lib.rs b/dsp-fixedpoint/src/lib.rs index b85daa3..0a698b5 100644 --- a/dsp-fixedpoint/src/lib.rs +++ b/dsp-fixedpoint/src/lib.rs @@ -16,6 +16,54 @@ use core::{ ops::{Div, Mul, Shl, Shr}, }; +/// Construct a value from the ratio of two raw values. +/// +/// For positive `F`, `Q` computes `(numerator << F) / denominator` in +/// `A`, then narrows to `T`. The denominator must be nonzero and the result must +/// fit `T`. +pub trait FromRatio { + /// Return `numerator / denominator` in `Self`'s representation. + fn from_ratio(numerator: T, denominator: T) -> Self; + + /// Divide several numerators by one denominator. + /// + /// Implementations may prepare the denominator once; floating-point types + /// multiply every numerator by one reciprocal. + fn from_ratios(numerators: [T; N], denominator: T) -> [Self; N] + where + T: Copy, + Self: Sized, + { + numerators.map(|numerator| Self::from_ratio(numerator, denominator)) + } +} + +impl FromRatio for f32 { + #[inline] + fn from_ratio(numerator: f32, denominator: f32) -> Self { + numerator / denominator + } + + #[inline] + fn from_ratios(numerators: [f32; N], denominator: f32) -> [Self; N] { + let reciprocal = denominator.recip(); + numerators.map(|numerator| numerator * reciprocal) + } +} + +impl FromRatio for f64 { + #[inline] + fn from_ratio(numerator: f64, denominator: f64) -> Self { + numerator / denominator + } + + #[inline] + fn from_ratios(numerators: [f64; N], denominator: f64) -> [Self; N] { + let reciprocal = denominator.recip(); + numerators.map(|numerator| numerator * reciprocal) + } +} + /// Helper trait to unify over missing impl AsPrimitive for Wrapping pub(crate) trait AsFloat: Copy { fn as_f32(self) -> f32; @@ -121,7 +169,7 @@ pub trait Accu { // fn f64_as(value: f64) -> Self; } -/// Fixed point integer +/// Fixed-point value with storage `T`, accumulator `A`, and `F` fractional bits. /// /// Generics: /// * `T`: Base integer @@ -300,7 +348,10 @@ impl, const F: i8> Q { } impl, A: Mul, const F: i8> Q { - /// Multiply by a raw integer and keep the widened accumulator result. + /// Multiply a coefficient by a raw value without quantizing. + /// + /// Accumulate the returned `Q` values, then call [`Q::quantize`] + /// once. /// /// ``` /// # use dsp_fixedpoint::{Q, Q8}; @@ -312,6 +363,23 @@ impl, A: Mul, const F: i8> Q { } } +impl FromRatio for Q +where + T: Copy + Shift + Accu + Div, + A: Shift + Div, +{ + #[inline] + fn from_ratio(numerator: T, denominator: T) -> Self { + const { assert!(F > i8::MIN, "fractional bits must not be i8::MIN") } + let inner = if F > 0 { + T::down(numerator.up().shs(F) / denominator.up()) + } else { + numerator.shs(F) / denominator + }; + Self::new(inner) + } +} + impl, A: Shift + Mul, const F: i8> Q { /// Apply this fixed-point value as a gain to a raw integer and quantize. /// @@ -498,6 +566,23 @@ mod test { use super::*; use num_traits::{Bounded, FromPrimitive, Signed, ToPrimitive}; + #[test] + fn ratio() { + let third = Q32::<28>::from_ratio(1i32, 3); + assert!((third.as_f64() - 1.0 / 3.0).abs() < Q32::<28>::DELTA as f64); + assert_eq!(Q32::<8>::from_ratio(-3, 2), Q32::from_bits(-384)); + + let reciprocal = 3.0f32.recip(); + assert_eq!( + f32::from_ratios([1.0, 2.0], 3.0), + [reciprocal, 2.0 * reciprocal] + ); + assert_eq!( + Q32::<8>::from_ratios([-3, 3], 2), + [Q32::from_bits(-384), Q32::from_bits(384)] + ); + } + #[test] fn simple() { assert_eq!( diff --git a/dsp-process/README.md b/dsp-process/README.md index 957e97a..64113de 100644 --- a/dsp-process/README.md +++ b/dsp-process/README.md @@ -1,251 +1,109 @@ # `dsp-process` -`dsp-process` provides small `no_std` processing traits and composition adapters -for DSP code that needs to stay explicit about state, memory layout, and hot-path -costs. +Small `no_std` traits for static DSP composition. State, layout, and loop shape +remain explicit; no allocation or dynamic dispatch is required. -It was extracted from [`idsp`](https://github.com/quartiq/idsp), where the same -abstractions are used to build fixed-point and floating-point filters for embedded -and real-time signal-processing pipelines. The crate is intended for code that -cares about: +## Processing shapes -* predictable code generation -* `no_std` and no allocation -* separating immutable configuration from mutable runtime state -* composing filters without hiding the data layout -* sharing one configuration across many lanes or states +| Shape | Representation | Use | +| --- | --- | --- | +| one config, one state | `Split` | ordinary stateful filter | +| one config, many states | `Split::lanes()` | shared coefficients | +| many configs, one state | direct `SplitProcess` calls | prediction/correction phases | +| many config/state pairs | tuples, arrays, `Minor`, `Major` | static pipelines | -The root `idsp` repository also carries composite examples that show how these -primitives fit together in real DSP graphs. +`SplitProcess` is the primitive: `&self` is immutable configuration; +`&mut S` is caller-owned state. Different configurations may therefore operate +on the same state: -This README is the crate-level documentation via `#![doc = include_str!(...)]`. - -## Mission - -The core idea is simple: treat a DSP stage as a tiny object with a `process()` -method, and make composition cheap enough that it still works in embedded hot -paths. - -The crate is deliberately narrower than general stream-processing frameworks. It -does not try to model async execution, dynamic graphs, allocation, buffering -ownership, or runtime scheduling. It focuses on synchronous sample/slice -processing and on layouts that map cleanly to loops the compiler can optimize. - -## Core Traits - -The four main traits are: +```rust +use dsp_process::SplitProcess; + +struct State(i32); +struct Predict(i32); +struct Correct; + +impl SplitProcess<(), (), State> for Predict { + fn process(&self, state: &mut State, (): ()) { + state.0 += self.0; + } +} + +impl SplitProcess for Correct { + fn process(&self, state: &mut State, measurement: i32) -> i32 { + state.0 = (state.0 + measurement) / 2; + state.0 + } +} + +let mut state = State(2); +Predict(3).process(&mut state, ()); +assert_eq!(Correct.process(&mut state, 9), 7); +``` -* [`Process`]: stateful single-input processing, `&mut self` -* [`Inplace`]: stateful in-place processing -* [`SplitProcess`]: immutable configuration plus separate mutable state -* [`SplitInplace`]: in-place variant of `SplitProcess` +This is useful when processing has distinct phases but one state, such as a +Kalman transition and observation. Tuple composition instead gives each stage +its own state. -`SplitProcess` is the distinctive part of the crate. It lets one configuration be -reused across many independent states, which is a good fit for multi-lane DSP, -I/Q processing, polyphase banks, or coefficient sharing in embedded systems. +## Owned processors -## Basic Example +`Split` binds one configuration to one state and implements `Process`: ```rust -use dsp_process::{Process, Split, Offset, Gain}; +use dsp_process::{Offset, Process, Split}; let mut offset = Split::stateless(Offset(3)); assert_eq!(offset.process(5), 8); - -let mut gain = Split::stateless(Gain(4)); -assert_eq!(gain.process(5), 20); ``` -## Composition Example - -Serial composition uses tuples or arrays. Here two stateless stages are combined -into one processor: +One configuration can drive several independent states: ```rust -use dsp_process::{Process, Split, Offset, Gain}; +use dsp_process::{Offset, Process, Split}; -let mut pipeline = (Split::stateless(Offset(3)) * Split::stateless(Gain(4))).minor(); -assert_eq!(pipeline.process(5), 32); +let mut lanes = Split::stateless(Offset(3)).lanes::<2>(); +assert_eq!(lanes.process([1, 10]), [4, 13]); ``` -The tuple/array implementations cover common static compositions without dynamic -dispatch or heap allocation. - -## Split Configuration and Shared Coefficients +## Composition -A single filter configuration can be applied to multiple states: +Tuples and arrays form serial pipelines. `Parallel` forms branches. `Minor` and +`Major` select loop nesting and scratch placement. ```rust -use dsp_process::{Process, Split, Offset}; +use dsp_process::{Gain, Offset, Process, Split}; -let mut lanes = Split::stateless(Offset(3)).lanes::<2>(); -assert_eq!(lanes.process([1, 2]), [4, 5]); -assert_eq!(lanes.process([10, 20]), [13, 23]); +let mut pipeline = (Split::stateless(Offset(3)) * Split::stateless(Gain(4))).minor(); +assert_eq!(pipeline.process(5), 32); ``` -This is one of the main reasons the crate exists: the split form makes sharing -configuration explicit instead of forcing each lane to own a full copy. +Adapters (`Chunk`, `ChunkIn`, `ChunkOut`, `Interpolator`, `Decimator`, `Map`) +change rate or call shape without hiding state. -## Closures and Adapters +## Layout -`FnProcess` and `FnSplitProcess` let closures participate when that is the -clearest representation: - -```rust -use dsp_process::{Process, FnProcess}; - -let mut abs = FnProcess(|x: i32| x.abs()); -assert_eq!(abs.process(-7), 7); -``` - -Adapters such as [`Chunk`], [`ChunkIn`], [`ChunkOut`], [`ChunkInOut`], -[`Interpolator`], [`Decimator`], and [`Map`] help lift sample processors to chunk -processors and back without changing the underlying stage. - -## Layout and Composition Modes - -The crate supports several composition styles: - -* plain tuples and arrays for straightforward serial composition -* [`Minor`] for processor-minor/data-major composition -* [`Major`] for slice processing with explicit intermediate scratch -* [`Parallel`] for parallel branches -* [`Lanes`] for many states with shared configuration -* [`ByLane`] for explicit lane-major view processing - -These are not interchangeable in performance terms. The point is to make the -choice explicit. - -`Minor` tends to fit processors with small state and configuration. `Major` is -for cases where slice processing and explicit intermediate storage improve cache -behavior or register pressure. `Lanes` and `ByLane` pair with typed views so -multi-lane locality and vectorization can be expressed explicitly. - -## Context in `idsp` - -Inside `idsp`, these traits are used to express IIR sections, half-band filters, -decimators, interpolators, low-pass stages, and lock-in style pipelines, often on -fixed-point integer data and often with strong embedded constraints. The crate is -therefore biased toward: - -* static composition -* small reusable building blocks -* explicit scratch buffers -* low ceremony in `no_std` - -If your problem is “I have a hot DSP inner loop and want composition without -giving up control”, this crate is aimed at that. - -## Benefits and Unique Selling Points - -Compared with hand-written monolithic loops, `dsp-process` gives: - -* reusable composition primitives instead of copy-pasted loop nests -* explicit split config/state for coefficient sharing -* static dispatch and fixed layouts instead of runtime graph machinery -* adapters for common DSP reshaping tasks such as chunking, interpolation, and - decimation - -Compared with more general iterator or stream-style APIs, it keeps: - -* `no_std` support -* no allocation requirement -* layout control as part of the API -* a closer mapping between the public abstraction and the generated loop nest - -## Costs and Limitations - -This crate is intentionally not beginner-friendly in every corner. - -* The API is low level. Callers are expected to understand sample, slice, and - view layout. -* The traits are designed for hot paths, so some contracts are preconditions - rather than dynamically checked ergonomic errors. -* Static composition means type signatures can become large. -* Some adapters rely on const-generic shape relations that are correct but not - always obvious at the callsite. -* `Lanes` and `ByLane` use explicit views for layout-sensitive view - processing, which is more precise but also a more advanced API than ordinary - ordinary slice use. - -In short: this crate optimizes for control and performance first, convenience -second. - -## Alternatives - -Concisely: - -* Hand-written loops: maximal control, minimal reuse. Often best for a single - kernel, worse once many variants or compositions need to stay consistent. -* Iterator-heavy style: concise for non-hot code, usually a poor fit when state, - aliasing, and exact loop shape matter. -* Dynamic flowgraph runtimes such as GNU Radio or FutureSDR: better when the - graph, scheduling policy, runtime reconfiguration, or heterogeneous execution - are part of the problem. Those frameworks operate at a higher level around - blocks, buffers, message passing, and schedulers; `dsp-process` is much lower - level and closer to the inner kernels inside such blocks. -* Dynamic in-process graph libraries such as `dasp_graph`: better when nodes and - edges must be edited at runtime. `dsp-process` instead assumes fixed topology - and avoids graph ownership and scheduler concerns entirely. -* Static signal or graph composition libraries such as `dasp_signal` or FunDSP: - those also support static composition, but they focus on frame/signal or - audio-node abstractions. `dsp-process` is narrower: split config/state, - explicit view layout, `no_std`, and predictable loop shape are the center of - the design. -* Plain `Process`-only designs: simpler surface, but weaker support for - coefficient sharing across many states. - -`dsp-process` is most useful in the middle ground: fixed topology, static -composition, explicit state, and performance-sensitive DSP. - -## Notes on `Lanes` and `ByLane` - -`Lanes` and `ByLane` are layout tools, not just semantic conveniences. Their -layout-sensitive view behavior is explicit through -[`View<_, _, LaneMajor, _>`] and [`ViewMut<_, _, LaneMajor, _>`], so the -lane-major interpretation is visible at the type level instead of being -silently inferred from `[[X; N]]`. +Layout-sensitive processing uses typed views. `FrameMajor` and `LaneMajor` +make the physical interpretation part of the type; sample newtypes do not. ```rust use dsp_process::{LaneMajor, Offset, Split, View, ViewMut, ViewProcess}; -let mut p = Split::stateless(Offset(3)).lanes::<2>(); -let x = View::<_, LaneMajor, 2>::from_flat(&[1, 2, 3, 10, 20, 30], 3); -let mut y = [0; 6]; -let yb = ViewMut::<_, LaneMajor, 2>::from_flat(&mut y, 3); -ViewProcess::process_view(&mut p, x, yb); -assert_eq!(y, [4, 5, 6, 13, 23, 33]); +let mut lanes = Split::stateless(Offset(3)).lanes::<2>(); +let input = View::<_, LaneMajor, 2>::from_flat(&[1, 2, 3, 10, 20, 30], 3); +let mut output = [0; 6]; +let output = ViewMut::<_, LaneMajor, 2>::from_flat(&mut output, 3); +lanes.process_view(input, output); ``` -Chunk adapters remain useful alongside typed views. Use [`Split::per_frame()`] -and then call `process_frames()` or `inplace_frames()` on the resulting -processor to apply a chunk processor frame by frame to a frame-major view. - -## Guidance for Implementors +Use `Split::per_frame()` to apply a chunk processor to each frame of a +frame-major view. -As a rule of thumb: +## Implementing a stage -* implement [`SplitProcess`] for stateless/config-only processors -* implement [`Process`] for processors that carry all state internally -* implement [`SplitInplace`] or [`Inplace`] when a true in-place specialization - exists -* override `block()` only when it meaningfully improves the loop shape or memory - traffic +- Implement `SplitProcess` when configuration and state are distinct. +- Implement `Process` when one value naturally owns both. +- Implement `SplitInplace` or `Inplace` for a real in-place specialization. +- Override `block()` only to improve the loop or memory traffic. -## Small Reference Example - -```rust -use dsp_process::{Buffer, Inplace, Process}; - -let mut dly = Buffer::<[i32; 2]>::default(); -let y: i32 = dly.process(10); -assert_eq!(y, 0); -let y: i32 = dly.process(20); -assert_eq!(y, 0); -let y: i32 = dly.process(30); -assert_eq!(y, 10); - -let mut block = [1, 2, 3]; -dly.inplace(&mut block); -assert_eq!(block, [20, 30, 1]); -``` +Runtime fields hold values; const generics encode shape; wrappers encode +composition and layout. diff --git a/dsp-process/src/lib.rs b/dsp-process/src/lib.rs index c84c469..80476a5 100644 --- a/dsp-process/src/lib.rs +++ b/dsp-process/src/lib.rs @@ -1,29 +1,5 @@ #![no_std] #![doc = include_str!("../README.md")] -//! -//! ## API Parameterization Rules -//! -//! `dsp-process` uses a small set of parameterization rules throughout the API: -//! -//! - Use runtime fields for per-instance values that may reasonably vary without -//! changing the type, such as gains, offsets, clamps, held samples, and -//! downsample counts. -//! - Use const generics for structural facts that define shape, topology, or -//! fixed ratios at compile time, such as lane counts, chunk widths, -//! input/output ratios, slot indices, and view layouts. -//! - When a structural parameter also determines stored memory shape, encode it -//! in the field type itself, typically with arrays like `[T; N]`, rather than -//! duplicating it as both a runtime field and a const generic. -//! - Use wrapper newtypes for semantic adaptation and composition order -//! (`Chunk`, `Decimator`, `Lanes`, `Minor`, `Major`, `ByLane`) rather -//! than for user-tunable configuration. -//! - Use stream adapters with explicit state when phase matters across time -//! (`Downsample`, `Hold`, `Decimator`, `Interpolator`); use structural adapters -//! when only per-call shape changes (`Rate`, `ChunkIn`, `ChunkOut`, -//! `ChunkInOut`, view layout markers). -//! -//! In short: values live at runtime, shapes live in types, and composition -//! semantics live in wrappers. mod process; pub use process::*; diff --git a/dsp-process/src/process.rs b/dsp-process/src/process.rs index fb2af79..7575165 100644 --- a/dsp-process/src/process.rs +++ b/dsp-process/src/process.rs @@ -67,47 +67,11 @@ pub trait Inplace: Process { /// Processing with split state /// -/// Splitting configuration (the part of the filter that is unaffected -/// by processing inputs, e.g. "coefficients"), from state (the part -/// that is modified by processing) allows: -/// -/// * Separating mutable from immutable state guarantees consistency -/// (configuration can not change state and processing can -/// not change configuration) -/// * Reduces memory traffic when swapping configuration -/// * Allows the same filter to be applied to multiple states -/// (e.g. IQ data, multiple lanes) guaranteeing consistency, -/// reducing memory usage, and improving caching. -/// -/// This is the central abstraction used throughout `dsp-process`. A typical DSP -/// filter coefficient set becomes `Self`, while delay lines, accumulators, and -/// history buffers become the separate `state` argument. -/// -/// Use this when one configuration should drive many runtime states, or when it -/// is beneficial to keep mutable state small and move immutable data out of hot -/// loops. -/// -/// [`Process`] is often easier when state and configuration naturally live -/// together, while [`crate::Split`] turns a `SplitProcess` back into a stateful -/// [`Process`] value. +/// Immutable configuration operating on explicit mutable state. /// -/// # Examples -/// -/// ```rust -/// use dsp_process::SplitProcess; -/// -/// #[derive(Copy, Clone)] -/// struct Gain(i32); -/// -/// impl SplitProcess for Gain { -/// fn process(&self, _: &mut (), x: i32) -> i32 { -/// self.0 * x -/// } -/// } -/// -/// let mut state = (); -/// assert_eq!(Gain(4).process(&mut state, 3), 12); -/// ``` +/// One configuration may drive many states; several configurations may also +/// implement distinct phases over the same `S`. [`crate::Split`] binds +/// one pair into an ordinary [`Process`]. pub trait SplitProcess { /// Process an input into an output /// diff --git a/dsp-process/src/split.rs b/dsp-process/src/split.rs index 5d45339..fb5726e 100644 --- a/dsp-process/src/split.rs +++ b/dsp-process/src/split.rs @@ -9,13 +9,9 @@ use crate::{ /// A stateful processor assembled from split configuration and state. /// -/// [`Split`] is the bridge between [`SplitProcess`] and [`Process`]: it stores -/// the immutable configuration and mutable runtime state together so the pair can -/// be passed around as a conventional stateful processor. -/// -/// Reach for this when a split-state filter needs to be owned as one value, and -/// use [`lanes()`](Self::lanes), [`minor()`](Self::minor), or -/// [`major()`](Self::major) when changing how that owned processor is composed. +/// This binds one [`SplitProcess`] configuration to one state and exposes the +/// pair as [`Process`]. [`lanes()`](Self::lanes) instead shares the configuration +/// across independent states. /// /// # Examples /// diff --git a/src/kalman.rs b/src/kalman.rs new file mode 100644 index 0000000..f9a751d --- /dev/null +++ b/src/kalman.rs @@ -0,0 +1,632 @@ +//! Statically sized linear Kalman filters with scalar measurements. +//! +//! The implementation uses raw values for the estimate, covariance, and noise, +//! and coefficient values for `F`, `H`, and the Kalman gain. +//! Consequently fixed-point operation follows the same widened dot-product +//! pattern as [`crate::iir::Biquad`]: `coefficient * raw` products accumulate +//! wide and quantize once per result. +//! +//! [`RandomWalk`] is the smallest complete filter and an ordinary scalar DSP +//! stage: +//! +//! ``` +//! use dsp_process::{Process, Split}; +//! use idsp::kalman::{Estimate, RandomWalk}; +//! +//! let config = RandomWalk::::new(0.01, 1.0); +//! let mut filter = Split::new(config, Estimate::new([0.0], [[10.0]])); +//! let filtered = filter.process(2.0); +//! assert!(filtered > 0.0 && filtered < 2.0); +//! ``` +//! +//! Use [`ConstantVelocity`] with [`Direct`] for the common position sensor; +//! use [`Transition`] and [`Observation`] only when the matrices are dense. + +use core::{ + iter::Sum, + marker::PhantomData, + ops::{Add, Mul, Sub}, +}; + +use dsp_fixedpoint::FromRatio; +use dsp_process::{Map, SplitInplace, SplitProcess}; +use num_traits::{AsPrimitive, ConstOne, ConstZero}; + +/// State estimate and its error covariance. +/// +/// Fixed-point models must normalize all state components and measurements to +/// compatible numerical ranges. `covariance`, process noise, and measurement +/// noise must use one common raw covariance scale. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Estimate { + /// Estimated state vector. + pub state: [T; N], + /// Symmetric state-error covariance matrix. + /// + /// The transition reads the complete matrix. Prediction and correction + /// restore exact symmetry by computing the upper triangle and mirroring it. + pub covariance: [[T; N]; N], +} + +impl Estimate { + /// Construct an estimate from a state and covariance. + #[must_use] + pub const fn new(state: [T; N], covariance: [[T; N]; N]) -> Self { + Self { state, covariance } + } +} + +/// Linear state transition model. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Transition { + /// State transition matrix, conventionally `F`. + pub matrix: [[C; N]; N], + /// Symmetric process-noise covariance, conventionally `Q`. + /// + /// Only the upper triangle is used. + pub noise: [[T; N]; N], +} + +impl Transition { + /// Construct a transition model. + #[must_use] + pub const fn new(matrix: [[C; N]; N], noise: [[T; N]; N]) -> Self { + Self { matrix, noise } + } + + /// Advance an estimate through this transition model. + pub fn predict(&self, estimate: &mut Estimate) + where + T: 'static + Copy, + C: Copy + ConstOne + Mul, + A: Add + Sum + AsPrimitive, + { + const { assert!(N > 0, "a Kalman filter needs at least one state") } + + let state = estimate.state; + for (row, output) in self.matrix.iter().zip(&mut estimate.state) { + let sum: A = row.iter().zip(&state).map(|(&a, &b)| a * b).sum(); + *output = sum.as_(); + } + + // Store F*P. This is the only covariance-sized scratch matrix. + let mut fp = [[estimate.covariance[0][0]; N]; N]; + for (i, row) in fp.iter_mut().enumerate() { + for (j, output) in row.iter_mut().enumerate() { + let sum: A = self.matrix[i] + .iter() + .zip(&estimate.covariance) + .map(|(&a, row)| a * row[j]) + .sum(); + *output = sum.as_(); + } + } + + // F*P*F' + Q is symmetric. Compute one triangle and mirror it. + for (i, fp) in fp.iter().enumerate() { + for j in i..N { + let sum: A = self.matrix[j].iter().zip(fp).map(|(&a, &b)| a * b).sum(); + let covariance = (sum + C::ONE * self.noise[i][j]).as_(); + estimate.covariance[i][j] = covariance; + estimate.covariance[j][i] = covariance; + } + } + } +} + +impl Transition +where + C: Copy + ConstOne + ConstZero, + T: Copy, +{ + /// Construct an identity transition with the supplied process noise. + #[must_use] + pub fn identity(noise: [[T; N]; N]) -> Self { + let mut matrix = [[C::ZERO; N]; N]; + for (i, row) in matrix.iter_mut().enumerate() { + row[i] = C::ONE; + } + Self::new(matrix, noise) + } +} + +/// Linear scalar observation model. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Observation { + /// Scalar measurement matrix row, conventionally `H`. + pub matrix: [C; N], + /// Scalar measurement-noise variance, conventionally `R`. + pub noise: T, +} + +impl Observation { + /// Construct a scalar observation model. + #[must_use] + pub const fn new(matrix: [C; N], noise: T) -> Self { + Self { matrix, noise } + } + + /// Project the state estimate into measurement space. + #[must_use] + pub fn project(&self, estimate: &Estimate) -> T + where + T: 'static + Copy, + C: Copy + Mul, + A: Sum + AsPrimitive, + { + const { assert!(N > 0, "a Kalman filter needs at least one state") } + let sum: A = self + .matrix + .iter() + .zip(&estimate.state) + .map(|(&a, &b)| a * b) + .sum(); + sum.as_() + } + + /// Correct an estimate with one scalar measurement. + /// + /// Returns the posterior measurement estimate. The innovation covariance + /// must be positive and the calculated gain must fit `C`. + pub fn correct(&self, estimate: &mut Estimate, measurement: T) -> T + where + T: 'static + Copy, + C: Copy + ConstOne + FromRatio + Mul, + A: Add + Sub + Sum + AsPrimitive, + { + const { assert!(N > 0, "a Kalman filter needs at least one state") } + + let expected: A = self + .matrix + .iter() + .zip(&estimate.state) + .map(|(&a, &b)| a * b) + .sum(); + let innovation = (C::ONE * measurement - expected).as_(); + + // u = P*H' + let mut cross = [estimate.covariance[0][0]; N]; + for (row, output) in estimate.covariance.iter().zip(&mut cross) { + let sum: A = self.matrix.iter().zip(row).map(|(&a, &b)| a * b).sum(); + *output = sum.as_(); + } + + // s = H*u + R + let variance: A = self.matrix.iter().zip(&cross).map(|(&a, &b)| a * b).sum(); + let variance = (variance + C::ONE * self.noise).as_(); + finish_correction::( + estimate, + innovation, + cross, + variance, + self.noise, + |values| { + let sum: A = self.matrix.iter().zip(values).map(|(&a, &b)| a * b).sum(); + sum.as_() + }, + ) + } +} + +/// Direct observation of state component `I`. +/// +/// Unlike a dense [`Observation`], this carries no zero coefficients and lets +/// correction reduce `H*x`, `P*H'`, and `H*P*H'` to indexing. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Direct { + /// Scalar measurement-noise variance, conventionally `R`. + pub noise: T, + coefficient: PhantomData C>, +} + +impl Direct { + /// Construct a direct observation. + #[must_use] + pub const fn new(noise: T) -> Self { + Self { + noise, + coefficient: PhantomData, + } + } + + /// Project the state estimate into measurement space. + #[must_use] + pub fn project(&self, estimate: &Estimate) -> T + where + T: Copy, + { + const { assert!(I < N, "state index out of range") } + estimate.state[I] + } + + /// Correct an estimate with one scalar measurement. + /// + /// Returns the posterior measurement estimate. The innovation covariance + /// must be positive and the calculated gain must fit `C`. + pub fn correct(&self, estimate: &mut Estimate, measurement: T) -> T + where + T: 'static + Copy, + C: Copy + ConstOne + FromRatio + Mul, + A: Add + Sub + AsPrimitive, + { + const { assert!(I < N, "state index out of range") } + + let innovation = (C::ONE * measurement - C::ONE * estimate.state[I]).as_(); + let mut cross = [estimate.covariance[0][I]; N]; + for (row, output) in estimate.covariance.iter().zip(&mut cross) { + *output = row[I]; + } + let variance = (C::ONE * cross[I] + C::ONE * self.noise).as_(); + finish_correction::( + estimate, + innovation, + cross, + variance, + self.noise, + |values| values[I], + ) + } +} + +/// Constant-velocity motion model for `[position, velocity]`. +/// +/// Its transition matrix is `[[1, interval], [0, 1]]`. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct ConstantVelocity { + /// Sample interval. + pub interval: C, + /// Symmetric process-noise covariance. Only the upper triangle is used. + pub noise: [[T; 2]; 2], +} + +impl ConstantVelocity { + /// Construct a constant-velocity transition. + #[must_use] + pub const fn new(interval: C, noise: [[T; 2]; 2]) -> Self { + Self { interval, noise } + } + + /// Advance a position-velocity estimate. + pub fn predict(&self, estimate: &mut Estimate) + where + T: 'static + Copy, + C: Copy + ConstOne + Mul, + A: Add + AsPrimitive, + { + let [[p00, p01], [p10, p11]] = estimate.covariance; + estimate.state[0] = (C::ONE * estimate.state[0] + self.interval * estimate.state[1]).as_(); + + let fp00 = (C::ONE * p00 + self.interval * p10).as_(); + let fp01 = (C::ONE * p01 + self.interval * p11).as_(); + let p00 = (C::ONE * fp00 + self.interval * fp01 + C::ONE * self.noise[0][0]).as_(); + let p01 = (C::ONE * fp01 + C::ONE * self.noise[0][1]).as_(); + let p11 = (C::ONE * p11 + C::ONE * self.noise[1][1]).as_(); + estimate.covariance = [[p00, p01], [p01, p11]]; + } +} + +/// One prediction phase followed by one measurement-update phase. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Kalman { + /// Prediction phase. + pub predict: P, + /// Measurement-update phase. + pub update: U, +} + +impl Kalman { + /// Compose prediction and measurement-update phases over shared state. + #[must_use] + pub const fn new(predict: P, update: U) -> Self { + Self { predict, update } + } + + /// Run prediction but skip the measurement update when input is absent. + #[must_use] + pub fn optional(self) -> Kalman> { + Kalman::new(self.predict, Map(self.update)) + } +} + +/// Dense linear transition and scalar measurement model. +pub type DenseKalman = Kalman, Observation>; + +/// Complete scalar random-walk filter with `F = H = 1`. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct RandomWalk { + /// Scalar process-noise variance. + pub process_noise: T, + /// Scalar measurement-noise variance. + pub measurement_noise: T, + coefficient: PhantomData C>, +} + +impl RandomWalk { + /// Construct a scalar random-walk filter. + #[must_use] + pub const fn new(process_noise: T, measurement_noise: T) -> Self { + Self { + process_noise, + measurement_noise, + coefficient: PhantomData, + } + } +} + +impl SplitProcess<(), (), Estimate> for Transition +where + T: 'static + Copy, + C: Copy + ConstOne + Mul, + A: Add + Sum + AsPrimitive, +{ + fn process(&self, estimate: &mut Estimate, (): ()) { + self.predict(estimate); + } +} + +impl SplitProcess> for Observation +where + T: 'static + Copy, + C: Copy + ConstOne + FromRatio + Mul, + A: Add + Sub + Sum + AsPrimitive, +{ + fn process(&self, estimate: &mut Estimate, measurement: T) -> T { + self.correct(estimate, measurement) + } +} + +impl SplitInplace> for Observation +where + T: Copy, + Self: SplitProcess>, +{ +} + +impl SplitProcess> for Direct +where + T: 'static + Copy, + C: Copy + ConstOne + FromRatio + Mul, + A: Add + Sub + AsPrimitive, +{ + fn process(&self, estimate: &mut Estimate, measurement: T) -> T { + self.correct(estimate, measurement) + } +} + +impl SplitInplace> for Direct +where + T: Copy, + Self: SplitProcess>, +{ +} + +impl SplitProcess<(), (), Estimate> for ConstantVelocity +where + T: 'static + Copy, + C: Copy + ConstOne + Mul, + A: Add + AsPrimitive, +{ + fn process(&self, estimate: &mut Estimate, (): ()) { + self.predict(estimate); + } +} + +impl SplitProcess for Kalman +where + P: SplitProcess<(), (), S>, + U: SplitProcess, +{ + fn process(&self, state: &mut S, input: X) -> Y { + self.predict.process(state, ()); + self.update.process(state, input) + } +} + +impl SplitInplace for Kalman where Self: SplitProcess {} + +impl SplitProcess> for RandomWalk +where + T: 'static + Copy, + C: Copy + ConstOne + FromRatio + Mul, + A: Add + Sub + AsPrimitive, +{ + fn process(&self, estimate: &mut Estimate, measurement: T) -> T { + estimate.covariance[0][0] = + (C::ONE * estimate.covariance[0][0] + C::ONE * self.process_noise).as_(); + Direct::::new(self.measurement_noise).correct(estimate, measurement) + } +} + +impl SplitInplace> for RandomWalk +where + T: Copy, + Self: SplitProcess>, +{ +} + +fn finish_correction( + estimate: &mut Estimate, + innovation: T, + cross: [T; N], + variance: T, + noise: T, + project: F, +) -> T +where + T: 'static + Copy, + C: Copy + ConstOne + FromRatio + Mul, + A: Add + Sub + AsPrimitive, + F: Fn(&[T; N]) -> T, +{ + let gain = C::from_ratios(cross, variance); + + for (state, gain) in estimate.state.iter_mut().zip(gain.iter().copied()) { + *state = (C::ONE * *state + gain * innovation).as_(); + } + + // Scalar Joseph update. P1 = P - K*(P*H')'. + for (i, gain) in gain.iter().copied().enumerate() { + for (j, cross) in cross.iter().copied().enumerate() { + estimate.covariance[i][j] = (C::ONE * estimate.covariance[i][j] - gain * cross).as_(); + } + } + + // v = P1*H'. + let mut projected = [project(&estimate.covariance[0]); N]; + for (row, output) in estimate.covariance.iter().zip(&mut projected) { + *output = project(row); + } + + // P+ = P1 - v*K' + (K*R)*K'. Compute one triangle and mirror it. + for (i, gain_i) in gain.iter().copied().enumerate() { + let weighted_noise = (gain_i * noise).as_(); + for (j, gain_j) in gain.iter().copied().enumerate().skip(i) { + let covariance = (C::ONE * estimate.covariance[i][j] - gain_j * projected[i] + + gain_j * weighted_noise) + .as_(); + estimate.covariance[i][j] = covariance; + estimate.covariance[j][i] = covariance; + } + } + + project(&estimate.state) +} + +#[cfg(test)] +mod tests { + use super::*; + use dsp_fixedpoint::Q32; + use dsp_process::{Process, Split}; + + const EPS: f64 = 1e-12; + + fn assert_close(actual: f64, expected: f64) { + assert!((actual - expected).abs() < EPS, "{actual} != {expected}"); + } + + #[test] + fn scalar_reference() { + let mut filter = Split::new( + RandomWalk::::new(0.25, 4.0), + Estimate::new([2.0], [[9.0]]), + ); + + let output = filter.process(5.0); + let prior_covariance = 9.25; + let gain = prior_covariance / (prior_covariance + 4.0); + let expected = 2.0 + gain * (5.0 - 2.0); + let expected_covariance = (1.0 - gain) * prior_covariance; + + assert_close(output, expected); + assert_close(filter.state.state[0], expected); + assert_close(filter.state.covariance[0][0], expected_covariance); + } + + #[test] + fn transition_and_observation_match_combined_process() { + let transition = ConstantVelocity::new(1.0, [[0.1, 0.0], [0.0, 0.1]]); + let observation = Direct::::new(0.5); + let config = Kalman::new(transition, observation); + let estimate = Estimate::new([0.0, 1.0], [[2.0, 0.0], [0.0, 1.0]]); + let mut separate = estimate; + let mut combined = Split::new(config, estimate); + + transition.predict(&mut separate); + assert_eq!(separate.state, [1.0, 1.0]); + assert_eq!(separate.covariance, [[3.1, 1.0], [1.0, 1.1]]); + let expected = observation.correct(&mut separate, 1.25); + let actual = combined.process(1.25); + + assert_eq!(actual, expected); + assert_eq!(combined.state, separate); + assert_eq!( + combined.state.covariance[0][1], + combined.state.covariance[1][0] + ); + } + + #[test] + fn direct_matches_dense() { + let estimate = Estimate::new([2.0, -1.0], [[4.0, 1.5], [1.5, 3.0]]); + let mut direct = estimate; + let mut dense = estimate; + + let direct_output = Direct::::new(0.75).correct(&mut direct, 3.5); + let dense_output = Observation::new([1.0, 0.0], 0.75).correct(&mut dense, 3.5); + + assert_eq!(direct_output, dense_output); + assert_eq!(direct, dense); + } + + #[test] + fn joseph_matches_closed_form() { + let observation = Observation::new([1.25, -0.5], 0.75); + let prior = Estimate::new([2.0, -1.0], [[4.0, 1.5], [1.5, 3.0]]); + let mut actual = prior; + + let output = observation.correct(&mut actual, 3.5); + + let h = observation.matrix; + let u = [ + prior.covariance[0][0] * h[0] + prior.covariance[0][1] * h[1], + prior.covariance[1][0] * h[0] + prior.covariance[1][1] * h[1], + ]; + let variance = h[0] * u[0] + h[1] * u[1] + observation.noise; + let gain = [u[0] / variance, u[1] / variance]; + let innovation = 3.5 - h[0] * prior.state[0] - h[1] * prior.state[1]; + let state = [ + prior.state[0] + gain[0] * innovation, + prior.state[1] + gain[1] * innovation, + ]; + for (actual, expected) in actual.state.iter().zip(state) { + assert_close(*actual, expected); + } + for (i, (actual, prior)) in actual.covariance.iter().zip(prior.covariance).enumerate() { + for (j, (actual, prior)) in actual.iter().zip(prior).enumerate() { + assert_close(*actual, prior - gain[i] * u[j]); + } + } + assert_close(output, h[0] * state[0] + h[1] * state[1]); + } + + #[test] + fn optional_measurement_still_predicts() { + let config = Kalman::new( + ConstantVelocity::new(1.0, [[0.0; 2]; 2]), + Direct::::new(1.0), + ) + .optional(); + let mut filter = Split::new(config, Estimate::new([2.0, 3.0], [[1.0, 0.0], [0.0, 1.0]])); + + let output = filter.process(None); + + assert_eq!(output, None); + assert_eq!(filter.state.state, [5.0, 3.0]); + } + + #[test] + fn fixed_point_tracks_float() { + type C = Q32<28>; + + let transition = ConstantVelocity::new(C::from_f32(0.25), [[16, 0], [0, 4]]); + let observation = Direct::::new(64); + let mut fixed = Split::new( + Kalman::new(transition, observation), + Estimate::new([0, 40], [[256, 0], [0, 64]]), + ); + let float_transition = ConstantVelocity::new(0.25, [[16.0, 0.0], [0.0, 4.0]]); + let float_observation = Direct::::new(64.0); + let mut float = Split::new( + Kalman::new(float_transition, float_observation), + Estimate::new([0.0, 40.0], [[256.0, 0.0], [0.0, 64.0]]), + ); + + for measurement in [12, 24, 37, 49, 63, 74] { + let fixed_output = fixed.process(measurement); + let float_output = float.process(measurement as f64); + assert!((fixed_output as f64 - float_output).abs() < 3.0); + } + + assert_eq!(fixed.state.covariance[0][1], fixed.state.covariance[1][0]); + } +} diff --git a/src/lib.rs b/src/lib.rs index a5891a0..dc8f31d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,6 +11,7 @@ mod cossin; pub use cossin::*; mod lockin; pub use lockin::*; +pub mod kalman; mod lowpass; pub use lowpass::*; mod pll; diff --git a/tests/embedded/README.md b/tests/embedded/README.md index a4ebba2..cf60d3f 100644 --- a/tests/embedded/README.md +++ b/tests/embedded/README.md @@ -7,6 +7,9 @@ * Large slice, small chunk, single sample and inplace processing * `idsp` IIR biquads in various configurations * `idsp` compared with `biquad-rs` +* Scalar-measurement Kalman filters: structural N=1 random walk, dense versus + structural N=2 constant velocity, and dense versus direct-measurement N=4 + constant jerk, using floating- and fixed-point arithmetic * `hbf` FIRs * Tooling routines: `cossin`, `atan2` diff --git a/tests/embedded/src/bin/kalman.rs b/tests/embedded/src/bin/kalman.rs new file mode 100644 index 0000000..f28747d --- /dev/null +++ b/tests/embedded/src/bin/kalman.rs @@ -0,0 +1,132 @@ +#![no_std] +#![no_main] + +use cortex_m::asm; +use defmt::*; +use dsp_fixedpoint::Q32; +use dsp_process::Split; +use idsp::kalman::{ + ConstantVelocity, Direct, Estimate, Kalman, Observation, RandomWalk, Transition, +}; +use num_traits::{ConstOne, ConstZero}; + +use idsp_embedded_bench::*; + +const KINEMATIC_4: [[f32; 4]; 4] = [ + [1.0, 0.25, 0.03125, 1.0 / 384.0], + [0.0, 1.0, 0.25, 0.03125], + [0.0, 0.0, 1.0, 0.25], + [0.0, 0.0, 0.0, 1.0], +]; + +#[cortex_m_rt::entry] +fn main() -> ! { + info!("Setup"); + let mut c = unwrap!(cortex_m::Peripherals::take()); + c.DCB.enable_trace(); + c.DWT.enable_cycle_counter(); + + info!("Starting"); + CyclesResults::header(); + + bench_inplace(&mut Split::new( + RandomWalk::::new(0.01, 1.0), + Estimate::new([0.0], [[10.0]]), + )) + .show("kalman f32 n1 structural"); + + let estimate = Estimate::new([0.0; 2], [[10.0, 0.0], [0.0, 1.0]]); + bench_inplace(&mut Split::new( + Kalman::new( + Transition::new([[1.0, 1.0], [0.0, 1.0]], [[0.01, 0.0], [0.0, 0.01]]), + Observation::new([1.0, 0.0], 1.0), + ), + estimate, + )) + .show("kalman f32 n2 dense"); + bench_inplace(&mut Split::new( + Kalman::new( + ConstantVelocity::new(1.0, [[0.01, 0.0], [0.0, 0.01]]), + Direct::::new(1.0), + ), + estimate, + )) + .show("kalman f32 n2 structural"); + + let transition = Transition::new( + KINEMATIC_4, + [ + [0.01, 0.0, 0.0, 0.0], + [0.0, 0.01, 0.0, 0.0], + [0.0, 0.0, 0.01, 0.0], + [0.0, 0.0, 0.0, 0.01], + ], + ); + let estimate = Estimate::new( + [0.0; 4], + [ + [10.0, 0.0, 0.0, 0.0], + [0.0, 4.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 0.25], + ], + ); + bench_inplace(&mut Split::new( + Kalman::new(transition, Observation::new([1.0, 0.0, 0.0, 0.0], 1.0)), + estimate, + )) + .show("kalman f32 n4 dense"); + bench_inplace(&mut Split::new( + Kalman::new(transition, Direct::::new(1.0)), + estimate, + )) + .show("kalman f32 n4 direct"); + + type C = Q32<28>; + let transition = Transition::new( + [[C::ONE, C::from_f32(0.25)], [C::ZERO, C::ONE]], + [[16, 0], [0, 4]], + ); + let estimate = Estimate::new([0, 40], [[256, 0], [0, 64]]); + bench_inplace(&mut Split::new( + Kalman::new(transition, Observation::new([C::ONE, C::ZERO], 64)), + estimate, + )) + .show("kalman q32 n2 dense"); + bench_inplace(&mut Split::new( + Kalman::new( + ConstantVelocity::new(C::from_f32(0.25), [[16, 0], [0, 4]]), + Direct::::new(64), + ), + estimate, + )) + .show("kalman q32 n2 structural"); + + let transition = Transition::new( + KINEMATIC_4.map(|row| row.map(C::from_f32)), + [[16, 0, 0, 0], [0, 4, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], + ); + let estimate = Estimate::new( + [0, 40, 0, 0], + [[256, 0, 0, 0], [0, 64, 0, 0], [0, 0, 16, 0], [0, 0, 0, 4]], + ); + bench_inplace(&mut Split::new( + Kalman::new( + transition, + Observation::new([C::ONE, C::ZERO, C::ZERO, C::ZERO], 64), + ), + estimate, + )) + .show("kalman q32 n4 dense"); + bench_inplace(&mut Split::new( + Kalman::new(transition, Direct::::new(64)), + estimate, + )) + .show("kalman q32 n4 direct"); + + info!("Done"); + asm::bkpt(); + loop { + asm::nop(); + } +}