Transforms — Sim, EKF, UKF, INS, LQR

The compile-time siblings. Each takes a World, writes its math symbolically over the shared linearized system, and emits a typed Module. Lower one with a target to get a callable runtime.

Sim

manta.Sim

Sim(world, *, discretization='exact', parameters=None)

Forward-dynamics transform: model validation + the linearized tick, emitting oracle/deploy Modules.

Source code in manta/sim.py

def __init__(self, world: World, *,
             discretization: str = "exact",
             parameters: list[str] | None = None) -> None:
    # Model validation (planet prep, requires_fields/requires_planet,
    # craft back-pointers) happens inside LinearizedSystem — the one
    # choke point every transform passes through. `discretization`
    # selects how predict_jacobian discretizes F ("exact" | "euler" —
    # see LinearizedSystem; "euler" trades an O(dt²) jacobian
    # difference for much smaller generated deploy code).
    # `parameters` promotes the named promotable Parameters to a live
    # `params` port on every emitted Module (system ID — `manta.fit`);
    # passing the port's declared defaults reproduces the baked model
    # bit-for-bit.
    self._sys = LinearizedSystem(           # full state, all sensors
        world, discretization=discretization, parameters=parameters)
    self.world = self._sys.world
    self.crafts = self._sys.crafts
    self.model = self._sys.model

tick property

tick

The compiled world tick (named CasADi I/O).

module

module()

The scheduled simulation-truth Module.

Measurements with the same declared positive rate are emitted as one sample_group_* entry and omitted from the plant step outputs. Backends can therefore schedule the kernel without evaluating its symbolic dependencies on every physics tick. Measurements with no rate remain inline and are evaluated every tick.

Source code in manta/sim.py

def module(self) -> Module:
    """The scheduled simulation-truth Module.

    Measurements with the same declared positive rate are emitted as one
    ``sample_group_*`` entry and omitted from the plant ``step`` outputs.
    Backends can therefore schedule the kernel without evaluating its
    symbolic dependencies on every physics tick. Measurements with no
    rate remain inline and are evaluated every tick.
    """
    return self._oracle_module(schedule_rate_limited=True)

inline_module

inline_module()

The all-inline simulation oracle for smooth/batched callers.

Every measurement is returned by step regardless of declared rate. Rate metadata remains present, but this artifact deliberately performs no acquisition scheduling.

Source code in manta/sim.py

def inline_module(self) -> Module:
    """The all-inline simulation oracle for smooth/batched callers.

    Every measurement is returned by ``step`` regardless of declared
    rate. Rate metadata remains present, but this artifact deliberately
    performs no acquisition scheduling.
    """
    return self._oracle_module(schedule_rate_limited=False)

deploy_module

deploy_module()

The deploy Module (runs on a robot against real sensors): noiseless forward map + per-sensor measurement models + Jacobians.

Source code in manta/sim.py

def deploy_module(self) -> Module:
    """The **deploy** Module (runs on a robot against real sensors):
    noiseless forward map + per-sensor measurement models + Jacobians."""
    sys = self._sys
    spec = sys.spec
    x_field, u_port, dtp, tp, meas_ports = self._module_scaffold()
    # A measurement is dt-independent — dt is eliminated at construction,
    # so the measure kernels honestly take (x, u, t).
    tan = spec.tangent_dim
    zero_dt = ca.MX.zeros(1, 1)
    functions = {"predict": sys.predict_fn, "predict_jacobian": sys.F_fn}
    p_port = self._param_port()
    p_ref = () if p_port is None else (PortRef("params"),)
    ports = [u_port, *((p_port,) if p_port is not None else ()),
             dtp, tp, *meas_ports,
             Port("F", Role.MATRIX, (tan, tan))]
    entries = [
        EntryPoint("predict", "predict",
                   (StateRef("x"), PortRef("u"), *p_ref, PortRef("dt"),
                    PortRef("t")), writes=("x",)),
        EntryPoint("predict_jacobian", "predict_jacobian",
                   (StateRef("x"), PortRef("u"), *p_ref, PortRef("dt"),
                    PortRef("t")), returns=("F",)),
    ]
    margs = [sys.x_sym, sys.u_sym, sys.t_sym]
    margn = ["x", "u", "t"]
    if p_port is not None:
        margs.insert(2, sys.p_sym)
        margn.insert(2, "p")
    for full, s in sys.sensors.items():
        ident = entry_ident(full)
        h = ca.substitute(s.h_sym, sys.dt_sym, zero_dt)
        H = ca.substitute(s.H_sym, sys.dt_sym, zero_dt)
        functions[f"measure_{ident}"] = ca.Function(
            f"h_{ident}", margs, [h], margn, ["h"])
        functions[f"measure_{ident}_jacobian"] = ca.Function(
            f"H_{ident}", margs, [H], margn, ["H"])
        ports.append(Port(f"H_{ident}", Role.MATRIX, (s.dim, tan)))
        entries.append(EntryPoint(
            f"measure_{ident}", f"measure_{ident}",
            (StateRef("x"), PortRef("u"), *p_ref, PortRef("t")),
            returns=(full,)))
        entries.append(EntryPoint(
            f"measure_{ident}_jacobian", f"measure_{ident}_jacobian",
            (StateRef("x"), PortRef("u"), *p_ref, PortRef("t")),
            returns=(f"H_{ident}",)))
    return Module(
        name=self.world.name, state=StateLayout((x_field,)),
        ports=tuple(ports), functions=functions,
        entry_points=tuple(entries), kind=ModuleKind.KERNEL,
        hosting=Hosting.THREADED,
        metadata=self.model.transform_metadata({
            "transform": "deploy_model",
            "discretization": sys.discretization,
            "parameters": tuple(p.full for p in sys.param_specs),
        }))

EKF

manta.EKF

EKF(world, *, track=None, sensors=None, inputs=None, discretization='exact', gates=None)

Bases: _FilterBase

Error-state EKF over a World — symbolic recursion + typed Module. The analysis surface (module, n_blocks, observability, sigma_horizon) is the shared _FilterBase tail.

Args: track: {craft_name: SlotSet} lower bound of what to estimate (closed under the dynamics; the rest freezes). None keeps the full state. sensors: measurement full-names (or unambiguous suffixes). None keeps every output (of tracked crafts). inputs: known control inputs; None keeps all, excluded ones freeze at their default. discretization: how F discretizes the dynamics — "exact" (default; jacobian of the full discrete tick) or "euler" (F = I + dt·∂ẋ/∂δ; O(dt²) from exact, much smaller generated deploy code). See LinearizedSystem. gates: optional normalized-innovation-squared threshold: one positive scalar for every sensor, or a mapping from sensor name/suffix to threshold. Rejected updates leave both state and covariance unchanged while still returning their innovation diagnostics.

Source code in manta/estimation/ekf.py

