eml-stdlib 0.4.0__tar.gz → 0.5.0__tar.gz

{eml_stdlib-0.4.0 → eml_stdlib-0.5.0}/PKG-INFO

@@ -1,10 +1,10 @@
 Metadata-Version: 2.4
 Name: eml-stdlib
-Version: 0.4.0
+Version: 0.5.0
 Summary: EML standard-library prototype for game development. PBR shaders, procedural noise, procedural textures, lighting, terrain, animation curves, particles, audio attenuation + Doppler, rigid-body physics, and math/signal/control/biology/ballistics/ML kernels with explicit verification-contract metadata where available.
 Author: Mosa Creates LLC
-Author-email: "Arturo R. Almaguer" <almaguer1986@gmail.com>
-Maintainer-email: "Arturo R. Almaguer" <almaguer1986@gmail.com>
+Author-email: "Arturo R. Almaguer" <contact@monogate.dev>
+Maintainer-email: "Arturo R. Almaguer" <contact@monogate.dev>
 License: MIT
 Project-URL: Homepage, https://monogate.dev/learn/eml/intro
 Project-URL: Repository, https://github.com/agent-maestro/eml-stdlib
@@ -36,27 +36,26 @@ Dynamic: license-file
 
 # eml-stdlib
 
-**An EML standard-library prototype for game development.**
-Shaders, textures, and physics formulas carry explicit verification contracts
-or proof-obligation hooks where available.
+**The first verified standard library for game development.**
+Every shader, every texture, every physics formula — with a mathematical proof.
 
 Plus the math, signal-processing, control, biology, ballistics, and ML kernels
-that built the rest of the Monogate stack. Modules are intended to compile to
-GDScript, GLSL, HLSL, C, Python, and Lean through Forge-supported targets;
-`@verify` contracts are tracked as proof obligations against MachLib.
+that built the rest of the Monogate stack. Every module compiles to GDScript,
+GLSL, HLSL, C, Python, and Lean (and ~30 other backends via Forge); every
+`@verify` contract has a Lean proof obligation against MachLib.
 
 ```bash
 pip install eml-stdlib
 eml-compile eml_stdlib/gaming/shading/pbr_specular.eml --target glsl
 ```
 
-And get a PBR specular shader with explicit verification metadata.
+And get a verified PBR specular shader.
 
 ## At a glance
 
-- **178 modules** across **22 categories**
-- **521 functions** total
-- **466 carry an `@verify(lean, …)` contract** (89%)
+- **185 modules** across **22 categories**
+- **550 functions** total
+- **476 carry an `@verify(lean, …)` contract** (86%)
 - Every function declares its **Pfaffian chain order** in the type
 
 ## Used by
@@ -142,18 +141,25 @@ DSP — filters, FFT, windows, matched-filter.
 | `window_hamming.eml` | hamming, hamming_at_start, hamming_apply | 0,1 | 3/3 | **new** |
 | `window_hann.eml` | hann, hann_at_start, hann_apply | 0,1 | 3/3 | **new** |
 
-### `control/` — 6 modules / 12 fns / 12 verified
+### `control/` — 13 modules / 41 fns / 22 verified
 
 Control — PID, Kalman, LQR, hysteresis, deadband.
 
 | File | Functions | Chain | Verified | Origin |
 |------|-----------|-------|----------|--------|
+| `attitude_quaternion.eml` | quat_mul, quat_conjugate, quat_rotate_vec, quat_normalize | 0 | 1/4 | **new** |
 | `bangbang.eml` | bangbang, bangbang_symmetric | 0 | 2/2 | **new** |
+| `complementary_filter.eml` | integrate, fuse, fuse_bounded, fuse_tilt | 0 | 2/4 | **new** |
 | `deadband.eml` | deadband, deadband_asymmetric | 0 | 2/2 | **new** |
+| `ekf_range_bearing.eml` | range, range_jacobian, bearing_jacobian | 0 | 1/3 | **new** |
 | `hysteresis.eml` | hysteresis_step | 0 | 1/1 | **new** |
