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.
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?
cp.square(cp.pos(cp.norm(x[t] - sim.anchors[i]) - range_)) solvesthe problem