def __init__(self, world, *,
             track: dict | None = None,
             sensors: list[str] | None = None,
             inputs: list[str] | None = None,
             discretization: str = "exact",
             gates: float | dict[str, float] | None = None) -> None:
    """Args:
        track:   `{craft_name: SlotSet}` lower bound of what to estimate
                 (closed under the dynamics; the rest freezes). `None`
                 keeps the full state.
        sensors: measurement full-names (or unambiguous suffixes).
                 `None` keeps every output (of tracked crafts).
        inputs:  known control inputs; `None` keeps all, excluded ones
                 freeze at their default.
        discretization: how F discretizes the dynamics — "exact"
                 (default; jacobian of the full discrete tick) or
                 "euler" (F = I + dt·∂ẋ/∂δ; O(dt²) from exact, much
                 smaller generated deploy code). See LinearizedSystem.
        gates:   optional normalized-innovation-squared threshold: one
                 positive scalar for every sensor, or a mapping from
                 sensor name/suffix to threshold. Rejected updates leave
                 both state and covariance unchanged while still
                 returning their innovation diagnostics.
    """
    sys = LinearizedSystem(world, track=track, sensors=sensors,
                           inputs=inputs, track_mode="closure",
                           discretization=discretization)
    require_measurement_only_consider(sys, who="EKF")
    self._bind_system(world, sys)
    resolved_gates = resolve_gates(sys, gates, who="EKF")

    # ---- the Kalman recursion, symbolically, once -------------------
    spec, n_tan = sys.spec, sys.spec.tangent_dim
    n_consider = consider_dimension(sys)
    x, u = sys.x_sym, sys.u_sym
    dt, t = sys.dt_sym, sys.t_sym
    P = ca.MX.sym("P", n_tan, n_tan)
    P_consider = (
        ca.MX.sym("P_consider", n_tan, n_consider)
        if n_consider else None
    )
    Q = ca.MX.sym("Q", n_tan, n_tan)
    F = sys.F_sym

    # predict: auto process noise Q = L Σ Lᵀ baked into the kernel
    # (zero when the model declares none) + an explicit-Q override.
    Q_auto = _q_auto(sys)
    predict_inputs = [x, P] + ([P_consider] if n_consider else [])
    predict_input_names = ["x", "P"] + (
        ["P_consider"] if n_consider else []
    )
    predict_outputs = [
        sys.x_new,
        symmetrize(F @ P @ F.T + Q_auto),
    ] + ([F @ P_consider] if n_consider else [])
    predict_output_names = ["x_new", "P_new"] + (
        ["P_consider_new"] if n_consider else []
    )
    predict_fn = ca.Function(
        "ekf_predict", [*predict_inputs, u, dt, t], predict_outputs,
        [*predict_input_names, "u", "dt", "t"], predict_output_names)
    predict_q_fn = ca.Function(
        "ekf_predict_with_Q", [*predict_inputs, Q, u, dt, t],
        [
            sys.x_new,
            symmetrize(F @ P @ F.T + Q),
            *([F @ P_consider] if n_consider else []),
        ],
        [*predict_input_names, "Q", "u", "dt", "t"],
        predict_output_names)

    # per-sensor Joseph update (the shared `joseph_update` kernel —
    # see estimation/_kalman.py). `prepared_sensors` eliminates dt and
    # refuses σ=0 sensors — the kernel honestly takes only
    # (x, P, z, u, t).
    x0 = initial_ambient(sys.world, spec)
    zero_dt = ca.MX.zeros(1, 1)
    updates: dict[str, ca.Function] = {}
    diagnostic_updates: dict[str, ca.Function] = {}
    override_updates: dict[str, ca.Function] = {}
    for ps in prepared_sensors(sys, spec, x0=x0, who="EKF"):
        H = ca.substitute(sys.sensors[ps.full].H_sym, dt, zero_dt)
        threshold = resolved_gates[ps.full]

        def expressions(R, ps=ps, H=H, threshold=threshold):
            if n_consider:
                x_candidate, P_candidate, C_candidate, nu, S = (
                    schmidt_update(
                        x,
                        P,
                        P_consider,
                        ca.MX.eye(n_consider),
                        ps.h,
                        H,
                        ps.consider_H,
                        R,
                        ps.z,
                        spec,
                    )
                )
            else:
                x_candidate, P_candidate, nu, S = joseph_update(
                    x, P, ps.h, H, R, ps.z, spec)
            nis = ca.dot(nu, spd_solve(S, nu))
            accepted = (ca.MX.ones(1, 1) if threshold is None
                        else nis <= threshold)
            x_result = ca.if_else(accepted, x_candidate, x)
            P_result = ca.if_else(accepted, P_candidate, P)
            states = [x_result, P_result]
            if n_consider:
                states.append(ca.if_else(
                    accepted, C_candidate, P_consider
                ))
            return (*states, nu, S, nis, accepted)

        values = expressions(ps.R)
        n_state_outputs = 3 if n_consider else 2
        state_values = values[:n_state_outputs]
        state_input_values = [x, P] + (
            [P_consider] if n_consider else []
        )
        state_input_names = ["x", "P"] + (
            ["P_consider"] if n_consider else []
        )
        state_output_names = ["x_new", "P_new"] + (
            ["P_consider_new"] if n_consider else []
        )
        updates[ps.full] = ca.Function(
            f"ekf_update_{entry_ident(ps.full)}",
            [*state_input_values, ps.z, u, t], state_values,
            [*state_input_names, "z", "u", "t"], state_output_names)
        diagnostic_updates[ps.full] = ca.Function(
            f"ekf_update_diagnostic_{entry_ident(ps.full)}",
            [*state_input_values, ps.z, u, t], values,
            [*state_input_names, "z", "u", "t"],
            [*state_output_names, "innovation", "innovation_covariance",
             "nis", "accepted"])
        R_override = ca.MX.sym(f"R_{entry_ident(ps.full)}", ps.dim,
                               ps.dim)
        # Runtime covariance replaces device-reportable white noise.
        # Ordinary non-overrideable model noise remains additive; static
        # calibration uncertainty is already carried by the Schmidt
        # consider state and must not be folded into white R here.
        overridden = expressions(R_override + ps.model_R)
        override_updates[ps.full] = ca.Function(
            f"ekf_update_with_R_{entry_ident(ps.full)}",
            [*state_input_values, ps.z, R_override, u, t], overridden,
            [*state_input_names, "z", "R", "u", "t"],
            [*state_output_names, "innovation", "innovation_covariance",
             "nis", "accepted"])

    self._module = emit_filter_module(
        sys, spec, name=f"{world.name}_ekf", x0=x0,
        predict_fn=predict_fn, predict_q_fn=predict_q_fn,
        updates=updates, diagnostic_updates=diagnostic_updates,
        override_updates=override_updates, gates=resolved_gates,
        consider_dim=n_consider)

UKF

manta.UKF

UKF(world, *, track=None, sensors=None, inputs=None, alpha=None, beta=2.0, kappa=0.0, mean_iters=1, jitter=1e-12, gates=None)

Bases: _FilterBase

Error-state UKF over a World — symbolic sigma-point recursion + typed Module. Drop-in alternative to EKF with the same constructor, runtime surface, and emitted Module shape. The analysis surface (module, n_blocks, observability, sigma_horizon) is the shared _FilterBase tail.

