4. 定位算法基础：三边定位、最小二乘法与卡尔曼滤波入门

说到UWB定位，很多人第一反应就是“测距准不准”。但说实话，测距只是第一步。真正决定定位精度的，是后面的算法。我见过不少项目，硬件选型花了大价钱，结果算法没选对，定位飘得没法看。

这一章，我们就来聊聊定位算法的三个核心：三边定位、最小二乘法、卡尔曼滤波。这三者层层递进，基本覆盖了UWB定位从静态到动态、从理想到现实的全部场景。

三边定位的原理，说白了就是初中几何——已知三个点的坐标，以及目标点到这三个点的距离，求目标点的坐标。

你想想看，如果只有1个基站，目标点只能确定在一个圆上。2个基站，两个圆相交，得到两个候选点。3个基站，三个圆交于一点，位置就唯一确定了。

核心公式：

假设基站坐标分别为 (x₁, y₁), (x₂, y₂), (x₃, y₃)，测距值分别为 d₁, d₂, d₃，目标点坐标为 (x, y)。

(x - x₁)² + (y - y₁)² = d₁²
(x - x₂)² + (y - y₂)² = d₂²
(x - x₃)² + (y - y₃)² = d₃²

解这个方程组，把后两个方程减去第一个，消去二次项，就得到一个线性方程组。然后直接求解即可。

我的经验：三边定位在理想环境下效果很好。但实际项目中，测距误差、基站位置误差、多径效应都会导致三个圆不交于一点。这时候，三边定位就失效了。

既然三边定位在现实中有局限，那怎么办？换个思路——我们不要求三个圆精确交于一点，而是找一个点，让它到三个基站的距离误差的平方和最小。

这就是最小二乘法的核心思想。

数学上，我们定义误差函数：

f(x, y) = Σ [√((x - xᵢ)² + (y - yᵢ)²) - dᵢ]²

然后求这个函数的最小值。通常的做法是线性化处理，把问题转化为求解一个超定线性方程组的最小二乘解。

具体步骤：

避坑指南：我曾经在一个项目中，直接用三边定位，结果定位点经常跳到基站外面去。后来换成最小二乘法，虽然计算量大了点，但稳定性好很多。记住，基站数量越多，最小二乘法的优势越明显。

下面这张图展示了三边定位与最小二乘法的核心区别：

前面讲的两种方法，都是基于单次测量做定位。但实际场景中，目标是在运动的。比如你拿着UWB标签在仓库里走，每次定位结果都会抖动。怎么办？

卡尔曼滤波就是干这个的。它把定位问题看作一个状态估计问题，结合运动模型和测量数据，给出最优估计。

核心思想就两句话：

卡尔曼滤波的五个核心公式：

我的习惯：刚开始用卡尔曼滤波时，别纠结公式推导。先把它当黑盒用，调好两个关键参数：过程噪声协方差 Q 和 测量噪声协方差 R。Q 越大，滤波器越相信测量值；R 越大，滤波器越相信预测值。

下面这张图展示了卡尔曼滤波在UWB定位中的工作流程：

说了这么多，到底该用哪个？我根据实际项目经验，整理了一个对比表：

我曾经踩过的坑：在一个AGV定位项目中，我直接用最小二乘法做实时定位，结果轨迹抖得像心电图。后来加上卡尔曼滤波，轨迹平滑了很多。但要注意，卡尔曼滤波的参数不是万能的——运动速度变了，Q和R就得重新调。

如果你刚开始做UWB定位，我的建议是：