4、UWB定位算法：最小二乘法、卡尔曼滤波、粒子滤波、Chan算法、Taylor算法
各位，这一章咱们来啃硬骨头——定位算法。说实话，UWB的硬件再好，天线再准，没有合适的算法，数据就是一堆乱码。我在项目里见过太多人，买了最好的模块，结果定位飘得跟喝醉了一样，最后发现是算法选错了。
UWB定位算法，说白了就是「用数学把距离换算成坐标」。你想想看，基站告诉你「标签离我5米」，但5米是个圆，三个圆交在一起才是点。怎么交？怎么让这个点更准？这就是算法的活。
我按工程实用度，把这五种算法分成两类：解析类和滤波类。咱们一个一个过。
核心观点：没有最好的算法，只有最合适的场景。静态场景用最小二乘法，动态场景用卡尔曼滤波，NLOS严重时上粒子滤波，基站布局好就用Chan+Taylor组合。
4.1 最小二乘法（LS）—— 最朴素的定位算法
最小二乘法，我习惯叫它「硬算」。它的思路特别直白：已知N个基站坐标和N个距离测量值，找一个点，让这个点到所有基站的理论距离和实测距离的误差平方和最小。
数学上就是解一个超定线性方程组。我给大家写个简化版的代码框架，实际项目中我经常这么用：
// 最小二乘法定位（C语言伪代码）
typedef struct {
double x, y;
} Point;
Point least_squares_localization(Point* anchors, double* ranges, int n) {
// n >= 3，基站数量
// 构造线性方程组 Ax = b
// 取第一个基站作为参考，做差分消除二次项
double A[(n-1) * 2];
double b[(n-1)];
for (int i = 1; i < n; i++) {
A[(i-1)*2 + 0] = 2 * (anchors[i].x - anchors[0].x);
A[(i-1)*2 + 1] = 2 * (anchors[i].y - anchors[0].y);
b[i-1] = ranges[0]*ranges[0] - ranges[i]*ranges[i]
+ anchors[i].x*anchors[i].x - anchors[0].x*anchors[0].x
+ anchors[i].y*anchors[i].y - anchors[0].y*anchors[0].y;
}
// 解最小二乘：x = (A^T A)^{-1} A^T b
// 实际工程用QR分解或SVD，别直接求逆
Point result;
// ... 矩阵运算代码
return result;
}我的经验：最小二乘法对测量噪声敏感。我在一个仓库项目里，直接用LS算出来位置跳了2米多，后来加了加权（WLS），给信号强的基站更高权重，立马稳到30厘米以内。加权矩阵一般取距离倒数的平方。
适用场景：静态标定、基站数量多且分布均匀、对实时性要求不高。
4.2 卡尔曼滤波（KF）—— 动态定位的标配
卡尔曼滤波，做定位的没人不知道。它不单独算位置，而是「预测+更新」两步走。你想想看，上一秒标签在A点，速度是v，下一秒大概率在A+v·Δt附近，然后拿UWB测量值去修正这个预测。
我刚开始用卡尔曼的时候，老觉得状态矩阵难调。后来总结了一个口诀：「状态要全，噪声要准，初值要稳」。
标准卡尔曼滤波只适用于线性高斯系统。UWB定位模型是非线性的（距离和坐标是平方关系），所以实际用的是扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。
// EKF 预测-更新流程（伪代码）
// 状态向量: [x, y, vx, vy]
// 观测向量: [d1, d2, ..., dn] (n个基站的距离)
void ekf_predict(State* s, double dt) {
// 状态预测：匀速模型
s->x += s->vx * dt;
s->y += s->vy * dt;
// 协方差预测：P = F * P * F^T + Q
// F是状态转移矩阵，Q是过程噪声协方差
}
void ekf_update(State* s, double* ranges, Point* anchors, int n) {
// 计算预测距离
double h[n];
for (int i = 0; i < n; i++) {
h[i] = sqrt(pow(s->x - anchors[i].x, 2) + pow(s->y - anchors[i].y, 2));
}
// 计算雅可比矩阵 H (n x 4)
// 卡尔曼增益: K = P * H^T * (H * P * H^T + R)^{-1}
// 状态更新: s += K * (z - h)
// 协方差更新: P = (I - K * H) * P
}避坑指南：我曾经在一个AGV小车项目里，卡尔曼滤波的Q矩阵设得太小，导致滤波器「过于自信」，测量值来了也不怎么更新，小车拐弯时定位滞后了半秒。后来把Q调大了一倍，跟踪就灵敏了。记住：Q控制平滑度，R控制信任度。
4.3 粒子滤波（PF）—— 暴力美学
粒子滤波，说白了就是「撒豆子」。在可能的位置撒几千个粒子，每个粒子带一个权重，然后根据测量值更新权重，最后加权平均得到位置。粒子多的区域就是最可能的位置。
为什么需要它？因为卡尔曼滤波假设噪声是高斯分布，但UWB在非视距（NLOS）环境下，误差分布可能是多峰的——比如信号穿墙后，测量值可能偏大2米，也可能偏大5米，没有固定规律。这时候卡尔曼就懵了，粒子滤波反而能扛住。
粒子滤波的步骤我总结为四步：
- 初始化：在初始位置附近撒N个粒子，权重均等
- 预测：每个粒子按运动模型随机移动（加一点噪声）
- 更新：根据UWB测量值，计算每个粒子的似然，更新权重
- 重采样：权重低的粒子淘汰，权重高的粒子复制，保持粒子总数不变
工程对比：粒子滤波精度高，但计算量大。我在一个室内定位项目里，用500个粒子，在STM32F4上跑，帧率只有10Hz。后来优化到200个粒子，配合重要性采样，帧率提到25Hz，精度损失不到5%。
4.4 Chan算法 —— 解析法的天花板
Chan算法，全名Chan氏算法，是TDOA（到达时间差）定位的经典解法。它分两步：第一步假设所有基站误差独立，得到一个初始解；第二步利用第一步的结果和基站几何关系，得到更优解。
Chan算法的好处是——一次计算，不需要迭代。在基站数量≥4且布局不太差的情况下，精度接近克拉美罗下界（CRLB）。
| 特性 | Chan算法 | 最小二乘法 |
|---|---|---|
| 输入类型 | TDOA（时间差） | TOA（绝对距离） |
| 计算方式 | 两步加权最小二乘 | 线性最小二乘 |
| 基站要求 | ≥4个 | ≥3个 |
| 噪声鲁棒性 | 较好（加权处理） | 一般 |
| 计算复杂度 | O(n) | O(n) |
我的习惯：Chan算法做初值，然后用Taylor算法迭代3-5次，精度能再提升一个量级。这个组合我在十几个项目里验证过，稳定可靠。
4.5 Taylor算法 —— 迭代求精
Taylor算法，也叫泰勒级数展开法。它的思路是：先猜一个初始位置，然后在这个位置附近做泰勒展开，忽略高阶项，得到一个线性方程组，解出位置修正量，然后迭代。
说白了就是「摸着石头过河」——每一步都往误差减小的方向走一小步，直到收敛。
Taylor算法的关键点有两个：
- 初值要准：初值离真实位置越近，收敛越快。我一般用Chan算法或最小二乘法给初值
- 迭代次数要控制：一般3-5次就够了，多了反而可能震荡。设置一个阈值，比如位置修正量小于1厘米就停止
// Taylor算法迭代流程
Point taylor_localization(Point init, Point* anchors, double* ranges, int n) {
Point pos = init;
double delta = 999;
int iter = 0;
while (delta > 0.01 && iter < 10) {
// 计算雅可比矩阵 G (n x 2)
// 计算残差向量 w (n x 1): 实测距离 - 预测距离
// 解: [dx, dy]^T = (G^T * Q^{-1} * G)^{-1} * G^T * Q^{-1} * w
// Q是测量噪声协方差矩阵
pos.x += dx;
pos.y += dy;
delta = sqrt(dx*dx + dy*dy);
iter++;
}
return pos;
}避坑指南：我曾经在基站布局呈直线（共线）的情况下用Taylor算法，结果雅可比矩阵奇异，直接算崩了。后来加了正则化项（Levenberg-Marquardt修正），才稳住。记住：基站几何布局不好时，任何解析算法都会出问题。
4.6 算法选型对照表
最后，我给大家整理了一张选型表。项目里怎么选，看这张表就够了：
| 场景 | 推荐算法 | 理由 |
|---|---|---|
| 静态标定/部署调试 | 最小二乘法 | 简单、快速、不需要初值 |
| 动态跟踪（LOS环境） | 扩展卡尔曼滤波 | 实时性好、平滑、适合嵌入式 |
| 强NLOS/多径环境 | 粒子滤波 | 能处理非高斯、多模态噪声 |
| 基站≥4、布局规则 | Chan + Taylor | 精度高、计算快、不需要滤波 |
| 资源受限MCU | 最小二乘法 或 Chan | 无迭代、内存占用小 |
嗯，这一章的内容就到这儿。五种算法，各有各的脾气。你只要记住：解析算法求快，滤波算法求稳，组合算法求准。项目里多试试，找到最适合你场景的那一款。