CVX range location optimization yields weird results


I have been banging my head for while trying to understand why my optimization is not working properly.

Here is the problem: there is a set of points (from a trajectory but that is not relevant). and 3 anchors.
The information that we have is the ranges seen by the 3 anchors, and the CVX will try to minimize a cost function to determine the position. The original points are plotted in green, and the red ones are the optimized points. As you can see, the program fails to find the correct points when they are outside the triangle formed by the three anchors. My professor does not understand why this is, and gets the same result. It should be able to locate every point, because we are in 2D and there are 3 anchors and 3 ranges for each point. Also, part of the cost function relates to velocities, but ignore that, that term is set to be equal to zero here anyway.

% Load data
%%%%% Data imported from generate_trajectory_data.m :
%anchor - coordinates for the anchors
%velocities - velocities measurements at each position
%points - coordinates for the positions
%ranges - range measurements of the anchors at each position
%angles - angle measurements of the anchors at each position
%Delta - time step between positions = 1/sampling rate

% Define the row index you want to remove
to_remove = 1;  % For example, to remove the second row

% Use indexing to remove the specified row
anchor(to_remove, :) = [];
ranges(:, to_remove) = [];

% Define parameters
T = size(points, 1);  % Number of time steps
M = size(ranges, 2);  % Number of anchors
mu = 0;  % Define a specific mu value
Delta = 1;  % Assuming Delta is 1, adjust as per your data

% Introduce noise to the measurements
noise_level = 0.1;  % Define the noise level
ranges_noisy = ranges ;%+ noise_level * randn(size(ranges));
velocities_noisy = velocities + noise_level * randn(size(velocities));

% Calculate velocities
vx = diff(points(:, 1)) / Delta;
vy = diff(points(:, 2)) / Delta;
velocity = [vx, vy];

% Begin CVX optimization

    variable x(T, 2)

    objective = 0;
    for t = 1:T
        % Add to the cost the measured ranges error with noise
        for k = 1:M
            difference = norm(x(t, :) - anchor(k, :)) - ranges_noisy(t,k);
            objective = objective + square_pos(difference);

        % Add to the cost the measured velocity error with noise
        if t == 1
            velocities_hat = (x(t+1, :) - x(t, :)) / Delta;
        elseif t == T
            velocities_hat = (x(t, :) - x(t-1, :)) / Delta;
            velocities_hat = (x(t+1, :) - x(t-1, :)) / (2*Delta);
        difference = velocities_hat - velocities_noisy(t, :);
        objective = objective + mu * quad_form(difference, eye(length(difference)));
    % Minimize the cost


% Create a scatter plot
figure; hold on;
scatter(x(:,1), x(:,2), 'r', 'filled');   % 'r' for red color
scatter(points(:,1), points(:,2), 'g*');  % 'g' for green color
scatter(anchor(:,1), anchor(:,2), 'bo');  % 'b' for blue color

% Set the axis limits
axis([-20 20 -20 20]);
axis square; % This ensures the aspect ratio is 1:1

% Assuming 'ranges' is a matrix where each row corresponds to a point and each column to an anchor
[numPoints, numAnchors] = size(ranges);

for i = 8:10
    for j = 1:numAnchors
        viscircles(anchor(j, :), ranges(i, j), 'LineWidth', 0.5, 'EdgeColor', 'c');  % 'c' for cyan color

for i = 8:10
    for j = 1:numAnchors
        distance = sqrt((x(i, 1) - anchor(j, 1))^2 + (x(i, 2) - anchor(j, 2))^2);
        viscircles(anchor(j, :), distance, 'LineWidth', 0.5, 'EdgeColor', 'r'); 

title('Scatter Plot of x and points with Noise');
legend('x', 'points');
grid on;

% Display the figure
hold off;

% Save the estimated positions from Task 12
save('estimated_positions_noise.mat', 'x');


Why not reduce complications for debugging purposes by trying your procedure with no noise added?

There is no noise in the code, the line for adding noise to the ranges is commented out

velocities_noisy = velocities + noise_level * randn(size(velocities));
isn’t commented out, and velocities_noisy feeds into the objective.

Right but as I said, that term is zero (multiplied by mu=0)

Sorry, I missed that.