+| `kalman2d_predict.eml` | predict_state, cov_predict | 0 | 1/2 | **new** |
+| `kalman2d_update.eml` | innovation, gain, update_state, … (+3) | 0 | 1/6 | **new** |
+| `kalman2d_update_matrix.eml` | innovation, innovation_cov, gain_matrix, … (+4) | 0 | 2/7 | **new** |
 | `kalman_1d.eml` | kalman1d_predict, kalman1d_update, kalman1d_step | 0 | 3/3 | **new** |
 | `lqr_1d.eml` | riccati, lqr_gain, lqr_control | 0,1 | 3/3 | **new** |
 | `pid.eml` | pid | 0 | 1/1 | forge/examples/pid_controller.eml |
+| `weighted_fusion.eml` | fuse3, fuse3_bounded, fuse4_bounded | 0 | 2/3 | **new** |
 
 ### `physics/` — 9 modules / 30 fns / 28 verified
 

{eml_stdlib-0.4.0 → eml_stdlib-0.5.0}/README.md

@@ -1,26 +1,25 @@
 # eml-stdlib
 
-**An EML standard-library prototype for game development.**
-Shaders, textures, and physics formulas carry explicit verification contracts
-or proof-obligation hooks where available.
+**The first verified standard library for game development.**
+Every shader, every texture, every physics formula — with a mathematical proof.
 
 Plus the math, signal-processing, control, biology, ballistics, and ML kernels
-that built the rest of the Monogate stack. Modules are intended to compile to
-GDScript, GLSL, HLSL, C, Python, and Lean through Forge-supported targets;
-`@verify` contracts are tracked as proof obligations against MachLib.
+that built the rest of the Monogate stack. Every module compiles to GDScript,
+GLSL, HLSL, C, Python, and Lean (and ~30 other backends via Forge); every
+`@verify` contract has a Lean proof obligation against MachLib.
 
 ```bash
 pip install eml-stdlib
 eml-compile eml_stdlib/gaming/shading/pbr_specular.eml --target glsl
 ```
 
-And get a PBR specular shader with explicit verification metadata.
+And get a verified PBR specular shader.
 
 ## At a glance
 
-- **178 modules** across **22 categories**
-- **521 functions** total
-- **466 carry an `@verify(lean, …)` contract** (89%)
+- **185 modules** across **22 categories**
+- **550 functions** total
+- **476 carry an `@verify(lean, …)` contract** (86%)
 - Every function declares its **Pfaffian chain order** in the type
 
 ## Used by
@@ -106,18 +105,25 @@ DSP — filters, FFT, windows, matched-filter.
 | `window_hamming.eml` | hamming, hamming_at_start, hamming_apply | 0,1 | 3/3 | **new** |
 | `window_hann.eml` | hann, hann_at_start, hann_apply | 0,1 | 3/3 | **new** |
 
-### `control/` — 6 modules / 12 fns / 12 verified
+### `control/` — 13 modules / 41 fns / 22 verified
 
 Control — PID, Kalman, LQR, hysteresis, deadband.
 
 | File | Functions | Chain | Verified | Origin |
 |------|-----------|-------|----------|--------|
+| `attitude_quaternion.eml` | quat_mul, quat_conjugate, quat_rotate_vec, quat_normalize | 0 | 1/4 | **new** |
 | `bangbang.eml` | bangbang, bangbang_symmetric | 0 | 2/2 | **new** |
+| `complementary_filter.eml` | integrate, fuse, fuse_bounded, fuse_tilt | 0 | 2/4 | **new** |
 | `deadband.eml` | deadband, deadband_asymmetric | 0 | 2/2 | **new** |
+| `ekf_range_bearing.eml` | range, range_jacobian, bearing_jacobian | 0 | 1/3 | **new** |
 | `hysteresis.eml` | hysteresis_step | 0 | 1/1 | **new** |
