Transforms — Sim, EKF, UKF, 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 = world
    self.crafts = self._sys.crafts

tick property

tick

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

module

module()

The oracle Module (simulation truth): one step entry — the full forward tick, one live noise draw → state + readings.

Source code in manta/sim.py

def module(self) -> Module:
    """The **oracle** Module (simulation truth): one `step` entry —
    the full forward tick, one live noise draw → state + readings."""
    sys = self._sys
    x_field, u_port, dtp, tp, meas_ports = self._module_scaffold()
    sensor_fulls = list(sys.sensors)
    noise_port = Port(
        "noise", Role.NOISE, (sys.n_noise,),
        fields=tuple(PortField(c.full, c.dim, 0.0, sigma=c.sigma)
                     for c in sys.noise_specs))
    p_port = self._param_port()
    kargs, kargn = [sys.x_sym, sys.u_sym, sys.n_sym], ["x", "u", "noise"]
    eargs = [StateRef("x"), PortRef("u"), PortRef("noise")]
    if p_port is not None:
        kargs.append(sys.p_sym); kargn.append("params")
        eargs.append(PortRef("params"))
    kargs += [sys.dt_sym, sys.t_sym]; kargn += ["dt", "t"]
    eargs += [PortRef("dt"), PortRef("t")]
    step_fn = ca.Function(
        "step", kargs,
        [sys.x_new_noisy] + [
            ca.reshape(sys.sensors[f].h_noisy_sym,
                       sys.sensors[f].dim, 1) for f in sensor_fulls],
        kargn,
        ["x_new"] + [entry_ident(f) for f in sensor_fulls])
    ports = [u_port, noise_port, dtp, tp, *meas_ports]
    if p_port is not None:
        ports.insert(2, p_port)
    return Module(
        name=self.world.name, state=StateLayout((x_field,)),
        ports=tuple(ports),
        functions={"step": step_fn},
        entry_points=(EntryPoint(
            "step", "step", tuple(eargs),
            writes=("x",), returns=tuple(sensor_fulls)),),
        hosting=Hosting.THREADED)

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), hosting=Hosting.THREADED)

EKF

manta.EKF

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

