Imported from hiyenwong/ai_collection (collection/skills/systems-engineering/density-driven-optimal-control/SKILL.md). Install upstream with npx skills add hiyenwong/ai_collection --skill density-driven-optimal-control. Copyright stays with the author.

Density-Driven Optimal Control (D²OC)

Overview

This skill provides methodology for Density-Driven Optimal Control - a framework that synthesizes density-based control with optimal control theory for multi-agent systems. The approach enables decentralized non-uniform area coverage with formal convergence guarantees, avoiding expensive spatial discretization while scaling linearly with the number of agents.

Key Innovation: Instead of computationally heavy Eulerian PDE solvers or heuristic planning, this framework derives closed-form analytical solutions for the optimal control problem with provable convergence properties.

When to Use This Skill

Primary Use Cases

1. Decentralized Area Coverage: Designing multi-agent systems for non-uniform coverage missions with spatial priority maps
2. Resource-Constrained Missions: When computational resources are limited and real-time control synthesis is required
3. Formal Guarantee Requirements: When convergence guarantees and error bounds are critical (e.g., safety-critical applications)
4. Scalable Swarm Control: Systems where the number of agents varies or grows
5. Stochastic Environment Control: Operating under uncertainty with known statistical properties

Domain Applications

Theoretical Foundation

Problem Formulation

Given:

• N agents with LTI dynamics: ẋᵢ(t) = Axᵢ(t) + Buᵢ(t) + wᵢ(t)
• Target density ρ*(x): desired spatial distribution
• Current density ρ(x,t): agent distribution

Objective: Minimize density mismatch: J = ∫∫ (ρ(x,t) - ρ*(x))² dx dt

Key Components

1. Density Mismatch Metric

D(t) = ∫ (ρ(x,t) - ρ*(x))² dx

Measures the difference between current agent distribution and target distribution.

2. Optimal Control Law

For LTI systems, the analytical solution yields:

uᵢ*(t) = -R⁻¹BᵀP(xᵢ(t) - x̄ᵢ(t))

where:

• P solves the algebraic Riccati equation
• x̄ᵢ(t) is the target position derived from density gradient

3. Convergence Guarantees

Under stochastic LTI dynamics with bounded noise, the framework provides:

• Mean-square convergence: E[‖ρ(t) - ρ*‖²] → 0 as t → ∞
• Exponential rate: Convergence with known decay rate λ
• Robustness bounds: Maximum steady-state error under disturbances

Workflow

Step 1: Define the Coverage Problem

Input Requirements:

• Target spatial density ρ*(x) (continuous or discrete representation)
• Agent dynamics model (LTI parameters A, B)
• Environment boundaries and constraints
• Agent count N and initial positions

Key Questions:

• What spatial distribution needs to be achieved?
• What are the agent movement constraints?
• Is the environment static or dynamic?

Step 2: Formulate the Optimal Control Problem

Mathematical Setup:

# Define system matrices
A = ...  # State transition matrix
B = ...  # Control input matrix
Q = ...  # State cost (density mismatch weight)
R = ...  # Control effort weight