+| `kalman2d_predict.eml` | predict_state, cov_predict | 0 | 1/2 | **new** |
+| `kalman2d_update.eml` | innovation, gain, update_state, … (+3) | 0 | 1/6 | **new** |
+| `kalman2d_update_matrix.eml` | innovation, innovation_cov, gain_matrix, … (+4) | 0 | 2/7 | **new** |
 | `kalman_1d.eml` | kalman1d_predict, kalman1d_update, kalman1d_step | 0 | 3/3 | **new** |
 | `lqr_1d.eml` | riccati, lqr_gain, lqr_control | 0,1 | 3/3 | **new** |
 | `pid.eml` | pid | 0 | 1/1 | forge/examples/pid_controller.eml |
+| `weighted_fusion.eml` | fuse3, fuse3_bounded, fuse4_bounded | 0 | 2/3 | **new** |
 
 ### `physics/` — 9 modules / 30 fns / 28 verified
 

{eml_stdlib-0.4.0 → eml_stdlib-0.5.0}/eml_stdlib/__init__.py

@@ -19,7 +19,7 @@ import json
 from pathlib import Path
 from typing import Any
 
-__version__ = "0.4.0"
+__version__ = "0.5.0"
 
 _PACKAGE_ROOT = Path(__file__).resolve().parent
 _CATALOG_PATH = _PACKAGE_ROOT / "catalog.json"

{eml_stdlib-0.4.0 → eml_stdlib-0.5.0}/eml_stdlib/ballistics/wind.eml

@@ -34,14 +34,14 @@ fn relative_airspeed(v_bullet: Real, v_wind_along: Real) -> Real
 // radians, measured from the shooter's forward direction.
 //   v_perp  =  v_wind * sin(bearing - heading)
 //   v_along = -v_wind * cos(bearing - heading)
-@verify(lean, theorem = "wind_components_pythagorean")
+@verify(lean, theorem = "wind_perp_component")
 fn perp_component(v_wind: Real, bearing: Real, heading: Real) -> Real
     where chain_order <= 1
 {
     v_wind * sin(bearing - heading)
 }
 
-@verify(lean, theorem = "wind_components_pythagorean")
+@verify(lean, theorem = "wind_along_component")
 fn along_component(v_wind: Real, bearing: Real, heading: Real) -> Real
     where chain_order <= 1
 {

{eml_stdlib-0.4.0 → eml_stdlib-0.5.0}/eml_stdlib/catalog.json

@@ -1283,6 +1283,35 @@
     "description": "the textbook resistive divider, the",
     "source": "forge/examples/voltage_divider.eml"
   },
+  {
+    "category": "control",
+    "file": "control/attitude_quaternion.eml",
+    "module": "attitude_quaternion",
+    "functions": [
+      {
+        "name": "quat_mul",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "quat_conjugate",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "quat_rotate_vec",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "quat_normalize",
+        "chain_order": 0,
+        "verified": true
+      }
+    ],
+    "description": "quaternion attitude representation, the standard",
+    "source": "new"
+  },
   {
     "category": "control",
     "file": "control/bangbang.eml",
@@ -1302,6 +1331,35 @@
     "description": "bang-bang (relay) controller.",
     "source": "new"
   },
+  {
+    "category": "control",
+    "file": "control/complementary_filter.eml",
+    "module": "complementary_filter",
+    "functions": [
+      {
+        "name": "integrate",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "fuse",
+        "chain_order": 0,
+        "verified": true
+      },
+      {
+        "name": "fuse_bounded",
+        "chain_order": 0,
+        "verified": true
+      },
+      {
+        "name": "fuse_tilt",
+        "chain_order": 0,
+        "verified": false
+      }
+    ],
+    "description": "IMU tilt estimation by gyro/accelerometer",
+    "source": "new"
+  },
   {
     "category": "control",
     "file": "control/deadband.eml",
@@ -1321,6 +1379,30 @@
     "description": "two-sided dead-zone (deadband) saturation.",
     "source": "new"
   },