Error-state EKF over a World — symbolic recursion + typed Module.

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.

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") -> 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.
    """
    sys = LinearizedSystem(world, track=track, sensors=sensors,
                           inputs=inputs, track_mode="closure",
                           discretization=discretization)
    self.sys = sys
    self.world = world
    self.crafts = sys.crafts
    self.spec: StateSpec = sys.spec

    # ---- the Kalman recursion, symbolically, once -------------------
    spec, n_tan = sys.spec, sys.spec.tangent_dim
    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)
    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 = lin_cov(sys.L_sym,
                     ca.DM(sys.Sigma) if sys.L_sym is not None else None,
                     n_tan)
    predict_fn = ca.Function(
        "ekf_predict", [x, P, u, dt, t],
        [sys.x_new, symmetrize(F @ P @ F.T + Q_auto)],
        ["x", "P", "u", "dt", "t"], ["x_new", "P_new"])
    predict_q_fn = ca.Function(
        "ekf_predict_with_Q", [x, P, Q, u, dt, t],
        [sys.x_new, symmetrize(F @ P @ F.T + Q)],
        ["x", "P", "Q", "u", "dt", "t"], ["x_new", "P_new"])

    # per-sensor Joseph update (the shared `joseph_update` kernel —
    # see estimation/_kalman.py). A measurement is dt-independent, so
    # dt is eliminated here (substituted to 0) — the kernel honestly
    # takes only (x, P, z, u, t).
    init_flat = flatten_nested(world._initial_state_dict())
    x0 = spec.pack_any(init_flat)
    zero_dt = ca.MX.zeros(1, 1)
    updates: dict[str, ca.Function] = {}
    for full, s in sys.sensors.items():
        z = ca.MX.sym("z", s.dim)
        h = ca.substitute(s.h_sym, dt, zero_dt)
        H = ca.substitute(s.H_sym, dt, zero_dt)
        L_h = (ca.substitute(s.L_h_sym, dt, zero_dt)
               if s.L_h_sym is not None and sys.Sigma is not None
               else None)
        R = lin_cov(L_h, ca.DM(sys.Sigma) if L_h is not None else None,
                    s.dim)
        # Refuse a σ=0 sensor (R ≡ 0 → singular S on the second fold).
        R_fn = ca.Function("R0", [x, u, t], [R])
        require_active_R(R, R_fn, ca.vertcat(x, u, t),
                         x0=x0, u_defaults=sys.u_defaults, spec=spec,
                         full=full, who="EKF")
        x_upd, P_upd, _, _ = joseph_update(x, P, h, H, R, z, spec)
        updates[full] = ca.Function(
            f"ekf_update_{entry_ident(full)}",
            [x, P, z, u, t], [x_upd, P_upd],
            ["x", "P", "z", "u", "t"], ["x_new", "P_new"])

    # ---- the typed Module -------------------------------------------
    fields = (
        StateField("x", "manifold", (spec.ambient_dim,),
                   init=x0, manifold=spec),
        StateField("P", "matrix", (n_tan, n_tan),
                   init=np.eye(n_tan) * 1e-2),
    )
    ports = [
        Port("u", Role.CONTROL, (len(sys.input_names),), fields=tuple(
            PortField(n, 1, float(sys.input_defaults[n]),
                      rate=sys.sample_rates.get(n))
            for n in sys.input_names)),
        Port("dt", Role.TIMESTEP),
        Port("t", Role.TIME),
        Port("Q", Role.MATRIX, (n_tan, n_tan)),
    ]
    functions = {"predict": predict_fn, "predict_with_Q": predict_q_fn}
    entries = [
        EntryPoint("predict", "predict",
                   (StateRef("x"), StateRef("P"), PortRef("u"),
                    PortRef("dt"), PortRef("t")),
                   writes=("x", "P")),
        EntryPoint("predict_with_Q", "predict_with_Q",
                   (StateRef("x"), StateRef("P"), PortRef("Q"),
                    PortRef("u"), PortRef("dt"), PortRef("t")),
                   writes=("x", "P")),
    ]
    for full, s in sys.sensors.items():
        ident = entry_ident(full)
        ports.append(Port(full, Role.MEASUREMENT, (s.dim,),
                          rate=sys.sample_rates.get(full)))
        functions[f"update_{ident}"] = updates[full]
        entries.append(EntryPoint(
            f"update_{ident}", f"update_{ident}",
            (StateRef("x"), StateRef("P"), PortRef(full),
             PortRef("u"), PortRef("t")),
            writes=("x", "P")))

    self._module = Module(
        name=f"{world.name}_ekf", state=StateLayout(fields),
        ports=tuple(ports), functions=functions,
        entry_points=tuple(entries), hosting=Hosting.HELD)

n_blocks property

n_blocks

Independent tangent subsystems (block-diagonal predict).

module

module()

The typed Module IR a backend lowers.

Source code in manta/estimation/ekf.py

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

observability

observability(**kwargs)

Local observability of the chosen sensor set at an operating point (see manta.estimation.observability).

Source code in manta/estimation/ekf.py

def observability(self, **kwargs):
    """Local observability of the chosen sensor set at an operating
    point (see `manta.estimation.observability`)."""
    from .observability import observability
    return observability(self, **kwargs)

sigma_horizon

sigma_horizon(**kwargs)

Per-slot σ attainable after a horizon — the covariance recursion run open-loop, resolving the weak/slow observability the rank test can't (see manta.estimation.observability).

Source code in manta/estimation/ekf.py

def sigma_horizon(self, **kwargs):
    """Per-slot σ attainable after a horizon — the covariance
    recursion run open-loop, resolving the weak/slow observability the
    rank test can't (see `manta.estimation.observability`)."""
    from .observability import sigma_horizon
    return sigma_horizon(self, **kwargs)

UKF

manta.UKF