Args: track: {craft_name: SlotSet} lower bound of what to estimate (closed under the dynamics; the rest freezes). None keeps the full state. sensors: measurement full-names (or unambiguous suffixes). None keeps every output (of tracked crafts). inputs: known control inputs; None keeps all, excluded ones freeze at their default. alpha: sigma-point spread (0 < α ≤ 1). None (default) resolves to min(1, √(3/n)) for tangent dim n, which pins the spread at γ = √(n+λ) = √3 — i.e. sigma points at ~1.7σ — independent of state size. The two failure modes this dodges are both real: α=1 spreads points at √n·σ, which at robotics-sized n wraps rotation offsets past π and corrupts the re-summarized covariance; the textbook "small α" (1e-3) drives the central covariance weight to ≈ −n/α² (−10⁶ at n=13, the classic indefinite-P failure) while shrinking the spread until the transform is a noisy finite difference of the EKF. beta: prior-knowledge term (2.0 is optimal for a Gaussian). kappa: secondary scaling (0.0 by default). mean_iters: retraction steps for the predict's manifold mean (1 suffices for the default spread; raise it for a wide spread on a strongly-curved manifold). jitter: variance floor (added as jitter·I, and as a pivot floor) when factoring P into sigma points. P is an iterated covariance, so roundoff can leave it marginally indefinite; the jitter turns the NaN cliff into a regularized factor. 0 disables. gates: optional normalized-innovation-squared threshold: one positive scalar for every sensor, or a mapping from sensor name/suffix to threshold. Rejection preserves the prior state and covariance.

An explicit tuning that produces a negative central covariance weight (w_c[0] < 0) is accepted but warns: the covariance sums lose their PSD guarantee and rely on the jitter backstop. The auto default produces a mildly negative w_c[0] once the tangent dimension exceeds n ≈ 11 (w_c[0] = 3−n)/3 + 3 − 3/n; the price of the bounded √3·σ spread), which the sigma-point Joseph update form and the jitter absorb. Either way the dependency is explicit: a negative w_c[0] REQUIRES jitter > 0 (construction refuses jitter=0 in that regime) and the resolved weights are published as UKF.sigma_weights and in the Module metadata under "unscented" so the tuning the artifact was built with is inspectable.

