Multi-Agent Tracking

MultiAgentTracking

Utilities for multi-agent, multi-target coordination via time-expanded cellular sheaves.

Exports types for problem specification (TrackingEdge, TrackingProblem, BobbingTarget, ScenarioResult) and functions to build the sheaf, generate reference trajectories, solve, and extract results.

BobbingTarget

Parameters of a vertically oscillating ("bobbing") target.

Fields:

• y_fixed: fixed lateral position (m).
• z_center: mean altitude (m).
• z_amplitude: oscillation amplitude (m).
• omega: angular frequency (rad/s).

Call trajectory(bt, t_range, h, nx, nu) to materialise the stalk sequence over any integer time interval.

ScenarioResult

Bundle of trajectory data and statistics for one solved scenario.

Fields:

• label: scenario name.
• times: discrete time vector (length k+1).
• agent_trajs: Vector of (k+1) × nx state matrices, one per agent.
• target_trajs: Vector of stalk-vector sequences, one per target.
• null_dim: nullspace dimension of the restricted Laplacian.
• residual: Laplacian energy sqrt(z' * L * z).
• y_col, z_col: which columns of the state matrix correspond to the y and z coordinates used for plotting.

TrackingEdge

One agent–target tracking relationship with its own restriction maps.

Fields:

• agent_index: index of the agent in 1:n_agents.
• target_index: index of the target in 1:n_targets.
• agent_restriction: matrix applied to the agent stalk.
• target_restriction: matrix applied to the target stalk.

Using a vector of TrackingEdges (rather than a single assignment vector) allows many-to-many relationships where each pairing can use different projection matrices.

TrackingProblem

Specification of a heterogeneous multi-agent / multi-target tracking problem encoded as a time-expanded cellular sheaf.

Fields:

• n_agents, n_targets: fleet sizes.
• k: horizon length; the trajectory has k+1 timesteps t = 0, …, k.
• Ad: ZOH-discretised dynamics matrices, one per agent (length n_agents).
• Bd: ZOH-discretised control matrices, one per agent (length n_agents).
• target_Ad: dynamics matrices, one per target (length n_targets).
• target_Bd: control matrices, one per target (length n_targets).
• agent_edges: undirected agent–agent consensus pairs (i, j).
• tracking_edges: many-to-many agent–target edges (see TrackingEdge).
• consensus_restriction: projection used for all agent–agent consensus edges.
• consensus_timesteps: timesteps at which consensus edges are added.
• tracking_timesteps: timesteps at which tracking edges are added.
• include_target_dynamics: add temporal dynamics edges between target vertices.
• consensus_weight, tracking_weight: Laplacian weights (the restriction maps are pre-scaled by their square roots).

WindowSolverCache

Reusable solver state for the cost-optimal harmonic extension of a fixed window sheaf, built once per window length and cached across MPC steps. Each step then reduces to z[interior] = M * x_B — a single dense matvec. Construct with tracking_extension_operator.

Fields: M (interior×boundary), boundary/interior (DOF index vectors), stalks, laplacian (for energy), null_dim, window.

_apply_first_control(window_prob, z_opt, stalks) -> Vector{Vector{Float64}}

Advance each agent one step using the first decision control of the window. In the reindexed window sheaf the initial vertex (t=0) is state-only and the control u_1 on vertex t=1 drives the first transition, so this computes x_{t+1} = Ad_i * x_0 + Bd_i * u_1. Both x_0 (the pinned initial state) and u_1 are read from the section.

_assemble_boundary(prob, x_agents, target_window) -> Dict{Int,Vector{Float64}}

Build the vertex-indexed boundary dictionary for harmonic_extension over the MPC-window sheaf (built with state_only_initial = true):

• Agent i's state-only vertex at the window's first timestep is pinned to x_agents[i].
• Target j's full stalk at each timestep is pinned to target_window[j][t+1].

Both are full-vertex pins, so this returns a Dict{Int,Vector{Float64}}; the target stalk dimensions are checked by harmonic_extension. target_window must have prob.k + 1 samples per target.

agent_vertex(prob, i, t)

Index of agent i at timestep t in the sheaf built from prob.

build_time_expanded_tracking_sheaf(prob::TrackingProblem; state_only_initial=false)

Construct the time-expanded EuclideanSheaf from a TrackingProblem.

Four edge families are added in order:

1. Agent dynamics — agent(i,t) ↔ agent(i,t+1) for every agent i and step t = 0, …, k-1, encoding x_{t+1} = Ad * x_t + Bd * u_t.
2. Target dynamics (when include_target_dynamics) — same structure for each target vertex.
3. Consensus — agent(i,t) ↔ agent(j,t) for each (i,j) in agent_edges, active at each t in consensus_timesteps.
4. Tracking — for each TrackingEdge(i, j, Ra, Rt), adds agent(i,t) ↔ target(j,t) active at each t in tracking_timesteps, using restriction maps Ra (agent side) and Rt (target side).

Restriction maps for consensus and tracking edges are pre-scaled by sqrt(consensus_weight) and sqrt(tracking_weight) respectively so that the resulting Laplacian is weight * R' * R.

When state_only_initial = true the initial agent vertices (t = 0) carry state only (no control), and the control is reindexed so that u_t produces x_t: the agent dynamics edge reads A·x_t from the source and x_{t+1} − B·u_{t+1} from the destination, encoding x_{t+1} = A·x_t + B·u_{t+1}. Consensus/tracking restriction maps at t = 0 are then truncated to the agent's state columns (exact, since those columns multiplied the now-absent control block by zero). This makes the initial condition a clean full-vertex pin and is the form used by the MPC window solver; the resulting optimal trajectory is identical to the default form, which keeps (x_0, u_0) on the first vertex.

extract_state_trajectories(z_harmonic, prob)

Return one (k+1) × nx_i matrix per agent, where nx_i is the state dimension for agent i. Pre-allocated and filled directly from the harmonic-extension output (no intermediate concatenations).

extract_target_trajectories(z_harmonic, prob)

Return a Vector of length n_targets, where each element is a Vector{Vector{Float64}} of length k+1 containing the full stalk (state + control) for that target at each timestep.

generate_reference_trajectory(x0, xk, k, Ad, Bd, nx, nu)

Compute the minimum-energy trajectory from x0 to xk in k steps by harmonic extension on a single-agent dynamics sheaf.

Each stalk has dimension nx + nu. Interior control components are in general nonzero—they are the minimum-energy inputs satisfying the linear dynamics. This function is designed to produce fully-pinned target boundary data; the control components of the returned stalks do not affect the multi-agent solution once the targets are pinned as boundary vertices.

run_mpc_scenario(label, prob, x0_agents, target_trajs, times;
                 window, y_col=1, z_col=2, cost=1.0, solver=:cached) -> ScenarioResult

Receding-horizon MPC: at each step t solves the sheaf QP on [t, min(t+window,k)], applies the first control, and advances agent dynamics. solver=:cached reuses a WindowSolverCache for recurring window lengths; falls back to :naive on inhomogeneous problems.

run_scenario(label, prob, boundary, times; target_trajs, y_col, z_col)

Build the sheaf from prob, run harmonic_extension, compute the Laplacian energy sqrt(z' * L * z), and return a ScenarioResult.

sheaf_laplacian_matrix_direct is used for the energy (rather than coboundary_map) because the coboundary matrix only spans vertices appearing in at least one edge; isolated target vertices (when include_target_dynamics = false) would cause a dimension mismatch.

y_col and z_col are the column indices of the y and z coordinates in the state vector; they are stored in the result for plotting. Defaults are 1 and 2, matching the planar-quadrotor model convention.

selector_matrix(indices, n) -> Matrix{Float64}

length(indices) × n row-selection matrix S such that S * v == v[indices].

state_projection_matrix(state_indices, nx, nu) -> Matrix{Float64}

length(state_indices) × (nx + nu) matrix selecting state coordinates from an augmented stalk.

target_vertex(prob, j, t)

Index of target j at timestep t in the sheaf built from prob.

tracking_extension_operator(window_prob; cost=1.0) -> WindowSolverCache

Factorize the window Laplacian once and fold in the control-cost nullspace projection to produce a dense operator M so each MPC step is M * x_B.

trajectory(bt::BobbingTarget, t_range, h, nx, nu, y_idx, z_idx, zdot_idx)

Return a Vector of length length(t_range) stalk vectors for bt.

Each stalk has dimension nx + nu. The lateral state component at y_idx is set to bt.y_fixed; the altitude component at z_idx follows a sinusoid centred at bt.z_center with amplitude bt.z_amplitude and angular frequency bt.omega; the altitude-rate component at zdot_idx is set to the analytic derivative. All other components are zero.

t_range may be any integer range, enabling evaluation over an arbitrary sub-interval without recomputing from scratch.

window_problem(prob, t, t_end) -> TrackingProblem

Extract the sub-problem for timesteps [t, t_end] from a full TrackingProblem. Activation timesteps are shifted to be relative to the window start.

window_targets(target_trajs, t0, t1)

Return target trajectories sliced to integer timesteps t0:t1.

mul!(z, op::WindowSolverCache, x_B) -> z

Apply the cached operator in place: scatter the boundary values x_B and write the harmonic interior M * x_B directly into z (length sum(op.stalks)), allocating nothing.