Difficulty formulating optimisation problem according to DCP rules cvxpy

37 views Asked by At

I'm trying to calculate the position of a target over time according to relative range and angle measurements from static anchors. For that I'm trying to formalise the problem with the same formulation as the following image.

Formulation

def solve_task10(sim, mu):
    x = cp.Variable((T, 2))

    cost = 0
    for t in range(1, T):
        # sum((|x-ak| - r)^2)
        range_differences = 0
        for i in range(4):
            range_, angle, velocity = sim.get_anchor_stamped_measurment(i, t)
            range_differences += cp.square(cp.norm(x[t] - sim.anchors[i]) - range_)

        # |v_est - v|^2
        v_estimated = (
            x[t] - x[t - 1]
        )  # Difference between x at time t and x at time t-1
        velocity_difference = mu * cp.square(cp.norm(v_estimated - velocity))

        cost += range_differences + velocity_difference

    objective = cp.Minimize(cost)
    constraints = [x[0] == np.array([0, 0])]
    problem = cp.Problem(objective, constraints)
    problem.solve(solver=cp.ECOS)

    return x.value

The issue I'm encountering is on:

range_differences += cp.square(cp.norm(x[t] - sim.anchors[i]) - range_)

How can i reformulate the problem to be DCP compliant?

1

There are 1 answers

0
Diogo Ferreira On

cp.square(cp.pos(cp.norm(x[t] - sim.anchors[i]) - range_)) solvesthe problem