Implementation of the dwell-time aware sum-up rounding algorithm (DSUR)

dsur.py

import numpy as np
from numpy.typing import NDArray


def calculate_control_deviation(
    control: int,
    start_index: int,
    end_index: int,
    alpha: NDArray[np.float64],
    beta: NDArray[np.int32],
) -> float:
    """Calculate the control deviation for the control `control` within the discrete interval {start_index, ..., end_index}."""
    return np.sum(alpha[control, 0 : end_index + 1]) - np.sum(
        beta[control, 0 : start_index + 1]
    )


def update_configs(
    configs: dict[int, bool], beta: NDArray, start: int, num_dwell_steps: int
):
    """Update the set of allowed/disallowed controls within the discrete interval {start, ..., start + num_dwell_steps}"""
    for control in configs.keys():
        configs[control] = all(
            value == 0 for value in beta[control, start : start + num_dwell_steps]
        )


def find_control_with_maximal_deviation(
    time_start: int,
    time_end: int,
    alpha: NDArray[np.float64],
    beta: NDArray[np.int32],
    configs: dict[int, bool],
) -> int:
    """Find the control index which is still allowed according to configs
    and maximizes the accumulated control deviation."""

    control_deviations = {
        control: calculate_control_deviation(control, time_start, time_end, alpha, beta)
        for (control, is_allowed) in configs.items()
        if is_allowed
    }

    return max(
        control_deviations,
        key=lambda control: control_deviations[control],
    )


def sum_up_rounding(
    alpha: NDArray[np.float64], min_up_time_steps: int, min_down_time_steps: int
) -> NDArray[np.int32]:
    """Given a relaxed control alpha, compute a binary-feasible control which fulfills the given dwell-time constraints.
    The relaxed control alpha fulfills a SOS1-constraint, i.e. have sum(alpha[c, t] for all c) == 1 for each time step t."""
    beta = np.zeros_like(alpha, dtype=np.int32)
    num_controls, num_time_steps = alpha.shape
    configs: dict[int, bool] = {control: True for control in range(num_controls)}
    time_step = 0
    max_dwell_time = max(min_up_time_steps, min_down_time_steps)

    while time_step < num_time_steps:
        dwell_time = min_up_time_steps if time_step == 0 else max_dwell_time
        time_rounding_end = min(time_step + dwell_time - 1, num_time_steps - 1)
        control = find_control_with_maximal_deviation(
            time_step, time_rounding_end, alpha, beta, configs
        )

        # round up
        beta[control, time_step : time_rounding_end + 1] = 1

        time_step = time_rounding_end + 1
        update_configs(configs, beta, time_step, dwell_time)
    return beta


if __name__ == "__main__":
    # Relaxed control fulfilling SOS1-constraint
    # alpha = np.array(
    #     [
    #         [
    #             0.47131227,
    #             0.78736104,
    #             0.97325193,
    #             0.53496864,
    #             0.73187786,
    #             0.07838749,
    #             0.48948843,
    #             0.64580892,
    #         ],
    #         [
    #             0.52868773,
    #             0.21263896,
    #             0.02674807,
    #             0.46503136,
    #             0.26812214,
    #             0.92161251,
    #             0.51051157,
    #             0.35419108,
    #         ],
    #     ],
    #     dtype=np.float64,
    # )

    alpha = np.tile(np.array([[1 - 1e-8], [1e-8]]), (1, 20))

    # time grid
    dt = 1.0

    b = sum_up_rounding(alpha, min_up_time_steps=3, min_down_time_steps=3)
    print(b)