Function solve_horizon_consensus 

pub fn solve_horizon_consensus(
    config: AdmmConfig,
    agents: &[AgentTrajectory],
    smooth_weight: f64,
    anchor: Option<[f64; 2]>,
) -> RoboticsResult<HorizonConsensusReport>

Solve the receding-horizon formation-consensus problem: agents agree on a shared center trajectory z[0..H] rather than a single static center.

Each agent tracks a per-step reference a_i[t], sits at z[t] + offset_i, and respects a per-step box; the shared center carries a temporal-smoothness (acceleration) penalty (smooth_weight/2) sum_t ||z[t+1]-2 z[t]+z[t-1]||^2 that couples the center across time. The full problem is minimize sum_{i,t} (w_i/2)||x_i[t]-a_i[t]||^2 + (smooth_weight/2) sum_t ||z[t+1]-2z[t]+z[t-1]||^2 s.t. x_i[t] - offset_i = z[t] with per-step boxes.

Consensus ADMM mirrors solve_formation_consensus, but the z-update is no longer a per-step average: the smoothness term makes it a banded (pentadiagonal) symmetric-positive-definite linear solve (rho N I + smooth_weight D^T D) z = rho sum_i (x_i - offset_i + u_i) per coordinate, where D is the second-difference operator. The system matrix is constant across iterations, so it is Cholesky-factorized once and back-solved each iteration. An optional anchor fixes z[0] (the current team center), which is what makes the solve usable as the inner step of a receding-horizon MPC loop: solve over the horizon, apply z[1], shift, and re-solve.

With smooth_weight = 0 and a one-step horizon this reduces exactly to the static centralized consensus of solve_formation_consensus.