4、融合定位架构：松耦合与紧耦合架构、数据融合层级、卡尔曼滤波基础
各位做定位的朋友，咱们今天聊点实在的。UWB 和 BLE 各有各的脾气——UWB 精度高但容易受遮挡，BLE 覆盖广但稳定性差。把它们捏在一起，就是融合定位的核心思路。
但怎么捏？这里头有讲究。我最早做融合时，直接把两个定位结果求平均，结果精度还不如单用 UWB。后来才明白，融合架构选错了，后面全白搭。
4.1 松耦合 vs 紧耦合：两种融合思路
说白了，松耦合和紧耦合的区别，就是「各自算完再合」还是「原始数据就合」。我画了张图，你看一眼就明白。
核心区别一句话：松耦合融合的是定位结果，紧耦合融合的是原始观测数据。
4.1.1 松耦合架构
松耦合，就是 UWB 和 BLE 各自独立解算位置，然后把两个位置结果扔给一个融合模块做决策。我早期做的一个仓储项目用的就是这种方案。
优点很明显：
- 模块独立，UWB 和 BLE 可以分别调试、分别升级
- 计算量小，嵌入式 MCU 跑得动
- 系统鲁棒性好——一个挂了另一个还能用
缺点也扎心：
- 精度上限低，因为融合的是「结果」而不是「原始信息」
- 无法利用 UWB 的精确测距去校正 BLE 的 RSSI 波动
我的经验：松耦合适合对实时性要求高、MCU 资源紧张的场景。比如手持终端定位，每秒要出 10 次位置，松耦合轻松搞定。
4.1.2 紧耦合架构
紧耦合就狠多了。它把 UWB 的原始测距值（TOF）和 BLE 的 RSSI 观测值直接塞进同一个滤波器。状态向量里同时包含位置、速度、甚至时钟偏置。
我曾经在一个隧道定位项目里试过紧耦合。隧道里 UWB 信号经常被金属支架遮挡，但 BLE 信号反而稳定。紧耦合滤波器能自动利用 BLE 的 RSSI 变化趋势来「撑住」UWB 丢帧时的位置估计——这个能力松耦合做不到。
紧耦合的代价：
- 状态维度高，计算量大——STM32F4 级别勉强够用
- 调试复杂，任何一个传感器的噪声模型不准，整个滤波器可能发散
- 代码耦合度高，改一个传感器逻辑可能影响全局
| 对比维度 | 松耦合 | 紧耦合 |
|---|---|---|
| 融合层级 | 位置级 | 观测级 |
| 精度上限 | 中等（约 30-50cm） | 高（约 10-20cm） |
| 计算开销 | 低 | 高 |
| 调试难度 | 低 | 高 |
| 典型 MCU | STM32F1/F4 | STM32H7/RT1052 |
4.2 数据融合层级：你在哪一层融合？
除了松紧耦合，还有个维度——数据融合的层级。我习惯把它分成三层：
- 数据级融合：直接对原始采样数据做处理。比如把 UWB 的 TOF 值和 BLE 的 RSSI 值做归一化后加权。这层最底层，信息损失最少，但数据量巨大。
- 特征级融合：先提取特征再融合。比如从 UWB 测距序列中提取「视距/非视距」特征，从 BLE 序列中提取「信号稳定性」特征，然后融合这些特征做定位决策。
- 决策级融合：每个传感器独立做出定位决策，最后用投票或加权方式决定最终位置。松耦合本质上就是决策级融合。
避坑提醒：我曾经在一个项目中试图做数据级融合，把 UWB 和 BLE 的原始数据直接拼接成向量输入神经网络。结果数据量太大，MCU 内存直接爆了。后来老老实实降到特征级融合才跑通。
4.3 卡尔曼滤波基础：融合的数学引擎
聊融合架构，绕不开卡尔曼滤波。它几乎是所有定位融合算法的基石。我尽量用白话讲清楚。
4.3.1 卡尔曼滤波在干什么？
你想想看，你有一个系统状态（比如位置和速度），但你没法直接测量它。你只能通过传感器得到一些带噪声的观测值。卡尔曼滤波就是帮你从这些「脏数据」里，递归地估计出最可能的状态。
它分两步走：
- 预测（Predict）：根据上一时刻的状态和运动模型，猜一下当前状态大概在哪
- 更新（Update）：用当前的传感器观测值，修正这个猜测
说白了，就是「猜一下，量一下，修正一下」的循环。
4.3.2 核心公式（别怕，就五个）
卡尔曼滤波的核心就五个公式。我当年第一次看时也觉得头大，但实际写代码时发现，就是矩阵乘法和加减法。
// 预测步骤
x_pred = A * x_prev + B * u // 状态预测
P_pred = A * P_prev * A^T + Q // 协方差预测
// 更新步骤
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1) // 卡尔曼增益
x_est = x_pred + K * (z - H * x_pred) // 状态更新
P_est = (I - K * H) * P_pred // 协方差更新这里每个符号都有物理意义：
| 符号 | 含义 | 在融合定位中的例子 |
|---|---|---|
| x | 状态向量 | [位置x, 位置y, 速度vx, 速度vy] |
| A | 状态转移矩阵 | 匀速运动模型：x_k = x_{k-1} + v*dt |
| z | 观测向量 | UWB 测距值、BLE RSSI 值 |
| H | 观测矩阵 | 把状态映射到观测空间 |
| Q | 过程噪声协方差 | 运动模型的不确定性 |
| R | 观测噪声协方差 | 传感器的噪声水平 |
| K | 卡尔曼增益 | 相信模型还是相信传感器 |
关键理解：卡尔曼增益 K 决定了「我更相信预测模型」还是「我更相信传感器」。如果传感器噪声大（R 大），K 就小，滤波器更依赖模型预测。反之亦然。
4.3.3 在 UWB+BLE 融合中的实际用法
我拿一个实际项目举例。当时做室内人员定位，状态向量取 4 维：[x, y, vx, vy]。UWB 提供的是距离观测（TOF 转成距离），BLE 提供的是 RSSI 观测。
观测矩阵 H 的设计很有意思：
- 对于 UWB，H 是距离对位置的偏导数——也就是从状态空间到测距空间的雅可比矩阵
- 对于 BLE，H 是 RSSI 路径损耗模型对位置的偏导数
嗯，这里要注意：UWB 和 BLE 的观测更新频率不同。UWB 通常 10-50Hz，BLE 只有 1-5Hz。所以实际代码里，我通常把预测步骤跑在 UWB 的更新周期上，BLE 的观测到了才做一次更新。
// 伪代码：UWB + BLE 融合卡尔曼滤波主循环
while (1) {
// 1. 预测步骤（每个 UWB 周期执行）
x = A * x;
P = A * P * A^T + Q;
// 2. 如果有 UWB 测距到达
if (uwb_data_ready()) {
z_uwb = get_uwb_range();
H_uwb = compute_jacobian_uwb(x); // 距离对位置的雅可比
K = P * H_uwb^T * inv(H_uwb * P * H_uwb^T + R_uwb);
x = x + K * (z_uwb - h_uwb(x));
P = (I - K * H_uwb) * P;
}
// 3. 如果有 BLE RSSI 到达（频率较低）
if (ble_data_ready()) {
z_ble = get_ble_rssi();
H_ble = compute_jacobian_ble(x); // RSSI 对位置的雅可比
K = P * H_ble^T * inv(H_ble * P * H_ble^T + R_ble);
x = x + K * (z_ble - h_ble(x));
P = (I - K * H_ble) * P;
}
// 4. 输出当前最优位置估计
output_position(x[0], x[1]);
}调参经验：Q 和 R 的比值决定了滤波器的「惯性」。我习惯先让系统在纯 UWB 下跑通，调好 Q，再加入 BLE 观测，单独调 R_ble。两个一起调容易找不到问题根源。
4.3.4 扩展卡尔曼滤波（EKF）的必要性
标准的卡尔曼滤波假设系统是线性的。但 UWB 的测距方程是非线性的——距离和位置的关系是平方根关系。BLE 的 RSSI 路径损耗模型更是典型的对数非线性。
所以实际项目中，我几乎不用标准 KF，而是用 EKF（扩展卡尔曼滤波）。EKF 的核心思想就是：在每次预测和更新时，对非线性函数做一阶泰勒展开，用雅可比矩阵代替线性 KF 中的 A 和 H。
说白了，就是「把弯的掰直了再算」。
我曾经踩过的坑：EKF 的雅可比矩阵如果算错了，滤波器会直接发散。有一次我在计算 UWB 测距的雅可比时，把偏导数符号搞反了，结果位置估计直接往反方向跑。调试了整整两天才发现是符号问题。所以建议你写 EKF 时，先用数值微分验证一下雅可比矩阵的正确性。
4.4 架构选型建议
说了这么多，到底怎么选？我个人的经验是：
- 产品原型阶段：先用松耦合快速验证。把 UWB 和 BLE 各自的位置结果用加权平均或简单切换融合，先跑通流程。
- 性能优化阶段：如果精度不达标，再上紧耦合 EKF。这时候你已经有了 UWB 和 BLE 的噪声特性数据，调参会快很多。
- 量产阶段：根据 MCU 资源做取舍。如果用的是 Cortex-M4 以下的内核，建议用松耦合+简单滤波；如果是 M7 或 A 系列芯片，紧耦合完全扛得住。
融合定位架构没有银弹。松耦合简单可靠，紧耦合精度更高但调试成本大。选哪个，取决于你的项目阶段和硬件资源。嗯，先跑起来，再优化，这是我一直坚持的原则。