UKF(world, *, track=None, sensors=None, inputs=None, alpha=0.001, beta=2.0, kappa=0.0, mean_iters=1)

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.

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). Small (default 1e-3) keeps the points near the mean — the canonical scaled UT, which matches the EKF in the linear limit. 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 is plenty for the default spread; raise it for a wide spread on a strongly-curved manifold).

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 = 1e-3,
             beta: float = 2.0,
             kappa: float = 0.0,
             mean_iters: int = 1) -> 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). Small (default 1e-3)
                 keeps the points near the mean — the canonical scaled
                 UT, which matches the EKF in the linear limit.
        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 is plenty for the default spread; raise it for a
                 wide spread on a strongly-curved manifold).

    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")
    self.sys = sys
    self.world = world
    self.crafts = sys.crafts
    self.spec: StateSpec = sys.spec
    self.alpha, self.beta, self.kappa = alpha, beta, kappa
    self.mean_iters = mean_iters

    # ---- the unscented recursion, symbolically, once ----------------
    spec, n_tan = sys.spec, sys.spec.tangent_dim
    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)

    # Prior sigma set, shared by predict and (regenerated identically by)
    # each update: tangent offsets → retract onto the manifold.
    deltas = sigma_deltas(P, gamma, n_tan)
    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ᵀ.
    Q_auto = lin_cov(sys.L_sym,
                     ca.DM(sys.Sigma) if sys.L_sym is not None else None,
                     n_tan)
    propagated = [ca.substitute(sys.x_new, x, Xi) for Xi in sigma_pts]
    x_pred, P_pred = ut_predict(deltas, propagated, Q_auto,
                                w_m, w_c, spec, mean_iters)
    x_pred_q, P_pred_q = ut_predict(deltas, propagated, Q,
                                    w_m, w_c, spec, mean_iters)
    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. A measurement is dt-independent, so dt
    # is eliminated (substituted to 0) — the kernel honestly takes only
    # (x, P, z, u, t).
    init_flat = flatten_nested(world._initial_state_dict())
    x0 = spec.pack_any(init_flat)
    zero_dt = ca.MX.zeros(1, 1)
    updates: dict[str, ca.Function] = {}
    for full, s in sys.sensors.items():
        z = ca.MX.sym("z", s.dim)
        h = ca.substitute(s.h_sym, dt, zero_dt)
        L_h = (ca.substitute(s.L_h_sym, dt, zero_dt)
               if s.L_h_sym is not None and sys.Sigma is not None
               else None)
        R = lin_cov(L_h, ca.DM(sys.Sigma) if L_h is not None else None,
                    s.dim)
        # Refuse a σ=0 sensor (R ≡ 0 → singular innovation S).
        R_fn = ca.Function("R0", [x, u, t], [R])
        require_active_R(R, R_fn, ca.vertcat(x, u, t),
                         x0=x0, u_defaults=sys.u_defaults, spec=spec,
                         full=full, who="UKF")
        measured = [ca.substitute(h, x, Xi) for Xi in sigma_pts]
        x_upd, P_upd, _, _ = ut_update(x, P, deltas, measured, R, z,
                                       w_m, w_c, spec)
        updates[full] = ca.Function(
            f"ukf_update_{entry_ident(full)}",
            [x, P, z, u, t], [x_upd, P_upd],
            ["x", "P", "z", "u", "t"], ["x_new", "P_new"])

    # ---- the typed Module (identical shape to EKF's) ----------------
    fields = (
        StateField("x", "manifold", (spec.ambient_dim,),
                   init=x0, manifold=spec),
        StateField("P", "matrix", (n_tan, n_tan),
                   init=np.eye(n_tan) * 1e-2),
    )
    ports = [
        Port("u", Role.CONTROL, (len(sys.input_names),), fields=tuple(
            PortField(n, 1, float(sys.input_defaults[n]),
                      rate=sys.sample_rates.get(n))
            for n in sys.input_names)),
        Port("dt", Role.TIMESTEP),
        Port("t", Role.TIME),
        Port("Q", Role.MATRIX, (n_tan, n_tan)),
    ]
    functions = {"predict": predict_fn, "predict_with_Q": predict_q_fn}
    entries = [
        EntryPoint("predict", "predict",
                   (StateRef("x"), StateRef("P"), PortRef("u"),
                    PortRef("dt"), PortRef("t")),
                   writes=("x", "P")),
        EntryPoint("predict_with_Q", "predict_with_Q",
                   (StateRef("x"), StateRef("P"), PortRef("Q"),
                    PortRef("u"), PortRef("dt"), PortRef("t")),
                   writes=("x", "P")),
    ]
    for full, s in sys.sensors.items():
        ident = entry_ident(full)
        ports.append(Port(full, Role.MEASUREMENT, (s.dim,),
                          rate=sys.sample_rates.get(full)))
        functions[f"update_{ident}"] = updates[full]
        entries.append(EntryPoint(
            f"update_{ident}", f"update_{ident}",
            (StateRef("x"), StateRef("P"), PortRef(full),
             PortRef("u"), PortRef("t")),
            writes=("x", "P")))

    self._module = Module(
        name=f"{world.name}_ukf", state=StateLayout(fields),
        ports=tuple(ports), functions=functions,
        entry_points=tuple(entries), hosting=Hosting.HELD)

n_blocks property

n_blocks

Independent tangent subsystems (structurally decoupled crafts).

module

module()

The typed Module IR a backend lowers.

Source code in manta/estimation/ukf.py

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

observability

observability(**kwargs)

Local observability of the chosen sensor set at an operating point (the linearized analysis shared with the EKF — see manta.estimation.observability).

Source code in manta/estimation/ukf.py

def observability(self, **kwargs):
    """Local observability of the chosen sensor set at an operating
    point (the linearized analysis shared with the EKF — see
    `manta.estimation.observability`)."""
    from .observability import observability
    return observability(self, **kwargs)

sigma_horizon

sigma_horizon(**kwargs)

Per-slot σ attainable after a horizon — the linearized covariance recursion run open-loop (see manta.estimation.observability).

Source code in manta/estimation/ukf.py

def sigma_horizon(self, **kwargs):
    """Per-slot σ attainable after a horizon — the linearized covariance
    recursion run open-loop (see `manta.estimation.observability`)."""
    from .observability import sigma_horizon
    return sigma_horizon(self, **kwargs)

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:
    if not world.crafts:
        raise ValueError("LQR: world has no crafts.")

    # 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   = world
    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)
    n_x = sys.spec.tangent_dim

    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,),
                 manifold=full_spec, init=self.x_ref),
            Port("x_ref", Role.STATE, (full_spec.ambient_dim,),
                 manifold=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",)),),
        hosting=Hosting.THREADED)

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)

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(kp)
    self.ki = float(ki)
    self.kd = float(kd)
    self.integral_limit = (None if integral_limit is None
                           else float(integral_limit))
    self.output_limit = (None if output_limit is None
                         else float(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`.
        d_meas = x["primed"] * (u["measurement"] - x["prev_measurement"]) / dt
        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)