Unlike the EKF there is no discretization knob: the UKF pushes sigma points through the exact nonlinear discrete tick f, so the Euler/exact distinction (which only shapes the EKF's linearized F) does not arise.

Source code in manta/estimation/ukf.py

def __init__(self, world, *,
             track: dict | None = None,
             sensors: list[str] | None = None,
             inputs: list[str] | None = None,
             alpha: float | None = None,
             beta: float = 2.0,
             kappa: float = 0.0,
             mean_iters: int = 1,
             jitter: float = 1e-12,
             gates: float | dict[str, float] | None = None) -> None:
    """Args:
        track:   `{craft_name: SlotSet}` lower bound of what to estimate
                 (closed under the dynamics; the rest freezes). `None`
                 keeps the full state.
        sensors: measurement full-names (or unambiguous suffixes).
                 `None` keeps every output (of tracked crafts).
        inputs:  known control inputs; `None` keeps all, excluded ones
                 freeze at their default.
        alpha:   sigma-point spread (0 < α ≤ 1). `None` (default)
                 resolves to `min(1, √(3/n))` for tangent dim `n`, which
                 pins the spread at `γ = √(n+λ) = √3` — i.e. sigma
                 points at ~1.7σ — *independent of state size*. The two
                 failure modes this dodges are both real: α=1 spreads
                 points at `√n`·σ, which at robotics-sized `n` wraps
                 rotation offsets past π and corrupts the re-summarized
                 covariance; the textbook "small α" (1e-3) drives the
                 central covariance weight to ≈ −n/α² (−10⁶ at n=13,
                 the classic indefinite-P failure) while shrinking the
                 spread until the transform is a noisy finite
                 difference of the EKF.
        beta:    prior-knowledge term (2.0 is optimal for a Gaussian).
        kappa:   secondary scaling (0.0 by default).
        mean_iters: retraction steps for the predict's manifold mean
                 (1 suffices for the default spread; raise it for a
                 wide spread on a strongly-curved manifold).
        jitter:  variance floor (added as `jitter·I`, and as a pivot
                 floor) when factoring P into sigma points. P is an
                 iterated covariance, so roundoff can leave it
                 marginally indefinite; the jitter turns the NaN cliff
                 into a regularized factor. 0 disables.
        gates:   optional normalized-innovation-squared threshold: one
                 positive scalar for every sensor, or a mapping from
                 sensor name/suffix to threshold. Rejection preserves
                 the prior state and covariance.

    An *explicit* tuning that produces a negative central covariance
    weight (`w_c[0] < 0`) is accepted but warns: the covariance sums
    lose their PSD guarantee and rely on the jitter backstop. The auto
    default produces a mildly negative `w_c[0]` once the tangent
    dimension exceeds `n ≈ 11` (`w_c[0] = 3−n)/3 + 3 − 3/n`; the price
    of the bounded √3·σ spread), which the sigma-point Joseph update
    form and the jitter absorb. Either way the dependency is explicit:
    a negative `w_c[0]` REQUIRES `jitter > 0` (construction refuses
    `jitter=0` in that regime) and the resolved weights are published
    as `UKF.sigma_weights` and in the Module metadata under
    ``"unscented"`` so the tuning the artifact was built with is
    inspectable.

    Unlike the EKF there is no `discretization` knob: the UKF pushes
    sigma points through the exact nonlinear discrete tick `f`, so the
    Euler/exact distinction (which only shapes the EKF's linearized F)
    does not arise.
    """
    sys = LinearizedSystem(world, track=track, sensors=sensors,
                           inputs=inputs, track_mode="closure")
    if consider_dimension(sys):
        raise NotImplementedError(
            "UKF does not yet support static Schmidt consider parameters; "
            "use EKF/INS or remove the calibration posterior explicitly"
        )
    self._bind_system(world, sys)
    resolved_gates = resolve_gates(sys, gates, who="UKF")

    n_tan = sys.spec.tangent_dim
    explicit_alpha = alpha is not None
    if alpha is None:
        alpha = min(1.0, math.sqrt(3.0 / n_tan))
    self.alpha, self.beta, self.kappa = alpha, beta, kappa
    self.mean_iters = mean_iters
    self.jitter = jitter

    # ---- the unscented recursion, symbolically, once ----------------
    spec = sys.spec
    x, u = sys.x_sym, sys.u_sym
    dt, t = sys.dt_sym, sys.t_sym
    P = ca.MX.sym("P", n_tan, n_tan)
    Q = ca.MX.sym("Q", n_tan, n_tan)

    w_m, w_c, gamma = unscented_weights(n_tan, alpha, beta, kappa)
    self.sigma_weights = MappingProxyType({
        "alpha": float(alpha), "beta": float(beta), "kappa": float(kappa),
        "gamma": float(gamma), "tangent_dim": int(n_tan),
        "w_m0": float(w_m[0]), "w_c0": float(w_c[0]),
        "w_i": float(w_m[1]), "jitter": float(jitter),
        "auto_alpha": not explicit_alpha,
    })
    if w_c[0] < 0.0 and not jitter > 0.0:
        raise ValueError(
            f"UKF: alpha={alpha}, beta={beta}, kappa={kappa} give a "
            f"negative central covariance weight (w_c[0] = {w_c[0]:.3g} "
            f"at tangent dim {n_tan}); the covariance sums then rely on "
            "the jitter backstop, so jitter must be > 0 (or choose a "
            "tuning with w_c[0] >= 0)")
    if explicit_alpha and w_c[0] < 0.0:
        warnings.warn(
            f"UKF: alpha={alpha}, beta={beta}, kappa={kappa} give a "
            f"negative central covariance weight (w_c[0] = {w_c[0]:.3g}"
            f" at tangent dim {n_tan}) — the covariance updates lose "
            f"their PSD-by-construction guarantee and rely on the "
            f"jitter backstop. alpha=None (auto) bounds the spread at "
            f"√3·σ with a near-minimal negative weight.",
            RuntimeWarning, stacklevel=2)

    # Prior sigma set, shared by predict and (regenerated identically by)
    # each update: tangent offsets → retract onto the manifold. The
    # jitter regularizes the factorization of an iterated covariance.
    deltas = sigma_deltas(P, gamma, n_tan, jitter=jitter)
    sigma_pts = [spec.boxplus_sym(x, d) for d in deltas]

    # predict: push each sigma point through the nonlinear tick (inline
    # via substitution so the kernel stays one expandable scalar graph),
    # then the unscented mean/cov + auto process noise Q = L Σ Lᵀ.
    # `ca.cse` shrinks the graph: the 2n+1 substituted tick copies share
    # nearly all their subexpressions (same reason the linearization
    # engine cse's its inlined H kernels).
    Q_auto = _q_auto(sys)
    propagated = [ca.substitute(sys.x_new, x, Xi) for Xi in sigma_pts]
    x_pred, P_pred = ut_predict(propagated, Q_auto,
                                w_m, w_c, spec, mean_iters)
    x_pred_q, P_pred_q = ut_predict(propagated, Q,
                                    w_m, w_c, spec, mean_iters)
    x_pred, P_pred = ca.cse([x_pred, P_pred])
    x_pred_q, P_pred_q = ca.cse([x_pred_q, P_pred_q])
    predict_fn = ca.Function(
        "ukf_predict", [x, P, u, dt, t], [x_pred, P_pred],
        ["x", "P", "u", "dt", "t"], ["x_new", "P_new"])
    predict_q_fn = ca.Function(
        "ukf_predict_with_Q", [x, P, Q, u, dt, t], [x_pred_q, P_pred_q],
        ["x", "P", "Q", "u", "dt", "t"], ["x_new", "P_new"])

    # per-sensor unscented update. `prepared_sensors` eliminates dt and
    # refuses σ=0 sensors — the kernel honestly takes only
    # (x, P, z, u, t).
    x0 = initial_ambient(sys.world, spec)
    updates: dict[str, ca.Function] = {}
    diagnostic_updates: dict[str, ca.Function] = {}
    override_updates: dict[str, ca.Function] = {}
    for ps in prepared_sensors(sys, spec, x0=x0, who="UKF"):
        measured = [ca.substitute(ps.h, x, Xi) for Xi in sigma_pts]

        threshold = resolved_gates[ps.full]

        def expressions(
            R, measured=measured, ps=ps, threshold=threshold
        ):
            x_candidate, P_candidate, nu, S = ut_update(
                x, P, deltas, measured, R, ps.z, w_m, w_c, spec)
            nis = ca.dot(nu, spd_solve(S, nu))
            accepted = (ca.MX.ones(1, 1) if threshold is None
                        else nis <= threshold)
            return (ca.if_else(accepted, x_candidate, x),
                    ca.if_else(accepted, P_candidate, P),
                    nu, S, nis, accepted)

        x_upd, P_upd, nu, S, nis, accepted = expressions(ps.R)
        x_upd, P_upd, nu, S, nis, accepted = ca.cse(
            [x_upd, P_upd, nu, S, nis, accepted])
        updates[ps.full] = ca.Function(
            f"ukf_update_{entry_ident(ps.full)}",
            [x, P, ps.z, u, t], [x_upd, P_upd],
            ["x", "P", "z", "u", "t"], ["x_new", "P_new"])
        diagnostic_updates[ps.full] = ca.Function(
            f"ukf_update_diagnostic_{entry_ident(ps.full)}",
            [x, P, ps.z, u, t],
            [x_upd, P_upd, nu, S, nis, accepted],
            ["x", "P", "z", "u", "t"],
            ["x_new", "P_new", "innovation", "innovation_covariance",
             "nis", "accepted"])
        R_override = ca.MX.sym(f"R_{entry_ident(ps.full)}", ps.dim,
                               ps.dim)
        # Preserve non-overrideable white model uncertainty when a driver
        # supplies the ordinary per-sample measurement covariance.
        xo, Po, nuo, So, niso, acceptedo = expressions(
            R_override + ps.model_R
        )
        xo, Po, nuo, So, niso, acceptedo = ca.cse(
            [xo, Po, nuo, So, niso, acceptedo])
        override_updates[ps.full] = ca.Function(
            f"ukf_update_with_R_{entry_ident(ps.full)}",
            [x, P, ps.z, R_override, u, t],
            [xo, Po, nuo, So, niso, acceptedo],
            ["x", "P", "z", "R", "u", "t"],
            ["x_new", "P_new", "innovation", "innovation_covariance",
             "nis", "accepted"])

    self._module = emit_filter_module(
        sys, spec, name=f"{world.name}_ukf", x0=x0,
        predict_fn=predict_fn, predict_q_fn=predict_q_fn,
        updates=updates, diagnostic_updates=diagnostic_updates,
        override_updates=override_updates, gates=resolved_gates,
        metadata_extra={"unscented": self.sigma_weights})

INS

manta.INS

INS(world, *, imu, track=None, sensors=None, inputs=None, discretization='exact', gates=None, propagation='raw', navigation_frame=None, covariance='linearized', expand=False)

Bases: _FilterBase

Error-state strapdown inertial navigation transform.

Args mirror EKF where their meanings agree. imu identifies the physical IMU whose accelerometer and gyro become required prediction inputs in u. Its own outputs are removed from the ordinary update set. A selected, colocated ModelForce.specific_force is automatically sourced from that IMU's accelerometer by analysis tools. Such a part must carry accepted fit evidence (ModelForce(evidence=...)): construction refuses one without evidence, or whose evidence failed its acceptance criteria, naming what is missing.

propagation="raw" consumes one accelerometer/gyro pair per predict. propagation="preintegrated" consumes packets emitted by :class:~manta.estimation.imu_preintegrator.IMUPreintegrator; the high-rate recurrence and the lower-rate INS can both be lowered to generated C/C++. navigation_frame supplies fixed planet-attached Cartesian kinematics. The caller resolves its anchor and rotation vector; the IMU remains inertial. Its gravity convention is explicit. See docs/explanation/earth-relative-ins.md for equations, packet schema, and qualification scope. covariance="geometric" uses analytic prediction and bias-mean transport in a gravity/Earth-referenced finite error chart. covariance="nonlinear" instead uses augmented sigma-point prediction in that chart. Both map physical priors and apply the coupled covariance reset. The default "linearized" retains the existing covariance recursion. expand=True expands the hot filter kernels to scalar expressions before lowering; this preserves equations but changes code size and evaluation cost. Initialization quadrature is kept separate. Unsupported scalar operations fail explicitly. See docs/explanation/nonlinear-ins-covariance.md.

Source code in manta/estimation/ins.py

def __init__(self, world, *, imu,
             track: dict | None = None,
             sensors: list[str] | None = None,
             inputs: list[str] | None = None,
             discretization: str = "exact",
             gates: float | dict[str, float] | None = None,
             propagation: str = "raw",
             navigation_frame: NavigationFrame | None = None,
             covariance: str = "linearized",
             expand: bool = False) -> None:
    if covariance not in {"linearized", "geometric", "nonlinear"}:
        raise ValueError("INS covariance must be 'linearized', 'geometric' or 'nonlinear'")
    if not isinstance(expand, bool):
        raise TypeError("INS expand must be a bool")
    finite_chart = covariance != "linearized"
    if navigation_frame is not None and not isinstance(navigation_frame, NavigationFrame):
        raise TypeError("INS navigation_frame must be a NavigationFrame")
    if discretization != "exact":
        raise ValueError(
            "INS derives its exact strapdown F by autodiff; "
            "discretization must be 'exact'")
    if propagation not in {"raw", "preintegrated"}:
        raise ValueError(
            "INS propagation must be 'raw' or 'preintegrated', got "
            f"{propagation!r}")
    sys = _INSSystem(world, imu=imu, track=track,
                     sensors=sensors, inputs=inputs,
                     propagation=propagation, navigation_frame=navigation_frame)
    if finite_chart and propagation == "raw" and np.any(sys.lever_arm):
        raise NotImplementedError(
            "Finite-chart INS with a displaced IMU requires propagation='preintegrated' "
            "and timestamped gyro endpoints. The single-sample raw lever correction "
            "uses model angular acceleration and has not passed noisy-IMU consistency "
            "qualification. One-sample framed packets are supported."
        )
    require_measurement_only_consider(sys, who="INS")
    selected_imu_prefix = f"{sys.imu_name}."
    unsupported_imu_consider = tuple(
        channel.full
        for channel in sys.noise_specs
        if channel.static_parameter
        and channel.full.startswith(selected_imu_prefix)
    )
    if unsupported_imu_consider:
        raise NotImplementedError(
            "INS selected-IMU mount uncertainty affects strapdown "
            "propagation and cannot be a measurement-only Schmidt "
            f"parameter: {list(unsupported_imu_consider)}"
        )
    if finite_chart and propagation == "preintegrated":
        from ._ins_boundary import with_boundary_state
        sys = with_boundary_state(sys)
    if finite_chart:
        from ._ins_error import INSStateSpec
        physical = sys.spec
        initial = initial_ambient(sys.world, physical)
        position = physical.slot(f"{sys.craft_name}.position")
        p0 = initial[position.ambient_offset:position.ambient_offset + 3]
        reference = -np.asarray(ca.evalf(sys._gravity(ca.MX(p0), ca.MX(0)))).ravel()
        sys.spec = INSStateSpec(
            physical, craft=sys.craft_name, imu=sys.imu_name,
            rotation_body_from_imu=sys.R_craft_from_sensor,
            reference_specific_force=reference,
            reference_angular_velocity=(navigation_frame.angular_velocity
                                        if navigation_frame is not None else (0, 0, 0)))
    self._bind_system(world, sys)
    self.covariance = covariance
    self.imu = sys.imu_name
    self.navigation_frame = navigation_frame
    self.propagation = propagation
    self.preintegration_input_map = MappingProxyType({
        **dict(sys.preintegration_input_map),
        **({"end_accel": sys.accel_input, "end_gyro": sys.gyro_input}
           if propagation == "preintegrated" else {}),
    })
    self.measurement_sources = MappingProxyType(dict(sys.measurement_sources))
    self.rho_by_sensor = MappingProxyType(dict(sys.rho_by_sensor))
    self.evidence_by_sensor = MappingProxyType(dict(sys.evidence_by_sensor))
    resolved_gates = resolve_gates(sys, gates, who="INS")

    spec, n_tan = sys.spec, sys.spec.tangent_dim
    n_static_consider = consider_dimension(sys)
    # A preintegrated filter carries the standardized error of the current
    # gyro boundary as a dynamic Schmidt nuisance. Its mean is never
    # estimated, but its navigation cross-covariance survives aiding
    # updates and is handed into the next packet prediction. This is the
    # missing memory in an otherwise insufficient 9x9 packet covariance.
    n_boundary_consider = 3 if propagation == "preintegrated" and covariance == "linearized" else 0
    n_consider = n_static_consider + n_boundary_consider
    x, u, dt, t = sys.x_sym, sys.u_sym, sys.dt_sym, sys.t_sym
    P = ca.MX.sym("P", n_tan, n_tan)
    P_consider = (
        ca.MX.sym("P_consider", n_tan, n_consider)
        if n_consider else None
    )
    Q = ca.MX.sym("Q", n_tan, n_tan)
    F = sys.F_sym
    # Packet uncertainty is intrinsic to a preintegrated measurement and
    # is therefore retained even when the caller overrides the model's Q.
    Q_packet = sys.packet_Q_sym
    Q_auto = _q_auto(sys) + Q_packet
    predict_inputs = [x, P] + ([P_consider] if n_consider else [])
    predict_input_names = ["x", "P"] + (
        ["P_consider"] if n_consider else []
    )
    def prediction_covariance(base_Q):
        covariance = F @ P @ F.T + base_Q
        if not n_boundary_consider:
            cross_new = F @ P_consider if n_consider else None
            return symmetrize(covariance), cross_new

        boundary_cross = P_consider[:, n_static_consider:n_consider]
        boundary_G = sys.boundary_start_total_G_sym
        covariance += (
            F @ boundary_cross @ boundary_G.T
            + boundary_G @ boundary_cross.T @ F.T
            + boundary_G @ boundary_G.T
        )
        # The packet joint covariance gives E[eta_end | eta_start].
        # Usually this is zero because the framer supplies a fresh right
        # boundary; retaining the general term also makes a one-sample
        # recurrence packet mathematically well-defined.
        end_from_start = sys.boundary_conditional_gain_sym[9:12, :]
        boundary_cross_new = (
            F @ boundary_cross @ end_from_start.T
            + boundary_G @ end_from_start.T
            + sys.boundary_end_residual_cross_sym
        )
        pieces = []
        if n_static_consider:
            pieces.append(F @ P_consider[:, :n_static_consider])
        pieces.append(boundary_cross_new)
        return symmetrize(covariance), ca.horzcat(*pieces)

    P_auto, P_consider_auto = prediction_covariance(Q_auto)
    x_auto = sys.x_new
    if finite_chart:
        from ._ins_moments import predict_first_order, predict_moments
        prediction = predict_first_order if covariance == "geometric" else predict_moments
        x_auto, P_auto, P_consider_auto = prediction(sys, spec, P, P_consider)
    predict_outputs = [
        x_auto,
        P_auto,
        *([P_consider_auto] if n_consider else []),
    ]
    predict_output_names = ["x_new", "P_new"] + (
        ["P_consider_new"] if n_consider else []
    )
    predict_fn = ca.Function(
        "ins_predict", [*predict_inputs, u, dt, t], predict_outputs,
        [*predict_input_names, "u", "dt", "t"], predict_output_names)
    P_override, P_consider_override = prediction_covariance(Q + Q_packet)
    x_override = sys.x_new
    if finite_chart:
        x_override, P_override, P_consider_override = prediction(
            sys, spec, P, P_consider, process_noise=False, extra_Q=Q)
    predict_q_fn = ca.Function(
        "ins_predict_with_Q", [*predict_inputs, Q, u, dt, t],
        [
            x_override,
            P_override,
            *([P_consider_override] if n_consider else []),
        ],
        [*predict_input_names, "Q", "u", "dt", "t"],
        predict_output_names)

    x0 = initial_ambient(sys.world, spec)
    updates = {}
    diagnostic_updates = {}
    override_updates = {}
    for ps in prepared_sensors(sys, spec, x0=x0, who="INS"):
        H = ca.substitute(sys.sensors[ps.full].H_sym, dt, ca.MX.zeros(1, 1))
        consider_H = ps.consider_H
        if n_boundary_consider:
            consider_H = ca.horzcat(
                consider_H,
                ca.substitute(
                    sys.boundary_sensor_H[ps.full], dt,
                    ca.MX.zeros(1, 1)),
            )
        threshold = resolved_gates[ps.full]

        def expressions(R, ps=ps, H=H, consider_H=consider_H,
                        threshold=threshold):
            if n_consider:
                candidate_x, candidate_P, C_candidate, nu, S = (
                    schmidt_update(
                        x,
                        P,
                        P_consider,
                        ca.MX.eye(n_consider),
                        ps.h,
                        H,
                        consider_H,
                        R,
                        ps.z,
                        spec,
                    )
                )
            else:
                candidate_x, candidate_P, nu, S = joseph_update(
                    x, P, ps.h, H, R, ps.z, spec)
            nis = ca.dot(nu, spd_solve(S, nu))
            accepted = (ca.MX.ones(1, 1) if threshold is None
                        else nis <= threshold)
            states = [
                ca.if_else(accepted, candidate_x, x),
                ca.if_else(accepted, candidate_P, P),
            ]
            if n_consider:
                states.append(ca.if_else(
                    accepted, C_candidate, P_consider
                ))
            return (*states, nu, S, nis, accepted)

        values = expressions(ps.R)
        ident = entry_ident(ps.full)
        n_state_outputs = 3 if n_consider else 2
        state_input_values = [x, P] + (
            [P_consider] if n_consider else []
        )
        state_input_names = ["x", "P"] + (
            ["P_consider"] if n_consider else []
        )
        state_output_names = ["x_new", "P_new"] + (
            ["P_consider_new"] if n_consider else []
        )
        updates[ps.full] = ca.Function(
            f"ins_update_{ident}",
            [*state_input_values, ps.z, u, t],
            values[:n_state_outputs],
            [*state_input_names, "z", "u", "t"],
            state_output_names)
        diagnostic_updates[ps.full] = ca.Function(
            f"ins_update_diagnostic_{ident}",
            [*state_input_values, ps.z, u, t], values,
            [*state_input_names, "z", "u", "t"],
            [*state_output_names, "innovation", "innovation_covariance",
             "nis", "accepted"])
        R_override = ca.MX.sym(f"R_{ident}", ps.dim, ps.dim)
        overridden = expressions(R_override + ps.model_R)
        override_updates[ps.full] = ca.Function(
            f"ins_update_with_R_{ident}",
            [*state_input_values, ps.z, R_override, u, t], overridden,
            [*state_input_names, "z", "R", "u", "t"],
            [*state_output_names, "innovation", "innovation_covariance",
             "nis", "accepted"])

    metadata = {
        "estimator": "ins",
        "covariance": covariance,
        "expanded_filter_kernels": expand,
        "uncertainty_prediction": (
            "augmented_unscented" if covariance == "nonlinear" else
            "first_order_with_quadratic_bias_mean_transport" if covariance == "geometric"
            else "first_order"),
        "error_model": getattr(spec, "error_model", "product_manifold"),
        "reference_specific_force": getattr(spec, "reference_specific_force", None),
        "reference_angular_velocity": getattr(spec, "reference_angular_velocity", None),
        "propagation": propagation,
        "navigation_frame": (None if navigation_frame is None
                             else navigation_frame.metadata()),
        "preintegration_packet_schema": PREINTEGRATION_PACKET_SCHEMA,
        "max_packet_frame_rotation_rad": MAX_PACKET_FRAME_ROTATION_RAD,
        "earth_translation_quadrature": "left_hold_cubic_rotation_midpoint_coriolis",
        "prediction_inputs": (
            (sys.accel_input, sys.gyro_input)
            if propagation == "raw"
            else (*sys.preintegration_input_map.values(),
                  sys.accel_input, sys.gyro_input)),
        "preintegration_input_map": self.preintegration_input_map,
        "preintegration_duration_rtol": (
            PREINTEGRATION_DURATION_RTOL
            if propagation == "preintegrated" else None),
        "measurement_sources": MappingProxyType(dict(sys.measurement_sources)),
        "rho_by_sensor": MappingProxyType(dict(sys.rho_by_sensor)),
        # The consumed fit evidence travels with the estimator artifact:
        # which held-out set, which bias, which tau/sigma, and the
        # acceptance checks that admitted it.
        "model_force_evidence": MappingProxyType(
            dict(sys.evidence_by_sensor)),
        "rho_ceiling": MODEL_FORCE_RHO_CEILING,
        "rho_warning": MODEL_FORCE_RHO_WARNING,
        "rho_warned_sensors": tuple(sorted(
            name for name, rho in sys.rho_by_sensor.items()
            if rho > MODEL_FORCE_RHO_WARNING)),
        "lever_arm_m": tuple(float(v) for v in sys.lever_arm),
        "preintegration_boundary_covariance_qualified": True,
        "preintegration_boundary_consider_dimension": n_boundary_consider,
        "gyro_boundary_error_state": getattr(sys,"boundary_state_name",None),
        "preintegration_boundary_covariance_model": (
            "estimated_transient_gyro_error"
            if hasattr(sys, "boundary_state_name") else
            "dynamic_schmidt_joint_packet" if n_boundary_consider else None),
        # The filter deliberately carries no angular-velocity state.
        # Runtime adapters can still publish the current body rate from
        # the selected gyro by applying this fixed rigid-mount rotation
        # and subtracting the estimated bias. With navigation_frame,
        # publishing a relative body rate also requires subtracting
        # R_nav_from_body.T @ frame.angular_velocity at that epoch.
        "rotation_body_from_imu": tuple(
            float(v) for v in sys.R_craft_from_sensor.reshape(-1)
        ),
    }
    self._module = emit_filter_module(
        sys, spec, name=f"{sys.world.name}_ins", x0=x0,
        predict_fn=predict_fn, predict_q_fn=predict_q_fn,
        updates=updates, diagnostic_updates=diagnostic_updates,
        override_updates=override_updates, gates=resolved_gates,
        consider_dim=n_consider,
        metadata_extra=metadata)
    if expand:
        functions = {}
        for name, function in self._module.functions.items():
            if name == "predict" or name.startswith(("predict_", "update_")):
                try:
                    function = function.expand()
                except RuntimeError as error:
                    raise ValueError(
                        f"INS expand=True cannot expand kernel {name!r}; "
                        "its graph contains operations without scalar expansion support"
                    ) from error
            functions[name] = function
        self._module = replace(self._module, functions=functions)
    if finite_chart:
        from ._ins_moments import with_prior_initialization
        self._module = with_prior_initialization(self._module, spec, n_consider)

LQR

manta.LQR

LQR(world, *, x_ref, u_ref=None, Q=None, R=None, dt=0.01, regulate=None, tol=1e-12, max_iter=10000)

Infinite-horizon discrete LQR about an operating point.

Args: world — the model. x_ref — target state (nested {owner: {slot: value}} or flat {"owner.slot": value}), merged over the world's initial state for any unspecified slot. u_ref — trim inputs ({input_name: value}), merged over each Part Input's default. The equilibrium command. Q, R — LQR cost weights (regulated-tangent², n_inputs²). Default to identity. R must be positive-definite. dt — the discrete step the controller will run at. regulate — slot full-names to regulate, taken verbatim (e.g. ["c.position", "c.velocity"]); the rest are frozen at x_ref. None regulates the full state (fully-actuated systems only). tol, max_iter — Riccati-iteration convergence: relative fixpoint tolerance (‖ΔP‖ ≤ tol·max(1, ‖P‖)) and iteration cap.

Attributes: spec (full), regulated (regulated slot names), input_names, K (n_u × tracked_tangent), A, B, P, Q, R, dt, x_ref/u_ref (vectors), solution (the built solve as data), control_fn (u(x_full, x_ref_full, K, u_ff) ca.Function; runtimes default every argument but the live state to the built operating point — see NumpyRegulator.retarget / reprogram).

Source code in manta/control/lqr.py

def __init__(self, world, *,
             x_ref: dict,
             u_ref: dict | None = None,
             Q=None, R=None,
             dt: float = 0.01,
             regulate: list[str] | None = None,
             tol: float = 1e-12,
             max_iter: int = 10000) -> None:
    # All the linearization plumbing — tick compile, signature, the
    # VERBATIM regulated subset frozen at the operating point, and
    # B = ∂f/∂u — lives in `LinearizedSystem`. `regulate` is taken
    # verbatim (NOT closed over the dynamics like the EKF's `track`):
    # for an underactuated craft the whole point is to freeze the
    # uncontrollable states (e.g. attitude) at the operating point so
    # the reduced system is stabilizable; closing the set would pull
    # them back and the Riccati solve would diverge. The reference
    # point doubles as the freeze value.
    sys = LinearizedSystem(world, track=regulate, inputs=None,
                           track_mode="verbatim", control=True,
                           ref=x_ref)
    self.sys     = sys
    self.world   = sys.world
    self.model   = sys.model
    if not self.world.crafts:
        raise ValueError("LQR: world has no crafts.")
    self.spec    = sys.full_spec      # full layout (the law gathers from it)
    self._spec   = sys.spec           # tracked subspec
    self.regulated = sys.tracked
    self.input_names = sys.input_names
    n_u = len(self.input_names)
    if n_u == 0:
        raise ValueError(
            "LQR: world has no Part Inputs — no control authority.")

    # --- operating point ----------------------------------------------
    self._u_full = self._merge_u(u_ref, base=sys.input_defaults, who="LQR")
    u_ref_vec = self._u_vector(self._u_full)
    self.x_ref, self.u_ref = sys.pack_ref(sys.full_spec), u_ref_vec
    self.dt = float(dt)
    if not np.isfinite(self.dt) or self.dt <= 0.0:
        raise ValueError("LQR.dt must be finite and positive")
    n_x = sys.spec.tangent_dim

    identity = sha256()
    identity.update(sys._cf.serialize().encode())
    identity.update(repr((tuple(sys.tracked), tuple(sys.input_names),
                          self.dt)).encode())
    self.controller_id = identity.hexdigest()

    self.Q = self._check_Q(np.eye(n_x) if Q is None else Q)
    self.R = self._check_R(np.eye(n_u) if R is None else R)
    self._tol, self._max_iter = tol, max_iter

    # A = F, B = ∂f/∂u, both at the operating point (subspec ambient).
    sol = self._solve(sys.ref_flat, u_ref_vec, self.Q, self.R)
    self.A, self.B = sol.A, sol.B
    self.K, self.P = sol.K, sol.P

    # --- the control law: u = u_ff − K·(x_tracked ⊟ x_ref_tracked).
    # Takes the FULL ambient state and the reference, gathering the
    # tracked slots from each — plus the gain and the feed-forward,
    # which are runtime DATA rather than baked constants. Every
    # argument defaults to the built solve, so a caller that ignores
    # them flies exactly the law this construction solved; handing
    # over a fresh `resolve_at` triple is what makes a genuinely new
    # operating point (new A/B or trim) reachable without rebuilding
    # anything. Moving `x_ref` alone keeps the old gain — exact only
    # where the dynamics are invariant along the move.
    full_spec, spec = sys.full_spec, sys.spec
    x_full_sym = ca.MX.sym("x", full_spec.ambient_dim, 1)
    x_ref_sym = ca.MX.sym("x_ref", full_spec.ambient_dim, 1)
    K_sym = ca.MX.sym("K", n_u, n_x)
    u_ff_sym = ca.MX.sym("u_ff", n_u, 1)

    def _gather(sym):
        chunks = []
        for s in spec.slots:
            fs = full_spec.slot(s.name)
            chunks.append(
                sym[fs.ambient_offset : fs.ambient_offset + fs.ambient_dim])
        return ca.vertcat(*chunks) if chunks else sym

    dx = spec.boxminus_sym(_gather(x_full_sym), _gather(x_ref_sym))
    u_expr = u_ff_sym - K_sym @ dx
    self.control_fn = ca.Function(
        "lqr_u", [x_full_sym, x_ref_sym, K_sym, u_ff_sym], [u_expr],
        ["x", "x_ref", "K", "u_ff"], ["u"])

    # --- the typed Module: stateless, one
    # control(x, x_ref, K, u_ff) -> u entry. Every port but the live
    # `x` carries the built operating point as `init`, so a backend
    # defaults the reference, the gain, and the trim to this solve.
    self._module = Module(
        name=f"{world.name}_lqr", state=StateLayout(()),
        ports=(
            Port("x", Role.STATE, (full_spec.ambient_dim,),
                 spec=full_spec, init=self.x_ref),
            Port("x_ref", Role.STATE, (full_spec.ambient_dim,),
                 spec=full_spec, init=self.x_ref),
            Port("K", Role.MATRIX, (n_u, n_x), init=self.K),
            Port("u_ff", Role.MATRIX, (n_u, 1),
                 init=u_ref_vec.reshape(-1, 1)),
            Port("u", Role.CONTROL, (n_u,), fields=tuple(
                PortField(n, 1, float(self._u_full[n]))
                for n in self.input_names)),
        ),
        functions={"control": self.control_fn},
        entry_points=(EntryPoint("control", "control",
                                 (PortRef("x"), PortRef("x_ref"),
                                  PortRef("K"), PortRef("u_ff")),
                                 returns=("u",)),),
        kind=ModuleKind.REGULATOR,
        hosting=Hosting.THREADED,
        metadata={
            **sys.model.transform_metadata({
                "transform": "lqr",
                "discretization": sys.discretization,
                "tracked": tuple(sys.tracked),
                "inputs": tuple(sys.input_names),
                "dt": self.dt,
            }),
            "controller_id": self.controller_id,
        })

solution property

solution

The built solve as data — what every Port defaults to, and the identity element for reprogram().

closed_loop_eigs property

closed_loop_eigs

Eigenvalues of the closed-loop tangent map A − B·K (over the tracked subspace). All inside the unit circle ⇒ stable.

module

module()

The typed Module IR a backend lowers.

Source code in manta/control/lqr.py

def module(self) -> Module:
    """The typed `Module` IR a backend lowers."""
    return self._module

resolve_at

resolve_at(*, x_ref=None, u_ref=None, Q=None, R=None)

Re-solve the gain about a NEW operating point.

Evaluates A, B at the moved reference and re-runs the Riccati iteration — the symbolic linearization is already compiled, so this is a matrix evaluation plus a small dense DARE (µs + ms on a ~12-dim tangent), not a rebuild. Returns an LQRSolution; install it on a live regulator with reprogram(), or ship it as data. self is untouched.

This is the correct way to move a setpoint whenever the dynamics are not invariant along the move — most importantly a heading change, where retarget() alone leaves the world-frame position feedback rotated with the reference (⊥ at 90°, positive feedback at 180°).

Args: x_ref — reference overrides (nested or flat), merged over the built reference. Every named slot must be one this LQR regulates: the complement is frozen at the built point and baked into A/B as a constant, so no re-evaluation can honour a move there. u_ref — trim overrides, merged over the built trim. The equilibrium command at the new point (attitude- dependent in general — solving for it is a root-solve and stays yours). Q, R — cost overrides; default to the built weights.

Raises: ValueError — a named slot is unregulated (frozen) or unknown, or the moved point is not stabilizable.

Source code in manta/control/lqr.py

def resolve_at(self, *, x_ref: dict | None = None,
               u_ref: dict | None = None,
               Q=None, R=None) -> LQRSolution:
    """Re-solve the gain about a NEW operating point.

    Evaluates `A`, `B` at the moved reference and re-runs the Riccati
    iteration — the symbolic linearization is already compiled, so
    this is a matrix evaluation plus a small dense DARE (µs + ms on a
    ~12-dim tangent), not a rebuild. Returns an `LQRSolution`;
    install it on a live regulator with `reprogram()`, or ship it as
    data. `self` is untouched.

    This is the correct way to move a setpoint whenever the dynamics
    are *not* invariant along the move — most importantly a heading
    change, where `retarget()` alone leaves the world-frame position
    feedback rotated with the reference (⊥ at 90°, positive feedback
    at 180°).

    Args:
        x_ref — reference overrides (nested or flat), merged over the
                built reference. Every named slot must be one this
                LQR **regulates**: the complement is frozen at the
                built point and baked into `A`/`B` as a constant, so
                no re-evaluation can honour a move there.
        u_ref — trim overrides, merged over the built trim. The
                equilibrium command at the new point (attitude-
                dependent in general — solving for it is a root-solve
                and stays yours).
        Q, R  — cost overrides; default to the built weights.

    Raises:
        ValueError — a named slot is unregulated (frozen) or unknown,
                or the moved point is not stabilizable.
    """
    sys = self.sys
    ref_flat = dict(sys.ref_flat)
    if x_ref is not None:
        moved = flatten_nested(x_ref)
        self._check_movable(moved)
        ref_flat.update(moved)
    u_vec = self._u_vector(
        self._merge_u(u_ref, base=self._u_full, who="LQR.resolve_at"))
    return self._solve(ref_flat,
                       u_vec,
                       self.Q if Q is None else self._check_Q(Q),
                       self.R if R is None else self._check_R(R))

LQRSolution

One Riccati solve as plain data — what LQR.resolve_at returns and a regulator's reprogram() installs. See moving the operating point.

manta.LQRSolution dataclass

LQRSolution(K, u_ff, x_ref, A, B, P, controller_id)

One Riccati solve at one operating point, as plain data.

The affine control law is u = u_ff − K·(x ⊟ x_ref); these three fields are the whole of it. LQR.resolve_at returns one and a runtime regulator's reprogram() installs it — all three together, because a gain is only valid about the point it was solved at.

Everything here is a plain array, so a retarget service can hand a compiled regulator (numpy, wasm, C++) a new setpoint over JSON with no CasADi on the other side.

Attrs: K — n_u × regulated-tangent feedback gain. u_ff — n_u feed-forward: the trim command at this point. x_ref — full ambient reference the law regulates to. A, B — the tangent linearization it was solved from. P — the Riccati fixpoint.

closed_loop_eigs property

closed_loop_eigs

Eigenvalues of A − B·K. All inside the unit circle ⇒ stable.

PID

manta.PID

PID(kp, ki=0.0, kd=0.0, *, integral_limit=None, output_limit=None, name='pid')

Bases: RecurrenceBlock

Scalar PID controller as a recurrence block.

Args: kp, ki, kd — proportional / integral / derivative gains. integral_limit — symmetric clamp on the integral accumulator (anti-windup). None disables it. output_limit — symmetric clamp on the command. None disables. name — codegen basename / default C++ class stem.

Ports: inputs setpoint + measurement (scalars); output command. State: integral, prev_measurement, primed.

Source code in manta/control/pid.py

def __init__(self, kp: float, ki: float = 0.0, kd: float = 0.0, *,
             integral_limit: float | None = None,
             output_limit: float | None = None,
             name: str = "pid") -> None:
    self.kp = float(require_finite(kp, name="PID.kp"))
    self.ki = float(require_finite(ki, name="PID.ki"))
    self.kd = float(require_finite(kd, name="PID.kd"))
    self.integral_limit = (None if integral_limit is None
                           else require_positive(
                               integral_limit, name="PID.integral_limit"))
    self.output_limit = (None if output_limit is None
                         else require_positive(
                             output_limit, name="PID.output_limit"))

    kp_, ki_, kd_ = self.kp, self.ki, self.kd
    i_lim, o_lim = self.integral_limit, self.output_limit

    def rec(x, u, dt, t):
        err   = u["setpoint"] - u["measurement"]
        integ = x["integral"] + err * dt
        if i_lim is not None:
            integ = ca.fmin(ca.fmax(integ, -i_lim), i_lim)
        # Derivative on measurement; zeroed on the first step via
        # `primed`, and at dt == 0 (a paused/stalled driving loop) —
        # the bare division there is 0·inf = NaN, which poisons the
        # command. At dt = 0 the block degrades to P + held-I.
        d_rate = ca.if_else(
            dt > 0,
            (u["measurement"] - x["prev_measurement"]) / ca.fmax(dt, 1e-300),
            0.0)
        d_meas = x["primed"] * d_rate
        cmd = kp_ * err + ki_ * integ - kd_ * d_meas
        if o_lim is not None:
            cmd = ca.fmin(ca.fmax(cmd, -o_lim), o_lim)
        x_next = {
            "integral":         integ,
            "prev_measurement": u["measurement"],
            "primed":           ca.MX.ones(1, 1),
        }
        return x_next, {"command": cmd}

    self._build_recurrence(
        name=name,
        state=[("integral",         ScalarManifold()),
               ("prev_measurement", ScalarManifold()),
               ("primed",           ScalarManifold())],
        inputs=[("setpoint", 1), ("measurement", 1)],
        outputs=[("command", 1)],
        x0={"integral": 0.0, "prev_measurement": 0.0, "primed": 0.0},
        recurrence=rec)