代码如下,这个代码是我根据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)