最近在做基于tdoa的室内目标定位的研究，主要是做chan算法定位方面的东西，但是做了实验之后发现效果不太稳定。师哥发给我一份他改写的scipy.optimize.minimize的函数，说是用这个就可以定出位置，然后让我好好看一看，最好能看懂，如下（注释是我自己写的，不知道都对不对）
def opt_location(range, anchors): #range是真实测得的toa
def con():
# 约束条件 分为eq 和ineq
# eq表示 函数结果等于0 ； ineq 表示 表达式大于等于0
x1min, x1max, x2min, x2max = -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})
return cons # **************我也不知道为什么约束条件要这么设置
def cost(pos): #***************
ref = np.sqrt(np.sum((anchors-pos.reshape(1,2))**2, axis=1)) #anchors为四个基站的位置，pos为要优化的位置
ref0 = ref[1:] - ref[0]
ref1 = ref[2:] - ref[1]
ref2 = ref[3:] - ref[2] #tdoa
Ri_0 = range[1:] - range[0]
Ri_1 = range[2:] - range[1]
Ri_2 = range[3:] - range[2] #真实测得的tdoa
return np.sum((Ri_0 - ref0)**2) + np.sum((Ri_1 - ref1)**2) + np.sum((Ri_2 - ref2)**2) #目标函数的目的是要求所估计位置和各基站的tdoa
#和真实测得的tdoa相差最小
x0 = np.array([5, 5])
res = minimize(cost, x0, constraints=con()).x
return res
之后，我的任务参照这个函数的优化方式，把chan算法的核心方程组（线性方程组）用这个优化算法求解出来，如图红框
这篇关于chan算法的csdn博客的链接是：https://blog.csdn.net/qq_23947237/article/details/82715784
由于之前没有接触过优化方面的东西，所以我没有思路，师哥说让我找出来目标函数和约束条件，然后依葫芦画瓢就行，但是我还是一头雾水，希望大佬可以帮忙解决！下面是我最后通过chan算法的第一次ls粗略位置估计得出Za0的代码，我的任务就是把这个Za0用优化的方式求出来
from math import *
from numpy import *
import numpy as np
from numpy.linalg import inv, qr
BSN = 4 #基站数目
BS=np.array([[0,0,10,10,-sqrt(3),dot(- 0.5,sqrt(3)),dot(0.5,sqrt(3))],[0,10,10,0,0,-1.5,-1.5]]) #锚点位置
BS=BS[:,:BSN] #取前4列
#BS=multiply(BS,50) #数值×50 四个基站位置固定
print("-----------------------------BS--------------------------------")
print(BS) #看看基站位置
MS=np.array([10,4]) #设置移动台初始位置
print("-----------------------------MS--------------------------------")
print(MS) #看看移动台位置
R0=[] #R0为四个锚点分别到移动台的距离
R0.clear() #清空R0
for i in range(0,BSN):
R0.append(sqrt((BS[(0,i)] - MS[0]) ** 2 + (BS[(1,i)] - MS[1])**2))#计算基站到移动台的距离
R0 = np.array(R0) #转换为矩阵
print("-----------------------------R0--------------------------------")
print(R0)
Noise = 1 #噪声方差
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(R0[i]-R0[0]+Noise*r[i-1])#计算距离差 R2-R1 R3-R1 R4-R1 并加噪声
R = np.array(R) #转换为矩阵
print("-----------------------------R--------------------------------")
print(R) #BSN BS R都有了，开始进行算法部分
Q = eye(BSN-1)
print("-----------------------------Q--------------------------------")
print(Q)
K1 = 0
k = (BS**2).sum(0) #BS各基站的x平方+y平方
print("-----------------------------K--------------------------------")
print(k)
Ga = np.array([[1.0,1.0,1.0],[1.0,1.0,1.0],[1.0,1.0,1.0]]) #弄一个3*3的矩阵
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]
print("-----------------------------Ga--------------------------------")
print(Ga)
h = []
for i in range(0,BSN-1):
h.append(0.5*(R[i]**2-k[i+1]+K1))
h = np.array(h)
print("-----------------------------h--------------------------------")
print(h)
Za0 = inv((Ga.T).dot(inv(Q)).dot(Ga)).dot((Ga.T).dot(inv(Q)).dot(h.T)) #第一次粗略估计的位置，使用这个值来估计B
print("-----------------------------Za0--------------------------------")
print(Za0)
主要是不知道思路和怎么写代码，麻烦了，谢谢！
