Error when using lsqnonlin in for loop

Hello, I am having some issuen when trying to implement the lsqnonlin iteration into a for loop.

I have the 3d coordinates of four known points, and also four known distances between each point to a fifth unknown point.

As I have a time-series of data I wanted to create a function that loops through all the data:

function Estimated_Position = trilaterate_new(p1, p2, p3, p4, distances, initial_guess)
% p1-p4 = 3D coordinated for the four known points, each the size of 100x3 (ex [x1 y1 z1; x2, y2, z2]
% distances = Known distance from each point to the fifth unknown point (ex. [d1 d2 d3 d4])
% initial_guess = initial guess of the position of  point 5 (ex. [0 0 0])
options = optimoptions(@lsqnonlin,'Display','off');
for i = 1:100
    objective_function = @(p5) [
        sqrt((p1(i,1) - p5(1))^2 + (p1(i,2) - p5(2))^2 + (p1(i,3) - p5(3))^2) - distances(i,1);
        sqrt((p2(i,1) - p5(1))^2 + (p2(i,2) - p5(2))^2 + (p2(i,3) - p5(3))^2) - distances(i,2);
        sqrt((p3(i,1) - p5(1))^2 + (p3(i,2) - p5(2))^2 + (p3(i,3) - p5(3))^2) - distances(i,3);
        sqrt((p4(i,1) - p5(1))^2 + (p4(i,2) - p5(2))^2 + (p4(i,3) - p5(3))^2) - distances(i,4)];
    
    Estimated_Position(i,:) = lsqnonlin(objective_function, initial_guess, [], [], options);
end
end

The issue occurs when i=2 and I get down to execute lsqnonlin. I then receieve the error "Index in position 1 exceeds array bounds. Index must not exceed 1."

If anyone has any suguestions, please let me know

[Torsten]

Execute the code

function Estimated_Position = trilaterate_new(p1, p2, p3, p4, distances, initial_guess)
% p1-p4 = 3D coordinated for the four known points, each the size of 100x3 (ex [x1 y1 z1; x2, y2, z2]
% distances = Known distance from each point to the fifth unknown point (ex. [d1 d2 d3 d4])
% initial_guess = initial guess of the position of  point 5 (ex. [0 0 0])
size(p1)
size(p2)
size(p3)
size(p4)
size(distances)
size(initial_guess)
end

and tell us what MATLAB reports about the sizes.

[Ellen Ryckenberg]

Hi! This is the size output:

ans =
   100     3
ans =
   100     3
ans =
   100     3
ans =
   100     3
ans =
     1     4
ans =
     1     3

[dpb]

i=1;
N=10;
p1=rand(N,3); p2=rand(N,3); p3=rand(N,3); p4=rand(N,3); distances=rand(N,4);
initial_guess=ones(3,1);
for i=1:4
  objective_function = @(p5) [
        sqrt((p1(i,1) - p5(1))^2 + (p1(i,2) - p5(2))^2 + (p1(i,3) - p5(3))^2) - distances(i,1);
        sqrt((p2(i,1) - p5(1))^2 + (p2(i,2) - p5(2))^2 + (p2(i,3) - p5(3))^2) - distances(i,2);
        sqrt((p3(i,1) - p5(1))^2 + (p3(i,2) - p5(2))^2 + (p3(i,3) - p5(3))^2) - distances(i,3);
        sqrt((p4(i,1) - p5(1))^2 + (p4(i,2) - p5(2))^2 + (p4(i,3) - p5(3))^2) - distances(i,4)];
  Estimated_Position(i,:) = lsqnonlin(objective_function, initial_guess);
end

Local minimum found.

Optimization completed because the size of the gradient is less than
the value of the optimality tolerance.


Local minimum possible.

lsqnonlin stopped because the final change in the sum of squares relative to 
its initial value is less than the value of the function tolerance.


Local minimum found.

Optimization completed because the size of the gradient is less than
the value of the optimality tolerance.


Local minimum possible.

lsqnonlin stopped because the final change in the sum of squares relative to 
its initial value is less than the value of the function tolerance.

Estimated_Position

Estimated_Position = 4×3

0.2834 -0.2140 0.3629 0.4751 0.1628 0.3481 0.8198 0.7758 0.6734 0.4578 0.5201 0.6442

Seems to work ok with made up data; something must be in what isn't being shown...

NOTA BENE: The array for Estimated_Position is not preallocated; perhaps your code sets an initial single vector?

clear Estimated_Position
Estimated_Position=zeros(1,3);      % preset an initial vector
i=1;
N=10;
p1=rand(N,3); p2=rand(N,3); p3=rand(N,3); p4=rand(N,3); distances=rand(N,4);
initial_guess=ones(3,1);
for i=1:4
  objective_function = @(p5) [
        sqrt((p1(i,1) - p5(1))^2 + (p1(i,2) - p5(2))^2 + (p1(i,3) - p5(3))^2) - distances(i,1);
        sqrt((p2(i,1) - p5(1))^2 + (p2(i,2) - p5(2))^2 + (p2(i,3) - p5(3))^2) - distances(i,2);
        sqrt((p3(i,1) - p5(1))^2 + (p3(i,2) - p5(2))^2 + (p3(i,3) - p5(3))^2) - distances(i,3);
        sqrt((p4(i,1) - p5(1))^2 + (p4(i,2) - p5(2))^2 + (p4(i,3) - p5(3))^2) - distances(i,4)];
  Estimated_Position(i,:) = lsqnonlin(objective_function, initial_guess);
end

Local minimum found.

Optimization completed because the size of the gradient is less than
the value of the optimality tolerance.


Local minimum possible.

lsqnonlin stopped because the final change in the sum of squares relative to 
its initial value is less than the value of the function tolerance.


Local minimum possible.

lsqnonlin stopped because the final change in the sum of squares relative to 
its initial value is less than the value of the function tolerance.


Local minimum possible.

lsqnonlin stopped because the final change in the sum of squares relative to 
its initial value is less than the value of the function tolerance.

Estimated_Position

Estimated_Position = 4×3

0.6266 0.2776 0.3004 0.0396 0.6907 0.5794 0.2869 0.9305 0.8099 0.4261 0.5603 0.7756

Well, no, that didn't cause a bounds error, either.

We'll have to see the actual code that created the error, can't reproduce it from the snippet given, sorry...

Answer 1

[Torsten]

As you can see, the dimension of "distances" is only 1x4. But it should be 100x4.

And better use

objective_function = @(p5) [
    (p1(i,1) - p5(1))^2 + (p1(i,2) - p5(2))^2 + (p1(i,3) - p5(3))^2 - distances(i,1)^2;
    (p2(i,1) - p5(1))^2 + (p2(i,2) - p5(2))^2 + (p2(i,3) - p5(3))^2 - distances(i,2)^2;
    (p3(i,1) - p5(1))^2 + (p3(i,2) - p5(2))^2 + (p3(i,3) - p5(3))^2 - distances(i,3)^2;
    (p4(i,1) - p5(1))^2 + (p4(i,2) - p5(2))^2 + (p4(i,3) - p5(3))^2 - distances(i,4)^2];

instead of

objective_function = @(p5) [
    sqrt((p1(i,1) - p5(1))^2 + (p1(i,2) - p5(2))^2 + (p1(i,3) - p5(3))^2) - distances(i,1);
    sqrt((p2(i,1) - p5(1))^2 + (p2(i,2) - p5(2))^2 + (p2(i,3) - p5(3))^2) - distances(i,2);
    sqrt((p3(i,1) - p5(1))^2 + (p3(i,2) - p5(2))^2 + (p3(i,3) - p5(3))^2) - distances(i,3);
    sqrt((p4(i,1) - p5(1))^2 + (p4(i,2) - p5(2))^2 + (p4(i,3) - p5(3))^2) - distances(i,4)];

[Ellen Ryckenberg]

Ah now I realize my mistake, thank you so much for your answers!