# Solve Riccati equation for optimal gain
P = solve_algebraic_riccati(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P

Parameter Selection Guidelines:

• Q matrix: Larger values prioritize density tracking accuracy
• R matrix: Larger values penalize aggressive control inputs
• Trade-off between coverage quality and energy efficiency

Step 3: Derive Target Positions from Density

Density-to-Position Mapping:

def density_to_target_positions(rho_star, N):
    """
    Convert target density to agent target positions.
    
    Args:
        rho_star: Target density function or grid
        N: Number of agents
    
    Returns:
        target_positions: Array of shape (N, dim)
    """
    # Method 1: Lloyd's algorithm (centroidal Voronoi tessellation)
    # Method 2: Gradient descent on density potential
    # Method 3: Sampling from probability distribution
    ...

Implementation Options:

1. Lloyd's Algorithm: Iterative centroid computation
2. Gradient Flow: Follow density gradient ascent
3. Importance Sampling: Sample proportional to density

Step 4: Implement Decentralized Control

Control Synthesis:

def compute_optimal_control(x_i, x_target_i, K):
    """
    Compute optimal control input for agent i.
    
    Args:
        x_i: Current position
        x_target_i: Target position from density
        K: Optimal gain matrix
    
    Returns:
        u_i: Control input
    """
    error = x_i - x_target_i
    u_i = -K @ error
    return u_i

Decentralization Strategy:

• Each agent computes its own target based on local density information
• No global coordination required after initialization
• Communication only needed for density map updates (if dynamic)

Step 5: Verify Convergence Properties

Theoretical Verification:

1. Check LTI dynamics assumption holds
2. Verify noise bounds match stochastic analysis assumptions
3. Validate density representation accuracy

Empirical Verification:

def verify_convergence(trajectories, rho_star, threshold=0.01):
    """
    Verify empirical convergence of the system.
    
    Args:
        trajectories: Agent position history
        rho_star: Target density
        threshold: Convergence threshold
    
    Returns:
        converged: Boolean
        final_error: Steady-state density mismatch
    """
    final_positions = trajectories[-1]
    rho_final = estimate_density(final_positions)
    error = np.mean((rho_final - rho_star)**2)
    return error < threshold, error

Advanced Topics

Non-Uniform Density Tracking

For time-varying target densities:

def time_varying_target(t):
    """Define time-varying density target."""
    # Example: Moving coverage window
    center = initial_center + velocity * t
    return gaussian_density(center, sigma)

# Update control law with time derivative
u_i = -K @ (x_i - x_target_i(t)) + feedforward_term

Handling Obstacles and Constraints

Barrier Function Approach:

def obstacle_barrier(x, obstacle_center, radius):
    """Compute barrier function for obstacle avoidance."""
    distance = np.linalg.norm(x - obstacle_center)
    if distance < radius:
        return float('inf')
    return -np.log(distance - radius)

# Modified control with barrier constraints
u_safe = project_to_safe_set(u_optimal, barriers)

Multi-Scale Coverage

For hierarchical coverage (macro + micro):

# Macro-level: Cluster assignment
cluster_assignments = cluster_agents_by_density(N_clusters)

# Micro-level: Intra-cluster density control
for cluster in clusters:
    u_cluster = density_driven_control(cluster.agents, cluster.local_density)

Implementation Examples

Example 1: 2D Area Coverage with Drone Swarm

import numpy as np
from scipy.linalg import solve_continuous_are

# System parameters
n_agents = 20
A = np.zeros((2, 2))  # Double integrator
B = np.eye(2)
Q = np.eye(2) * 10    # High priority on position accuracy
R = np.eye(2) * 0.1   # Moderate control cost

# Solve Riccati equation
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P

# Define target density (e.g., Gaussian centered at origin)
def target_density(x):
    return np.exp(-np.sum(x**2) / (2 * 5**2))

# Initialize agents
agent_positions = np.random.uniform(-10, 10, (n_agents, 2))

# Simulation loop
dt = 0.01
for t in range(1000):
    # Compute target positions from density
    target_positions = sample_from_density(target_density, n_agents)
    
    # Compute control for each agent
    for i in range(n_agents):
        error = agent_positions[i] - target_positions[i]
        control = -K @ error
        agent_positions[i] += control * dt

Example 2: Environmental Monitoring with Sensor Network

# Time-varying target density based on pollution measurements
def pollution_density(x, t, sensor_readings):
    """Dynamic density based on real-time sensor data."""
    base_density = uniform_density(x)
    for reading in sensor_readings:
        if reading.timestamp > t - dt:
            # Increase density near high pollution areas
            distance = np.linalg.norm(x - reading.location)
            base_density += gaussian(distance, reading.value)
    return base_density

# Adaptive control loop
while monitoring:
    current_readings = get_sensor_data()
    rho_star = lambda x: pollution_density(x, current_time, current_readings)
    
    # Recompute targets and controls
    targets = density_to_positions(rho_star, n_agents)
    controls = [compute_optimal_control(pos, tgt, K) 
                for pos, tgt in zip(agent_positions, targets)]

Comparison with Alternative Approaches

References

Core Paper

• Lee, K. (2026). "Density-Driven Optimal Control: Convergence Guarantees for Stochastic LTI Multi-Agent Systems." arXiv:2604.08495v1.

Related Work

• Cortés, J., et al. (2004). Coverage control for mobile sensing networks.
• Schwager, M., et al. (2009). Decentralized, adaptive coverage control.
• Liu, Y., et al. (2018). Density-aware robotic systems.

Mathematical Background

• Linear Quadratic Regulator (LQR) theory
• Voronoi tessellations and coverage
• Stochastic stability analysis
• Optimal transport theory

Tools and Resources

Required Libraries

# Core numerical computing
numpy >= 1.20.0
scipy >= 1.7.0

# Optional for visualization
matplotlib >= 3.4.0
plotly >= 5.0.0

# Optional for ROS integration
rospy >= 1.15.0

Visualization Tools

See references/visualization_examples.md for plotting density evolution, agent trajectories, and convergence metrics.

Troubleshooting

Common Issues

Issue: Slow convergence

• Check: Are Q and R matrices appropriately tuned?
• Solution: Increase Q for faster response, decrease R to allow larger controls

Issue: Oscillations around target

• Check: Is the system properly damped?
• Solution: Add velocity feedback or reduce proportional gain

Issue: Density estimation errors

• Check: Is the kernel density estimator bandwidth appropriate?
• Solution: Use adaptive bandwidth or increase agent count

Issue: Agents cluster in local optima

• Check: Is the density landscape multi-modal?
• Solution: Use simulated annealing or multi-start initialization

Extensions and Future Work

Potential Enhancements

• Nonlinear system extensions via feedback linearization
• Learning-based density estimation from data
• Game-theoretic formulations for competitive scenarios
• Hybrid discrete-continuous coverage problems

Research Opportunities

• Tightening convergence rate bounds
• Asynchronous and event-triggered control
• Communication-constrained implementations
• Real-world deployment case studies

Last updated: 2026-04-14 Source: arXiv:2604.08495v1 (April 2026)