4. UWB定位算法：三边定位、最小二乘法与卡尔曼滤波
聊完UWB的物理层和测距原理，咱们终于到了最核心的部分——定位算法。说白了，测距只是拿到了“距离值”，而定位是把这些距离值变成坐标。我见过不少项目，硬件选得挺好，测距精度也不错，但最后定位效果一塌糊涂，问题就出在算法上。
今天咱们把三种最常用的定位算法掰开揉碎讲清楚：三边定位、最小二乘法和卡尔曼滤波。它们不是互斥的，而是层层递进的关系。
核心逻辑：三边定位是基础理论，最小二乘法解决实际测量误差，卡尔曼滤波让结果更平滑稳定。实际项目中，这三者经常组合使用。
4.1 三边定位：最直观的几何解法
三边定位的原理其实特别简单。你想想看，如果我知道自己到A点的距离是d1，那我一定在以A为圆心、d1为半径的圆上。同样，到B点距离d2，就在以B为圆心的圆上。两个圆相交，得到两个候选点。再加第三个圆，就能唯一确定位置。
数学上，假设三个基站坐标分别为 (x1,y1)、(x2,y2)、(x3,y3)，测距值分别为 d1、d2、d3，那么待定位点 (x,y) 满足：
(x - x1)² + (y - y1)² = d1²
(x - x2)² + (y - y2)² = d2²
(x - x3)² + (y - y3)² = d3²把前两个方程减去第三个，消去二次项，就得到线性方程组：
2(x1 - x3)x + 2(y1 - y3)y = d3² - d1² + x1² - x3² + y1² - y3²
2(x2 - x3)x + 2(y2 - y3)y = d3² - d2² + x2² - x3² + y2² - y3²解这个二元一次方程组，就能得到坐标。
我曾经踩过的坑：三边定位在理想情况下完美，但实际测距值总有误差。三个圆可能不交于一点，而是形成一个“误差三角形”。这时候直接解方程会得到荒谬结果。我早期一个项目就因为这个，定位点直接飞到基站外面去了。
所以，三边定位更适合作为初始值估计，或者基站布局非常理想（基站围成等边三角形，标签在内部）的场景。基站少于3个时无法定位，多于3个时又浪费了信息——这就引出了最小二乘法。
4.2 最小二乘法：用冗余数据对抗误差
实际UWB定位系统通常部署4-8个基站。为什么不用3个就够了？因为冗余能提高精度。最小二乘法就是利用这些冗余数据，找到“最可能”的位置。
它的核心思想很简单：找一个点，使得它到所有基站的理论距离与实际测距值的误差平方和最小。
数学表达为：
min Σ [ √((x - xi)² + (y - yi)²) - di ]²这个方程是非线性的，没法直接解。常用的做法是线性化迭代，比如高斯-牛顿法：
- 先给一个初始坐标估计（比如用三边定位的结果）
- 计算当前估计值到各基站的理论距离
- 计算理论距离与实际测距的差值（残差）
- 构建雅可比矩阵，求解增量 Δx、Δy
- 更新坐标估计，重复2-5步直到收敛
代码实现大致长这样：
def least_squares_uwb(anchors, distances, initial_guess):
"""
anchors: 基站坐标列表 [(x1,y1), (x2,y2), ...]
distances: 对应测距值列表 [d1, d2, ...]
initial_guess: 初始坐标 (x0, y0)
"""
x, y = initial_guess
for _ in range(10): # 迭代10次足够
# 计算理论距离
d_est = [np.sqrt((x-ax)**2 + (y-ay)**2)
for ax, ay in anchors]
# 计算残差
residuals = [d_est[i] - distances[i]
for i in range(len(anchors))]
# 构建雅可比矩阵
J = []
for ax, ay in anchors:
d = np.sqrt((x-ax)**2 + (y-ay)**2)
J.append([(x-ax)/d, (y-ay)/d])
J = np.array(J)
# 求解增量 (H = J^T J)
delta = np.linalg.inv(J.T @ J) @ J.T @ residuals
x -= delta[0]
y -= delta[1]
if np.linalg.norm(delta) < 1e-6:
break
return x, y我个人习惯：迭代次数设10次就够了，再多也没意义。收敛阈值设1e-6米，UWB的物理精度极限也就厘米级。另外，初始值别随便给——用前三个基站的测距做三边定位，或者直接用最近基站的坐标加个偏移，都比给(0,0)强得多。
最小二乘法能处理任意数量的基站，基站越多，定位越稳。但它有个前提：测距误差服从高斯分布。如果遇到非视距（NLOS）导致的巨大偏差，最小二乘法会被严重带偏。这时候就需要做数据筛选或鲁棒估计。
4.3 卡尔曼滤波：让定位轨迹“丝滑”起来
三边定位和最小二乘法处理的是单帧数据。但UWB定位通常是连续输出的——每秒10次、50次甚至100次。如果每帧独立计算，你会发现定位点跳来跳去，像喝醉了一样。
卡尔曼滤波就是来解决这个问题的。它把定位看作一个时序过程，结合物理运动模型（比如匀速运动）和当前观测值，给出最优估计。
卡尔曼滤波分两步：预测和更新。
| 步骤 | 做什么 | 数学本质 |
|---|---|---|
| 预测 | 根据上一时刻的状态，推测当前时刻的状态和协方差 | 状态方程：x̂ₖ = F·xₖ₋₁ + B·uₖ |
| 更新 | 用当前测距值（经最小二乘法得到的坐标）修正预测值 | 观测方程：zₖ = H·xₖ + vₖ |
对于UWB定位，状态量通常取 [x, y, vx, vy]——位置和速度。观测值就是最小二乘法算出的 [x, y]。
核心代码片段：
class KalmanFilterUWB:
def __init__(self, dt=0.1):
# 状态向量 [x, y, vx, vy]
self.x = np.zeros((4, 1))
# 状态转移矩阵 (匀速模型)
self.F = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 观测矩阵 (只观测位置)
self.H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
# 过程噪声协方差
self.Q = np.eye(4) * 0.01
# 观测噪声协方差 (来自最小二乘的残差)
self.R = np.eye(2) * 0.1
# 误差协方差矩阵
self.P = np.eye(4) * 1.0
def predict(self):
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
def update(self, z):
# z: 最小二乘法得到的 [x, y]
y = z - self.H @ self.x # 残差
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S) # 卡尔曼增益
self.x = self.x + K @ y
self.P = (np.eye(4) - K @ self.H) @ self.P
return self.x[0,0], self.x[1,0]关键参数调优经验：
- Q矩阵（过程噪声）：设太大，滤波结果会跟着测量值剧烈跳动；设太小，轨迹过于平滑，跟不上快速移动。我一般先设0.01，然后根据实际运动速度调整。
- R矩阵（观测噪声）：可以从最小二乘法的残差中实时估算。残差大说明当前观测不可靠，R自动增大，滤波更依赖预测。
- dt（时间间隔）：必须和UWB的刷新率匹配。如果标签每秒输出10次，dt=0.1s。不匹配会导致模型失效。
4.4 三种算法的组合使用
在实际项目中，我很少只用其中一种。标准的处理流程是：
- 原始测距值（TOA/TDOA）进来，先做野值剔除（比如距离突变超过1米的直接扔掉）
- 三边定位给出初始坐标估计（或者用前3个最强信号的基站）
- 最小二乘法利用所有基站数据优化坐标，同时输出残差作为观测质量的指标
- 卡尔曼滤波对时序坐标做平滑和预测，输出最终位置
我曾经翻过车的地方：有次做AGV小车定位，卡尔曼滤波的Q矩阵设得太小，小车急转弯时滤波轨迹跟不上，偏差了半米多。后来加了自适应Q矩阵——根据加速度大小动态调整过程噪声，才算搞定。所以记住：卡尔曼滤波的参数不是一劳永逸的，要根据运动场景调。
4.5 算法选型建议
| 应用场景 | 推荐算法组合 | 原因 |
|---|---|---|
| 静态定位（资产盘点） | 最小二乘法 + 均值滤波 | 不需要实时跟踪，卡尔曼滤波反而可能引入滞后 |
| 低速移动（人员定位） | 最小二乘法 + 标准卡尔曼滤波 | 步行速度慢，匀速模型够用 |
| 高速移动（AGV、无人机） | 最小二乘法 + 扩展卡尔曼滤波（EKF） | 需要加速度模型，标准KF跟不上 |
| NLOS严重环境 | 鲁棒最小二乘法 + 粒子滤波 | 卡尔曼滤波假设高斯噪声，NLOS下会失效 |
嗯，算法这块内容不少，但核心就三条：三边定位给初值，最小二乘法抗误差，卡尔曼滤波稳轨迹。把这套组合拳打熟练了，UWB定位项目的基本盘就稳了。