传统最小二乘空间定位原理

假设UWB定位系统里有n个基站。基站坐标设为$(x_i,y_i,z_i)$(i=1,2,3…),标签坐标为（x,y,z),标签到基站的距离设为$d_i(i=1,2,\mathrm{3...})$可得以下关系式
$$\{\begin{array}{l}{\left(x-x_1\right)}^2+{\left(y-y_1\right)}^2+{\left(z-z_1\right)}^2=d_1^2\\ {\left(x-x_2\right)}^2+{\left(y-y_2\right)}^2+{\left(z-z_2\right)}^2=d_2^2\\ \cdots \\ {\left(x-x_n\right)}^2+{\left(y-y_n\right)}^2+{\left(z-z_n\right)}^2=d_n^2\end{array}$$
从第二式开始减去第一式，消去$x^2,y^2,z^2$可得
$$\{\begin{array}{l}({x_2}^2-{x_1}^2)+({y_2}^2-{y_1}^2)+({z_2}^2-{z_1}^2)-2(x_2-x_1)x-2(y_2-y_1)y-2(z_2-z_1)z={d_2}^2-{d_1}^2\\ ({x_3}^2-{x_1}^2)+({y_3}^2-{y_1}^2)+({z_3}^2-{z_1}^2)-2(x_3-x_1)x-2(y_3-y_1)y-2(z_3-z_1)z={d_3}^2-{d_1}^2\\ ⋮\\ ({x_n}^2-{x_1}^2)+({y_n}^2-{y_1}^2)+({z_n}^2-{z_1}^2)-2(x_n-x_1)x-2(y_n-y_1)y-2(z_n-z_1)z={d_n}^2-{d_1}^2\end{array}$$

化成矩阵形式为AX=B
$$A=2[\begin{array}{cccc}{x}_1-x_2& & y_1-y_2& z_1-z_2\\ x_1-x_3& & y_1-y_3& z_1-y{z}_3\\ ⋮& & ⋮& \\ x_1-x_n& & y_1-y_n& z_1-z_n\end{array}]$$
$$X=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$
$$B=[\begin{array}{cc}{d_2}^2-{d_1}^2+({x_1}^2-{x_2}^2)+({y_1}^2-{y_2}^2)+({z_1}^2-{z_2}^2)& \\ {d_3}^2-{d_1}^2+({x_1}^2-{x_3}^2)+({y_1}^2-{y_3}^2)+({z_1}^2-{z_3}^2)& \\ ⋮& \\ {d_n}^2-{d_1}^2+({x_1}^2-{x_n}^2)+({y_1}^2-{y_n}^2)+({z_1}^2-{z_n}^2)& \end{array}]$$

最小二乘法推导：

要求X可得
$$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\frac{1}{2}(A^{T}A{)}^{-1}{A}^{T}B$$
要求三维坐标至少四个基站

python算法模拟：
这里先模拟二维的，使用最小二乘法求解时，会因基站z轴坐标差值小导致误差较大，相较与水平坐标，z坐标不可靠，故采用其他方法获取z值

import tkinter as tk

import numpy as np

import math

import random

import matplotlib.pyplot as plt

  

class MapApp:

    def __init__(self, master):

        self.master = master

        self.master.title("UWB Emulation")

        self.master.geometry("800x600")

  

        self.num_base_stations = 4

        self.base_stations = [(random.randint(50, 550), random.randint(50, 550)) for _ in range(self.num_base_stations)]

        self.noise_mean = 0

        self.noise_stddev = 1

  

        self.canvas = tk.Canvas(self.master, width=600, height=600, bg="white")

        self.canvas.pack(side=tk.LEFT)

  

        self.label = tk.Label(self.master, text="")

        self.label.pack()

  

        self.mouse_marker = None

        self.distance_circles = []

  

        self.create_widgets()

        # 鼠标移动时进行一次计算

        self.canvas.bind("<Motion>", self.show_mouse_position)

  

    def create_widgets(self):

        self.create_base_stations()

        self.create_noise_slider()

  

    def create_base_stations(self):

        i=0

        for station in self.base_stations:

  

            x, y = station

            self.canvas.create_oval(x - 5, y - 5, x + 5, y + 5, fill="red")

             # 在标签中显示每个基站的坐标

            station_label = tk.Label(self.master, text=f"Base Station {i+1}: ({x}, {y})")

            station_label.pack()

            i = i+1

  

    def calculate_distance(self, x, y):

        distances = [math.sqrt((x - station[0])**2 + (y - station[1])**2) + random.gauss(self.noise_mean, self.noise_stddev) for station in self.base_stations]

        return distances

  

    def draw_distance_circles(self, x, y, distances):

        for i, station in enumerate(self.base_stations):

            x0, y0 = station

            radius = distances[i]

            circle = self.canvas.create_oval(x0 - radius, y0 - radius, x0 + radius, y0 + radius, outline="blue")

            self.distance_circles.append(circle)

  
  

    #最小二乘法

  

    def uwb_LeastQusare(self, distance_vector):

        # 创建A矩阵

        x_1, y_1 = self.base_stations[0]

        a = np.array([[x_1 - station[0], y_1 - station[1]] for station in self.base_stations[1:]])

        # 创建B矩阵

        d_1 = distance_vector[0]

        # print('test')

        # print(distance_vector[1]**2 - d_1**2)

        # print(distance_vector[1]**2 - d_1**2 + (x_1**2 - self.base_stations[1][0]**2))

        # print(distance_vector[1]**2 - d_1**2 + (x_1**2 - self.base_stations[1][0]**2) + (y_1**2 - self.base_stations[1][1]**2))

        b = np.array([[distance_vector[i]**2 - d_1**2 + (x_1**2 - station[0]**2) + (y_1**2 - station[1]**2)] for i, station in enumerate(self.base_stations[1:],start=1)])

  

        X = np.dot(np.linalg.inv(np.dot(np.transpose(a), a)), np.dot(np.transpose(a), b)) * 0.5

        # print('aa:')

        # print(a)

        # print('bb:')

        # print(b)

        # print('cc')

        # print(X)

        return X;

  

  

    def show_mouse_position(self, event):

        if self.mouse_marker:

            self.canvas.delete(self.mouse_marker)

        true_val = event.x, event.y

        x, y = event.x, event.y

        distances = self.calculate_distance(x, y)

        distance_text = "\n".join([f"Base Station {i+1}: {dist:.2f}" for i, dist in enumerate(distances)])

        self.label.config(text=f"Mouse Position: ({x}, {y})\nDistances to Base Stations:\n{distance_text}")

        #最小二乘法求解

        result = self.uwb_LeastQusare(distances)

        Err_val = (true_val[0]-result[0])**2 + (true_val[1]-result[1])**2

        print('result'+'     '+str(result))

        print(Err_val)

  

        self.mouse_marker = self.canvas.create_oval(x - 3, y - 3, x + 3, y + 3, fill="green")

  

        for circle in self.distance_circles:

            self.canvas.delete(circle)

        self.distance_circles = []

  

        self.draw_distance_circles(x, y, distances)

  

    def create_noise_slider(self):

        noise_frame = tk.Frame(self.master)

        noise_frame.pack()

  

        noise_label = tk.Label(noise_frame, text="Noise Level:")

        noise_label.pack(side=tk.LEFT)

  

        noise_slider = tk.Scale(noise_frame, from_=0, to=100, resolution=0.1, orient=tk.HORIZONTAL, command=self.update_noise_level)

        noise_slider.pack(side=tk.LEFT)

  

    def update_noise_level(self, value):

        self.noise_stddev = float(value)

  

    def get_distance_values(self):

        # 获取显示在窗口上的距离值

        return self.label["text"]

  
  

def main():

    root = tk.Tk()

    app = MapApp(root)

    root.mainloop()

  

    # 获取显示在窗口上的距离值

    distance_values = app.get_distance_values()

    print("Distance Values:", distance_values)

  

if __name__ == "__main__":

    main()