+  {
+    "category": "control",
+    "file": "control/ekf_range_bearing.eml",
+    "module": "ekf_range_bearing",
+    "functions": [
+      {
+        "name": "range",
+        "chain_order": 0,
+        "verified": true
+      },
+      {
+        "name": "range_jacobian",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "bearing_jacobian",
+        "chain_order": 0,
+        "verified": false
+      }
+    ],
+    "description": "Extended Kalman Filter measurement model for a",
+    "source": "new"
+  },
   {
     "category": "control",
     "file": "control/hysteresis.eml",
@@ -1335,6 +1417,108 @@
     "description": "Schmitt-trigger / hysteresis switching.",
     "source": "new"
   },
+  {
+    "category": "control",
+    "file": "control/kalman2d_predict.eml",
+    "module": "kalman2d_predict",
+    "functions": [
+      {
+        "name": "predict_state",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "cov_predict",
+        "chain_order": 0,
+        "verified": true
+      }
+    ],
+    "description": "N-D Kalman covariance propagation (predict step).",
+    "source": "new"
+  },
+  {
+    "category": "control",
+    "file": "control/kalman2d_update.eml",
+    "module": "kalman2d_update",
+    "functions": [
+      {
+        "name": "innovation",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "gain",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "update_state",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "update_cov",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "joseph_factor",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "update_cov_joseph",
+        "chain_order": 0,
+        "verified": true
+      }
+    ],
+    "description": "2-state Kalman filter, measurement-UPDATE step.",
+    "source": "new"
+  },
+  {
+    "category": "control",
+    "file": "control/kalman2d_update_matrix.eml",
+    "module": "kalman2d_update_matrix",
+    "functions": [
+      {
+        "name": "innovation",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "innovation_cov",
+        "chain_order": 0,
+        "verified": true
+      },
+      {
+        "name": "gain_matrix",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "update_state",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "update_cov",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "joseph_factor",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "update_cov_joseph",
+        "chain_order": 0,
+        "verified": true
+      }
+    ],
+    "description": "2-state Kalman filter, VECTOR-measurement",
+    "source": "new"
+  },
   {
     "category": "control",
     "file": "control/kalman_1d.eml",
@@ -1397,6 +1581,30 @@
     "description": "the textbook PID controller.",
     "source": "forge/examples/pid_controller.eml"
   },
