移动场景卡尔曼滤波详解

1. 核心思想

静态场景下只有一个时间点的好心人报告来估计位置。移动场景中，丢失设备持续移动，不同时刻有不同好心人靠近并上报。

卡尔曼滤波用物理运动模型把不同时刻的观测"串"起来，让信息跨时间累积。

2. 两步循环

时刻 k-1 的估计 x̂(k-1)
        │
        ▼
  ┌─────────────┐
  │  预测 (Predict) │  用运动模型推算：设备应该在哪？
  │  x̂⁻(k) = F·x̂(k-1)  │  不确定性增大（P⁻变大）
  └─────────────┘
        │
        ▼
  ┌─────────────┐
  │  更新 (Update)  │  收到好心人报告：修正预测
  │  x̂(k) = x̂⁻(k) + K·(z - h(x̂⁻))  │  不确定性缩小
  └─────────────┘
        │
        ▼
  时刻 k 的估计 x̂(k)

3. 状态向量

$$\mathbf{x} = \begin{bmatrix} x \ y \ v_x \ v_y \end{bmatrix}$$

位置 + 速度。有了速度，即使某一帧没有好心人报告，预测步仍能推算设备当前位置。

4. 预测步（运动模型）

假设匀速运动（constant velocity）：

$$\mathbf{x}^-(k) = F \cdot \mathbf{x}(k-1) + \mathbf{w}$$

$$F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \ 0 & 1 & 0 & \Delta t \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}$$

其中 $\mathbf{w} \sim \mathcal{N}(0, Q)$ 是过程噪声，表示运动不确定性（加速、转弯）。

Q 的大小决定"相信运动模型"的程度：

• Q 小 → 强信匀速，轨迹平滑，对突然转弯响应慢
• Q 大 → 不太信运动模型，更依赖观测，轨迹更抖

5. 更新步（观测模型）

收到好心人 $i$ 的报告时，观测方程：

$$z_i = f(r_i) = |x - I_i|_2 + \epsilon_i$$

这是非线性的（欧氏距离），需要用 EKF 或 UKF：

方法	处理非线性的方式	优缺点
EKF	对 h(x) 在当前估计点求雅可比矩阵线性化	简单高效，但线性化误差在强非线性时可能发散
UKF	用 sigma 点采样传播不确定性	无需求导，精度更高，计算量稍大

6. 为什么能"少变多"（多帧累积）

示例场景：

t=0s:  好心人 A 报告（设备北方 20m）  → 只知道大致方向
t=10s: 无人报告                      → 预测步：按估计速度推算位置
t=20s: 好心人 B 报告（设备东方 15m）  → 结合 A 的历史信息 + 运动推算 + B 的新观测
t=30s: 好心人 C 报告（设备南方 10m）  → 三个方向累积，几何精度大幅提升

没有卡尔曼滤波时，t=20s 只有好心人 B 一个报告（一个圆，无法定位）。有卡尔曼滤波后，t=0s 好心人 A 的信息通过运动模型传递到 t=20s，等效于同时有两个观测。

时间上分散的稀疏观测，通过运动模型在时间维度上聚合，等效于单帧有更多好心人。

即使每帧只有 1 个好心人，运动模型合理时，10 帧后的累积信息量可等效于单帧 5–8 个好心人。

7. 与静态场景 IRLS 的关系

静态场景：  IRLS 鲁棒优化（一次性，所有报告同时处理）
                    ↓
移动场景：  每个时间步先 IRLS 求当前帧的位置估计
            → 作为卡尔曼滤波的观测 z(k) 输入
            → 卡尔曼滤波融合历史信息输出最终轨迹

或更紧密的融合：直接在 EKF 更新步中使用鲁棒核函数，将 IRLS 和卡尔曼滤波合为一体。

8. 实际参数选择

参数	典型值	依据
Δt	1–10 s	好心人报告间隔
过程噪声 σ_a（加速度标准差）	0.5–2 m/s²（行人），1–5 m/s²（车辆）	运动模式
观测噪声 σ_z	由 RSSI 方差 + GPS accuracy 联合确定	见痛点一、二

9. EKF vs UKF 选择建议

对 Problem 1：

• RSSI-距离关系是对数（温和非线性）
• 多个好心人距离到位置的映射可能有多个局部极小值
• UKF 更稳健，推荐优先尝试
• EKF 作为计算量更低的备选方案