这个测试目标定位的优化代码问题出在哪里呢？

代码如下，这个代码是我根据scipy.optimize.minimize以及chan氏算法写出来的，目的是用scipy来对chan算法的线性方程组（h=Gz0）来进行直接优化求解，但是不知道这个代码哪里出问题了，我运行后结果时而变得成千上万那么大，时而很准确（靠近5 5 0），时而就保持（5 5 0）这个数值不变，请大佬帮忙看一下！
chan算法https://blog.csdn.net/qq_23947237/article/details/82715784

import numpy as np
from math import *
import matplotlib.pyplot as plt
from scipy.optimize import minimize
from numpy import *
from numpy.linalg import inv, qr

def opt_location(range_toa, anchors):               #range是真实测得的 *toa*  range是1*4矩阵，anchors是4*2矩阵每行是一个基站坐标

    def con():
        # 约束条件 分为eq 和ineq
        # eq表示 函数结果等于0 ； ineq 表示 表达式大于等于0
        x1min, x1max, x2min, x2max , x3min ,x3max = -5, 15, -5, 15,-5,15
        cons = ({'type': 'ineq', 'fun': lambda x: x[0] - x1min},              #x[0]-x1min >= 0
                {'type': 'ineq', 'fun': lambda x: -x[0] + x1max},
                {'type': 'ineq', 'fun': lambda x: x[1] - x2min},
                {'type': 'ineq', 'fun': lambda x: -x[1] + x2max},
                {'type': 'ineq', 'fun': lambda x: x[2]-x3min},
                {'type': 'ineq', 'fun': lambda x: -x[2]+x3max})
        return cons                

    def cost(pos):                 #***************通过range和anchors来计算Ga和h等 注意range是TOA
        BS = anchors.T         #根据算法先转置
        BSN = 4                #4锚点
        Noise = 1           #噪声方差
        distance = 299792458 * range_toa
        R = []
        R.clear()
        a = random.random(3)
        r = np.zeros(3)
        for i in range(0,3):
            r[i]=2*a[i]-1                                   #随机生成一个-1到1的数
        for i in range(1,BSN):  
            R.append(distance[i]-distance[0]+Noise*r[i-1])  #计算到达距离差 R2-R1 R3-R1 R4-R1 并加噪声
        R = np.array(R)                                     #转换为矩阵
        Q = eye(BSN-1)                                      #Q用得到吗？
        K1 = 0
        k = (BS**2).sum(0)                                  #BS各基站的x平方+y平方
        Ga = np.zeros((3,3),dtype = float)
        for i in range(0,BSN-1):           
            Ga[i][0] = -BS[0][i+1]
            Ga[i][1] = -BS[1][i+1]
            Ga[i][2] = -R[i]                               #构建Ga
        h = []
        for i in range(0,BSN-1):
            h.append(0.5*(R[i]**2-k[i+1]+K1))
        h = np.array(h)                                    #构建h
        z = pos
        return np.sum((Ga.dot(z)-h)**2)                                                      

    x0 = np.array([5 , 5 , 0])
    res = minimize(cost, x0, constraints=con()).x
    return res

#进入到测试函数部分
range_toa = np.array([0.00000002358654336 , 0.00000002358654336 , 0.00000002358654336 , 0.00000002358654336])  #此时移动台位置为[5,5]
anchors = np.array([[0,0],[0,10],[10,10],[10,0]])
#print(anchors)
pos = np.array([5,5,0])
a = opt_location(range_toa, anchors)
print(a)

你好，我是有问必答小助手，非常抱歉，本次您提出的有问必答问题，技术专家团超时未为您做出解答

本次提问扣除的有问必答次数，已经为您补发到账户，我们后续会持续优化，扩大我们的服务范围，为您带来更好地服务。