+  {
+    "category": "control",
+    "file": "control/weighted_fusion.eml",
+    "module": "weighted_fusion",
+    "functions": [
+      {
+        "name": "fuse3",
+        "chain_order": 0,
+        "verified": false
+      },
+      {
+        "name": "fuse3_bounded",
+        "chain_order": 0,
+        "verified": true
+      },
+      {
+        "name": "fuse4_bounded",
+        "chain_order": 0,
+        "verified": true
+      }
+    ],
+    "description": "confidence-weighted fusion of redundant sensors.",
+    "source": "new"
+  },
   {
     "category": "gaming/animation",
     "file": "gaming/animation/bounce.eml",

eml_stdlib-0.5.0/eml_stdlib/control/attitude_quaternion.eml

@@ -0,0 +1,75 @@
+// attitude_quaternion.eml -- quaternion attitude representation, the standard
+// state of a 3D orientation / attitude estimator (drones, spacecraft, AHRS).
+//
+// A rotation in 3D is carried as a unit quaternion q = (w, x, y, z): no gimbal
+// lock (unlike Euler angles), and composition is a single product (unlike
+// rotation matrices, 9 numbers with 6 constraints). An attitude filter keeps
+// q as its state, propagates it from the gyro, and corrects it from the
+// accelerometer/magnetometer — the nonlinear rung above the linear/EKF filters
+// in this category.
+//
+// All four kernels are order-0 and element-wise, so Forge emits them to every
+// target: quat_mul / quat_conjugate reach RTL (pure multiply/accumulate) and
+// are ULP-equal across C/Rust/C++/Java; quat_normalize divides (software +
+// proof, not RTL).
+//
+// The flagship is quat_normalize: an attitude filter must RE-NORMALISE the
+// quaternion each step, because gyro integration walks the state off the unit
+// sphere (‖q‖ drifts from 1). @verify proves the result is EXACTLY a unit
+// quaternion, ‖normalize(q)‖² = 1 — sorryAx-free, the same whole-vector
+// unit-norm proof the `normalize` kernel carries, now at dimension 4.
+
+module attitude_quaternion;
+
+// Hamilton product q ⊗ r -- composition of two rotations (apply r, then q).
+// (w, x, y, z) convention. Not commutative (rotations don't commute).
+fn quat_mul(q: Vec<4>, r: Vec<4>) -> Vec<4>
+    where chain_order <= 0
+{
+    [q[0]*r[0] - q[1]*r[1] - q[2]*r[2] - q[3]*r[3],
+     q[0]*r[1] + q[1]*r[0] + q[2]*r[3] - q[3]*r[2],
+     q[0]*r[2] - q[1]*r[3] + q[2]*r[0] + q[3]*r[1],
+     q[0]*r[3] + q[1]*r[2] - q[2]*r[1] + q[3]*r[0]]
+}
+
+// Conjugate q* = (w, -x, -y, -z). For a UNIT quaternion this is the inverse
+// rotation (q* = q⁻¹), so it rotates a vector back / expresses the opposite
+// frame transform.
+fn quat_conjugate(q: Vec<4>) -> Vec<4>
+    where chain_order <= 0
+{
+    [q[0], 0.0 - q[1], 0.0 - q[2], 0.0 - q[3]]
+}
+
+// Rotate a 3-vector by a unit quaternion: v' = q ⊗ (0,v) ⊗ q*. Written as the
+// expanded rotation matrix R(q)·v (which equals q⊗v⊗q* for a unit q), so it
+// stays a pure multiply/accumulate expression — no quaternion temporaries —
+// and reaches RTL. This is how an attitude estimate is USED: map a body-frame
+// vector to the world frame (or, via quat_conjugate, world gravity into the
+// body frame to compare against the accelerometer).
+fn quat_rotate_vec(q: Vec<4>, v: Vec<3>) -> Vec<3>
+    where chain_order <= 0
+{
+    [(1.0 - 2.0*(q[2]*q[2] + q[3]*q[3]))*v[0]
+        + 2.0*(q[1]*q[2] - q[0]*q[3])*v[1]
+        + 2.0*(q[1]*q[3] + q[0]*q[2])*v[2],
+     2.0*(q[1]*q[2] + q[0]*q[3])*v[0]
+        + (1.0 - 2.0*(q[1]*q[1] + q[3]*q[3]))*v[1]
+        + 2.0*(q[2]*q[3] - q[0]*q[1])*v[2],
+     2.0*(q[1]*q[3] - q[0]*q[2])*v[0]
+        + 2.0*(q[2]*q[3] + q[0]*q[1])*v[1]
+        + (1.0 - 2.0*(q[1]*q[1] + q[2]*q[2]))*v[2]]
+}
+
+// Re-normalise onto the unit sphere: q / ‖q‖. The projection back to a valid
+// rotation an attitude filter applies every step (gyro integration drifts off
+// the sphere). PROVEN: the result is EXACTLY a unit quaternion, ‖result‖² = 1
+// -- the whole-vector unit-norm contract, sorryAx-free, at dimension 4.
+@verify(lean, theorem = "quat_normalize_unit")
+fn quat_normalize(q: Vec<4>) -> Vec<4>
+    where chain_order <= 0
+    requires (dot(q, q) > 0.0)
+    ensures (dot(result, result) == 1.0)
+{
+    q / sqrt(dot(q, q))
+}

eml_stdlib-0.5.0/eml_stdlib/control/complementary_filter.eml

@@ -0,0 +1,89 @@
+// complementary_filter.eml -- IMU tilt estimation by gyro/accelerometer
+// sensor fusion (the workhorse of drones, self-balancing robots, phones).
+//
+// A gyroscope integrates cleanly over short horizons but DRIFTS without
+// bound; an accelerometer gives an absolute tilt from gravity but is noisy
+// and corrupted by linear acceleration. The complementary filter blends the
+// two with a single tuning weight α ∈ [0,1]:
+//
+//   θ_gyro⁺ = θ + ω·dt                       (integrate the gyro rate)
+//   θ⁺      = α·θ_gyro⁺ + (1-α)·θ_accel       (convex blend toward accel)
+//
+// α near 1 trusts the gyro (smooth, follows fast motion); α near 0 trusts
+// the accelerometer (drift-free, noisy). It is a one-line, division-free,
+// order-0 kernel -- ideal for a microcontroller ISR -- yet it is exactly a
+// 1-state steady-state Kalman filter, so it earns its place next to the
+// full 2D filter in this category.
+//
+// VERIFIED, stated honestly:
+//   * `fuse` carries an @verify(lean) contract that a convex blend of two
+//     NON-NEGATIVE estimates stays non-negative -- proven sorryAx-free.
+//   * `fuse_bounded` carries the STRONGER property that is the filter's whole
+//     reason for existing: INTERVAL PRESERVATION. If both estimates lie in a
+//     sensor's physical range [lo, hi], the fused estimate stays in [lo, hi]
+//     for ANY α ∈ [0,1] -- the anti-drift guarantee (a gyro alone would walk
+//     out of range unboundedly). This is a convexity bound, not a sign fact,
+//     so the positivity closers can't reach it; it is discharged by the
+//     MachLib lemmas `convex_comb_le` / `convex_comb_ge` (classical-free),
+//     proven sorryAx-free. (An earlier revision left this as a documented
+//     `sorry`; the convex-combination lemmas closed it.)
+
+module complementary_filter;
+
+// Gyro dead-reckoning step: θ_gyro⁺ = θ + ω·dt.  Affine, and INTENTIONALLY
+// unverified -- integrating a rate has no bounded-output invariant to prove;
+// its unbounded drift is precisely what the fusion step below corrects.
+fn integrate(theta: Real, omega: Real, dt: Real) -> Real
+    where chain_order <= 0
+{
+    theta + omega * dt
+}
+
+// Convex blend of the gyro-integrated angle and the accelerometer angle:
+// θ⁺ = α·g + (1-α)·a.  Proven: for α ∈ [0,1] and non-negative inputs the
+// fused estimate is non-negative (a convex combination of non-negatives).
+@verify(lean, theorem = "complementary_fuse_nonneg")
+fn fuse(gyro_est: Real, accel_est: Real, alpha: Real) -> Real
+    where chain_order <= 0
+    requires (alpha >= 0.0)
+    requires (alpha <= 1.0)
+    requires (gyro_est >= 0.0)
+    requires (accel_est >= 0.0)
+    ensures (result >= 0.0)
+{
+    alpha * gyro_est + (1.0 - alpha) * accel_est
+}
+
+// Interval-preserving blend: same convex combination, but PROVEN to keep the
+// fused estimate inside the sensor's physical range [lo, hi] for any
+// α ∈ [0,1] given both inputs are in range. This is the anti-drift guarantee
+// -- the whole point of fusing an absolute (accel) reference into a drifting
+// (gyro) one. lo/hi are the sensor range (e.g. tilt ∈ [-π, π]); they are
+// specification parameters, not used by the arithmetic.
+@verify(lean, theorem = "complementary_fuse_in_range")
+fn fuse_bounded(gyro_est: Real, accel_est: Real, alpha: Real,
+                lo: Real, hi: Real) -> Real
+    where chain_order <= 0
+    requires (alpha >= 0.0)
+    requires (alpha <= 1.0)
+    requires (gyro_est <= hi)
+    requires (accel_est <= hi)
+    requires (gyro_est >= lo)
+    requires (accel_est >= lo)
+    ensures (result <= hi)
+    ensures (result >= lo)
+{
+    alpha * gyro_est + (1.0 - alpha) * accel_est
+}
+
+// One-shot tilt update: integrate the gyro then blend toward the
+// accelerometer, θ⁺ = α·(θ + ω·dt) + (1-α)·θ_accel.  The form embedded
+// filters actually run each IMU sample.
+fn fuse_tilt(theta: Real, omega: Real, accel_angle: Real,
+             alpha: Real, dt: Real) -> Real
+    where chain_order <= 0
+    requires (alpha >= 0.0)
+    requires (alpha <= 1.0)
+{
+    alpha * (theta + omega * dt) + (1.0 - alpha) * accel_angle
+}

eml_stdlib-0.5.0/eml_stdlib/control/ekf_range_bearing.eml

@@ -0,0 +1,60 @@
+// ekf_range_bearing.eml -- Extended Kalman Filter measurement model for a
+// range/bearing sensor observing a known landmark from a 2D robot pose
+// (x, y, θ). The canonical EKF-SLAM / robot-localisation observation.
+//
+// A linear Kalman filter cannot use a range/bearing sensor: the measurement
+//   h(x) = [ √(Δx² + Δy²) ,  atan2(Δy, Δx) − θ ]   (Δx = lx−x, Δy = ly−y)
+// is NONLINEAR in the pose. The EXTENDED Kalman filter linearises it about
+// the current estimate with the measurement Jacobian H = ∂h/∂x, then reuses
+// the ordinary matrix update (see kalman2d_update_matrix.eml). Providing H
+// is the whole job of the EKF measurement model -- that is what this file is.
+//
+// The Jacobian rows are RATIONAL in (Δx, Δy, r) -- no atan2 needed for the
+// derivative even though the bearing measurement itself would be -- so they
+// lower to plain scalar arithmetic every Forge backend emits:
+//
+//   ∂range/∂(x,y)   = [ −Δx/r ,  −Δy/r ]         (unit vector, landmark→robot)
+//   ∂bearing/∂(x,y) = [  Δy/r² , −Δx/r² ]
+//   ∂bearing/∂θ     = −1                          (constant; omitted here)
+//
+// VERIFIED vs NUMERICAL, stated honestly:
+//   * `range` carries an @verify(lean) contract that the measured range is
+//     non-negative (a Euclidean distance) -- proven sorryAx-free.
+//   * The Jacobian rows are NUMERICALLY verified (cross-target vs a reference)
+//     but carry no proof contract: they divide by r (and r²), so a sound
+//     contract needs r>0 side conditions and division reasoning -- deferred,
+//     not asserted.
+
+module ekf_range_bearing;
+
+// Range measurement: r = √((lx−x)² + (ly−y)²).  A Euclidean distance, so
+// non-negative for ANY pose/landmark -- proven, not assumed.
+@verify(lean, theorem = "ekf_range_nonneg")
+fn range(x: Real, y: Real, lx: Real, ly: Real) -> Real
+    where chain_order <= 0
+    ensures (result >= 0.0)
+{
+    sqrt((lx - x)*(lx - x) + (ly - y)*(ly - y))
+}
+
+// Range row of the measurement Jacobian: ∂range/∂(x,y) = [−Δx/r, −Δy/r].
+// The unit vector pointing from the landmark toward the robot. Requires a
+// non-degenerate observation (robot not exactly on the landmark, r>0).
+fn range_jacobian(x: Real, y: Real, lx: Real, ly: Real) -> Vec<2>
+    where chain_order <= 0
+    requires ((lx - x)*(lx - x) + (ly - y)*(ly - y) > 0.0)
+{
+    [(x - lx) / sqrt((lx - x)*(lx - x) + (ly - y)*(ly - y)),
+     (y - ly) / sqrt((lx - x)*(lx - x) + (ly - y)*(ly - y))]
+}
+
+// Bearing row of the measurement Jacobian: ∂bearing/∂(x,y) = [Δy/r², −Δx/r²].
+// Rational in the deltas -- the bearing derivative needs no atan2. Requires
+// r²>0 (same non-degeneracy).
+fn bearing_jacobian(x: Real, y: Real, lx: Real, ly: Real) -> Vec<2>
+    where chain_order <= 0
+    requires ((lx - x)*(lx - x) + (ly - y)*(ly - y) > 0.0)
+{
+    [(ly - y) / ((lx - x)*(lx - x) + (ly - y)*(ly - y)),
+     (x - lx) / ((lx - x)*(lx - x) + (ly - y)*(ly - y))]
+}