Solving trilateration problem

[Titibo26]

Hi, I'm trying to solve trilateration problem assuming ranges are erroneous. I know 3 position with 3 ranges and i want to compute the best position of the 4th sensor.

My code is as follow :

(Error function)

public double BobyqaTestCalfun(int n, double[] x)
        {
            var f = 0.0;

            //f = (Math.Abs(Math.Sqrt((x[0] - pos[0][0]) * (x[0] - pos[0][0]) + (x[1] - pos[0][1]) * (x[1] - pos[0][1]) + (x[2] - pos[0][2]) * (x[2] - pos[0][2])) - range[0]) + Math.Abs(Math.Sqrt((x[0] - pos[1][0]) * (x[0] - pos[1][0]) + (x[1] - pos[1][1]) * (x[1] - pos[1][1]) + (x[2] - pos[1][2]) * (x[2] - pos[1][2])) - range[1]) + Math.Abs(Math.Sqrt((x[0] - pos[2][0]) * (x[0] - pos[2][0]) + (x[1] - pos[2][1]) * (x[1] - pos[2][1]) + (x[2] - pos[2][2]) * (x[2] - pos[2][2])) - range[2])) / 3;
            f = (Math.Pow(Math.Sqrt((x[0] - pos[0][0]) * (x[0] - pos[0][0]) + (x[1] - pos[0][1]) * (x[1] - pos[0][1]) + (x[2] - pos[0][2]) * (x[2] - pos[0][2])) - range[0],2.0) + Math.Pow(Math.Sqrt((x[0] - pos[1][0]) * (x[0] - pos[1][0]) + (x[1] - pos[1][1]) * (x[1] - pos[1][1]) + (x[2] - pos[1][2]) * (x[2] - pos[1][2])) - range[1], 2.0) + Math.Pow(Math.Sqrt((x[0] - pos[2][0]) * (x[0] - pos[2][0]) + (x[1] - pos[2][1]) * (x[1] - pos[2][1]) + (x[2] - pos[2][2]) * (x[2] - pos[2][2])) - range[2],2.0)) / 3;

            // Debug.WriteLine(f);

            return f;
        }

    (set & Optimization)

        public double[] FindMinimum(List<double[]> poss, List<double> ranges)
        {
            pos = poss;
            range = ranges;

            var computedPos = new double[3];
            //Ref formuule :https://pdfs.semanticscholar.org/62ef/01bf24ce619e5e9bd4ae911115381000679b.pdf

            const int iprint = 1;
            const int maxfun = 500000;
            const double rhobeg = 200f;
            const double rhoend = 0.000000000000000000000000000000001f;

          var optimizer = new Bobyqa(3, this.BobyqaTestCalfun, new double[] { -10000, -10000, 0 }, new double[] { 10000, 10000, 3000 });

            optimizer.TrustRegionRadiusStart = rhobeg;
            optimizer.InterpolationConditions = 8; //2*n+1
            optimizer.TrustRegionRadiusEnd = rhoend;
            optimizer.PrintLevel = iprint;
            optimizer.MaximumFunctionCalls = maxfun;
            optimizer.Logger = Console.Out;
            double[] midPts = new double[] { (pos[0][0] + pos[1][0] + pos[2][0]) / 3, (pos[0][1] + pos[1][1] + pos[2][1]) / 3, (pos[0][2] + pos[1][2] + pos[2][2])/3 };

            /*
            if (range[0] < range[1] && range[0] < range[2])
            {
                midPts = pos[0];
            }
            else if(range[1] < range[0] && range[1] < range[2])
            {
                midPts = pos[1];
            }
            else
            {
                midPts = pos[2];
            }
            */

            var summary = optimizer.FindMinimum(midPts);

            computedPos = summary.X;
          //  var ftest =  Math.Abs(Math.Sqrt((summary.X[0] - pos[0][0]) * (summary.X[0] - pos[0][0]) + (summary.X[1] - pos[0][1]) * (summary.X[1] - pos[0][1]) + (summary.X[2] - pos[0][2]) * (summary.X[2] - pos[0][2])) - range[0]) + Math.Abs(Math.Sqrt((summary.X[0] - pos[1][0]) * (summary.X[0] - pos[1][0]) + (summary.X[1] - pos[1][1]) * (summary.X[1] - pos[1][1]) + (summary.X[2] - pos[1][2]) * (summary.X[2] - pos[1][2])) - range[1]) + Math.Abs(Math.Sqrt((summary.X[0] - pos[2][0]) * (summary.X[0] - pos[2][0]) + (summary.X[1] - pos[2][1]) * (summary.X[1] - pos[2][1]) + (summary.X[2] - pos[2][2]) * (summary.X[2] - pos[2][2])) - range[2]);
          //  Debug.Write(" ftest  " + ftest + "   ");
            return computedPos;

        }

This code is my best set of parameter so far, i'm wondering if i'm setting "rhobeg" and "rhoend" well... The optimization sometimes works but most of the time it fails while being far from asked accuracy. Any clue on why optimization is innacurate most of the time ?