第6章：卡尔曼滤波与随机控制

本章概述

在实际控制系统中，不确定性无处不在——传感器噪声、过程干扰、模型误差等都会影响系统性能。本章介绍处理随机系统的核心理论与方法，重点讲解卡尔曼滤波器的设计原理及其在状态估计中的应用，以及如何将估计与控制结合形成完整的随机控制系统。我们还将探讨后向随机微分方程（BSDE）这一前沿工具在控制中的应用。通过GPS/INS组合导航系统案例，展示这些理论在实际工程中的威力。

学习目标：

• 掌握随机过程的基本概念和在控制系统中的建模方法
• 理解卡尔曼滤波器的推导过程和最优性质
• 学会设计和调试扩展卡尔曼滤波器（EKF）
• 掌握线性二次高斯（LQG）控制器设计
• 了解BSDE在随机控制中的应用
• 通过实际案例理解随机控制系统的工程实现

6.1 随机过程基础

6.1.1 随机系统建模

在确定性系统基础上，我们引入随机性来更准确地描述实际系统：

连续时间随机系统： $$\dot{x}(t) = f(x(t), u(t), t) + G(t)w(t)$$ $$y(t) = h(x(t), t) + v(t)$$ 其中：

• $w(t)$ 是过程噪声（如风扰、振动）
• $v(t)$ 是测量噪声（如传感器误差）
• $G(t)$ 是噪声输入矩阵

离散时间随机系统（更常用于数字控制）： $$x_{k+1} = f(x_k, u_k) + w_k$$ $$y_k = h(x_k) + v_k$$

6.1.2 噪声的统计特性

白噪声假设：

• 过程噪声：$w_k \sim \mathcal{N}(0, Q)$，协方差矩阵 $Q$
• 测量噪声：$v_k \sim \mathcal{N}(0, R)$，协方差矩阵 $R$
• 独立性：$E[w_i v_j^T] = 0$ 对所有 $i, j$

功率谱密度： 连续白噪声的功率谱密度是常数，这在实际中是理想化的，但在离散采样下是合理近似。

功率谱密度图示：
    |
PSD |━━━━━━━━━━━━━━  白噪声（理想）
    |
    |   ╱╲
    |  ╱  ╲           有色噪声（实际）
    |_╱____╲_________
     0     频率

6.1.3 随机状态的传播

均值传播： 对于线性系统 $x_{k+1} = Ax_k + Bu_k + w_k$： $$\mu_{k+1} = A\mu_k + Bu_k$$ 协方差传播： $$P_{k+1} = AP_kA^T + Q$$ 这描述了不确定性如何随时间演化——即使初始状态确定（$P_0 = 0$），过程噪声会导致不确定性增长。

6.2 卡尔曼滤波器设计

6.2.1 问题描述

给定带噪声的测量，如何最优地估计系统状态？卡尔曼滤波器提供了线性系统下的最优解（最小均方误差意义下）。

系统模型： $$x_{k+1} = A_kx_k + B_ku_k + w_k$$ $$y_k = C_kx_k + v_k$$ 目标：找到状态估计 $\hat{x}_k$，使得估计误差协方差 $P_k = E[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^T]$ 最小。

6.2.2 卡尔曼滤波递推公式

卡尔曼滤波包含两个步骤：预测和更新。

预测步骤（时间更新）： $$\hat{x}_{k+1|k} = A_k\hat{x}_{k|k} + B_ku_k$$ $$P_{k+1|k} = A_kP_{k|k}A_k^T + Q_k$$ 更新步骤（测量更新）： $$K_{k+1} = P_{k+1|k}C_{k+1}^T(C_{k+1}P_{k+1|k}C_{k+1}^T + R_{k+1})^{-1}$$ $$\hat{x}_{k+1|k+1} = \hat{x}_{k+1|k} + K_{k+1}(y_{k+1} - C_{k+1}\hat{x}_{k+1|k})$$ $$P_{k+1|k+1} = (I - K_{k+1}C_{k+1})P_{k+1|k}$$ 其中 $K_{k+1}$ 是卡尔曼增益，它平衡了预测和测量的可信度。

6.2.3 卡尔曼增益的直观理解

卡尔曼增益的作用：

测量噪声小 (R↓)  →  K↑  →  更相信测量
过程噪声小 (Q↓)  →  K↓  →  更相信预测

极限情况：

• 当 $R \to 0$（完美测量）：$K \to C^{-1}$，完全相信测量
• 当 $R \to \infty$（测量无用）：$K \to 0$，完全相信预测
• 当 $Q \to 0$（完美模型）：$P$ 收敛到零，估计变得确定

6.2.4 最优性证明要点

卡尔曼滤波器是线性最小方差无偏估计器（LMMSE）：

1. 线性：估计器形式为 $\hat{x} = Ky + b$
2. 无偏：$E[\hat{x}] = E[x]$
3. 最小方差：在所有线性无偏估计器中，协方差矩阵最小

对于高斯噪声，卡尔曼滤波器也是所有估计器（包括非线性）中的最优解。

6.3 扩展卡尔曼滤波（EKF）

6.3.1 非线性系统的处理

实际系统往往是非线性的： $$x_{k+1} = f(x_k, u_k) + w_k$$ $$y_k = h(x_k) + v_k$$ EKF通过局部线性化来应用卡尔曼滤波框架。

6.3.2 EKF算法

预测步骤： $$\hat{x}_{k+1|k} = f(\hat{x}_{k|k}, u_k)$$ $$P_{k+1|k} = F_kP_{k|k}F_k^T + Q_k$$ 其中 $F_k = \frac{\partial f}{\partial x}\bigg|_{x=\hat{x}_{k|k}}$ 是状态转移雅可比矩阵。

更新步骤： $$K_{k+1} = P_{k+1|k}H_{k+1}^T(H_{k+1}P_{k+1|k}H_{k+1}^T + R_{k+1})^{-1}$$ $$\hat{x}_{k+1|k+1} = \hat{x}_{k+1|k} + K_{k+1}(y_{k+1} - h(\hat{x}_{k+1|k}))$$ $$P_{k+1|k+1} = (I - K_{k+1}H_{k+1})P_{k+1|k}$$ 其中 $H_{k+1} = \frac{\partial h}{\partial x}\bigg|_{x=\hat{x}_{k+1|k}}$ 是测量雅可比矩阵。

6.3.3 EKF的局限性

1. 线性化误差：当非线性很强时，一阶泰勒展开不够准确
2. 雅可比计算：需要解析导数或数值微分，计算量大
3. 发散风险：初始误差大或模型不准确时可能发散
4. 非高斯分布：非线性变换后，状态分布不再是高斯的

线性化误差示意：
    真实轨迹
      ╱
     ╱ ← 线性化误差
    ╱
   ╱━━━ EKF估计
  ╱
 ╱ 线性化点

6.3.4 改进方法

无迹卡尔曼滤波（UKF）： 使用确定性采样点（sigma点）来捕获均值和协方差，避免显式计算雅可比矩阵：

• 选择2n+1个sigma点（n是状态维度）
• 通过非线性函数传播这些点
• 从传播后的点重构均值和协方差

迭代EKF（IEKF）： 在更新步骤中多次迭代，每次在新的线性化点重新计算雅可比矩阵，提高精度。

6.4 线性二次高斯（LQG）控制

6.4.1 分离原理

LQG控制的核心是分离原理：最优控制器可以分解为：

1. 卡尔曼滤波器：基于测量估计状态
2. LQR控制器：基于估计状态计算控制输入

这种分离在线性高斯系统下是最优的。

6.4.2 LQG问题设置

系统模型： $$x_{k+1} = Ax_k + Bu_k + w_k, \quad w_k \sim \mathcal{N}(0, Q_w)$$ $$y_k = Cx_k + v_k, \quad v_k \sim \mathcal{N}(0, R_v)$$ 代价函数： $$J = E\left[\sum_{k=0}^{N-1}(x_k^TQ_xx_k + u_k^TR_uu_k) + x_N^TQ_fx_N\right]$$

6.4.3 LQG控制器设计

步骤1：设计卡尔曼滤波器估计状态 $\hat{x}_k$

步骤2：解确定性LQR问题，得到反馈增益 $K$

步骤3：控制律为： $$u_k = -K\hat{x}_k$$ 完整的控制系统结构：

     ┌─────────────────────────────────┐
     │                                 │
w_k →┤  真实系统  x_{k+1} = Ax_k + Bu_k + w_k │
     │                                 │
     └────────┬────────────────────────┘
              │ y_k = Cx_k + v_k
              ↓
     ┌─────────────────────────────────┐
     │   卡尔曼滤波器                    │
     │   估计状态 \hat{x}_k              │
     └────────┬────────────────────────┘
              │ \hat{x}_k
              ↓
     ┌─────────────────────────────────┐
     │   LQR控制器                      │
     │   u_k = -K\hat{x}_k              │
     └────────┬────────────────────────┘
              │ u_k
              └──────────────┐
                            ↓
                          系统输入

6.4.4 LQG的鲁棒性问题

虽然LQG在理论上是最优的，但它缺乏鲁棒性保证：

• LQR有至少60°相位裕度和无穷增益裕度
• LQG可能有任意小的稳定裕度

这就是为什么实践中常需要加入鲁棒性考虑，发展出H∞控制等理论。

6.5 后向随机微分方程（BSDE）在控制中的应用

6.5.1 BSDE基本概念

经典的随机微分方程（前向SDE）： $$dx_t = f(t, x_t)dt + \sigma(t, x_t)dW_t, \quad x_0 = x$$ 后向随机微分方程（BSDE）： $$dY_t = -f(t, Y_t, Z_t)dt + Z_tdW_t, \quad Y_T = \xi$$ 其中终端条件 $\xi$ 给定，需要求解 $(Y_t, Z_t)$ 过程。

6.5.2 BSDE与随机控制的联系

随机最优控制问题： 最小化代价函数 $$J(t, x) = E\left[\int_t^T L(s, X_s, u_s)ds + g(X_T) \bigg| X_t = x\right]$$ 对应的BSDE： 值函数 $V(t, x)$ 满足 $$Y_t = V(t, X_t)$$ $$Z_t = \sigma^T(t, X_t)\nabla_xV(t, X_t)$$ 这提供了求解随机控制问题的新途径。

6.5.3 线性BSDE与随机Riccati方程

对于LQG问题，相应的BSDE是线性的： $$dY_t = -(A^TY_t + Y_tA + Q - Y_tBR^{-1}B^TY_t)dt + Z_tdW_t$$ 这等价于随机Riccati方程，但BSDE形式在某些情况下更便于数值求解。

6.5.4 BSDE的数值方法

时间离散化： 将 $[0, T]$ 分成 $N$ 个区间，在每个区间上： $$Y_{t_i} = E[Y_{t_{i+1}}|F_{t_i}] + \Delta t \cdot f(t_i, Y_{t_i}, Z_{t_i})$$ 最小二乘蒙特卡洛方法：

1. 生成前向SDE的样本路径
2. 从终端条件反向递推
3. 用基函数逼近条件期望

6.6 案例研究：GPS/INS组合导航系统

6.6.1 系统背景

GPS（全球定位系统）和INS（惯性导航系统）是现代导航的两大支柱技术：

GPS特点：

• 优点：长期精度高，误差不累积
• 缺点：更新率低（1-10 Hz），易受干扰，城市峡谷效应

INS特点：

• 优点：更新率高（100-1000 Hz），完全自主，短期精度高
• 缺点：误差随时间累积，需要初始对准

组合导航通过卡尔曼滤波融合两者优势，是现代飞机、导弹、自动驾驶车辆的标准配置。

6.6.2 系统建模

状态向量（15维）： $$x = [r^T, v^T, \psi^T, b_a^T, b_g^T]^T$$ 其中：

• $r = [x, y, z]^T$：位置
• $v = [v_x, v_y, v_z]^T$：速度
• $\psi = [\phi, \theta, \psi]^T$：姿态角（滚转、俯仰、偏航）
• $b_a$：加速度计偏差
• $b_g$：陀螺仪偏差

系统动力学： $$\dot{r} = v$$ $$\dot{v} = C_b^n(a_m - b_a - n_a) + g$$ $$\dot{\psi} = \Omega(\omega_m - b_g - n_g)$$ $$\dot{b}_a = -\frac{1}{\tau_a}b_a + w_a$$ $$\dot{b}_g = -\frac{1}{\tau_g}b_g + w_g$$ 其中：

• $C_b^n$：从机体坐标系到导航坐标系的转换矩阵
• $a_m, \omega_m$：IMU测量值
• $n_a, n_g$：测量噪声
• $\tau_a, \tau_g$：偏差相关时间常数

6.6.3 测量模型

GPS测量： $$y_{GPS} = \begin{bmatrix} r \\ v \end{bmatrix} + v_{GPS}$$ 磁力计测量（用于航向校正）： $$y_{mag} = C_n^b \cdot m_{ref} + v_{mag}$$ 其中 $m_{ref}$ 是地磁场参考向量。

6.6.4 EKF实现细节

误差状态EKF： 不直接估计状态，而是估计误差状态 $\delta x$： $$\delta x = [δr^T, δv^T, δ\psi^T, δb_a^T, δb_g^T]^T$$ 这样可以：

1. 保持小信号线性化假设
2. 简化四元数姿态表示
3. 提高数值稳定性

预测步骤（IMU更新，高频）：

每个IMU样本（200Hz）：

1. 积分运动方程更新名义状态
2. 传播误差状态协方差
3. 考虑IMU噪声增加不确定性

更新步骤（GPS更新，低频）：

每个GPS样本（1Hz）：

1. 计算创新（测量残差）
2. 检查创新是否合理（野值剔除）
3. 更新误差状态估计
4. 修正名义状态
5. 重置误差状态

6.6.5 实际工程考虑

时间同步： GPS和IMU的时间戳必须精确对齐，1ms的误差在高速运动时可导致米级定位误差。

野值检测： 使用马氏距离检验GPS测量： $$d^2 = (y - \hat{y})^T S^{-1} (y - \hat{y})$$ 其中 $S = HPH^T + R$ 是创新协方差。若 $d^2 > \chi^2_{threshold}$，拒绝该测量。

自适应滤波： 根据创新序列在线调整噪声参数： $$\hat{R}_k = \alpha \hat{R}_{k-1} + (1-\alpha)(e_ke_k^T - HP_{k|k-1}H^T)$$ 多路径效应处理： 城市环境中GPS信号反射严重，可以：

1. 使用载噪比（C/N0）加权
2. 几何精度因子（GDOP）筛选
3. 多假设跟踪处理多路径

6.6.6 性能指标

典型的GPS/INS组合导航系统性能：

| 指标 | GPS独立 | INS独立(1分钟) | 组合系统 |

6.6.7 实际代码框架

GPS/INS融合系统架构：

┌─────────────┐     ┌─────────────┐
│   IMU传感器  │     │  GPS接收机   │
│  (200 Hz)   │     │   (1 Hz)    │
└──────┬──────┘     └──────┬──────┘
       │                   │
       ↓                   ↓
┌─────────────────────────────────┐
│        时间同步与对准             │
└──────┬──────────────────┬───────┘
       │                  │
       ↓                  ↓
┌──────────────┐   ┌──────────────┐
│  IMU预处理    │   │  GPS预处理    │
│  -温度补偿    │   │  -野值剔除    │
│  -标定修正    │   │  -多路径检测  │
└──────┬───────┘   └──────┬───────┘
       │                  │
       ↓                  ↓
┌─────────────────────────────────┐
│         误差状态EKF              │
│  - 预测（IMU驱动）               │
│  - 更新（GPS触发）               │
│  - 零速检测（ZUPT）              │
└──────┬──────────────────────────┘
       │
       ↓
┌─────────────────────────────────┐
│      导航解输出                  │
│  - 位置、速度、姿态              │
│  - 不确定性估计                  │
└─────────────────────────────────┘

6.7 历史人物：Rudolf Kalman (1930-2016)

Rudolf Emil Kálmán 是现代控制理论的奠基人之一。1960年，他发表了具有里程碑意义的论文"A New Approach to Linear Filtering and Prediction Problems"，提出了卡尔曼滤波器。

主要贡献：

1. 卡尔曼滤波器（1960）：革命性的状态估计方法
2. 状态空间理论：可控性、可观性概念的提出
3. 线性二次调节器：与Bucy合作发展LQG理论

Apollo计划的关键技术： 卡尔曼滤波器是Apollo登月计划导航系统的核心。当时MIT的工程师最初对这个"过于理论化"的方法持怀疑态度，但Stanley Schmidt首先认识到其价值，开发了"Apollo滤波器"——实际上就是卡尔曼滤波器的实现。这使得Apollo飞船能够精确导航38万公里到达月球，误差仅几公里。

轶事：

• Kalman访问NASA时，工程师问他如何调试滤波器。他回答："如果不工作，说明你的模型是错的。"这句话虽然简单，却道出了模型准确性对滤波器性能的决定性影响。
• 他强调："The purpose of computing is insight, not numbers"——这成为现代计算科学的名言。

6.8 前沿专题：粒子滤波与高斯过程在控制中的应用

6.8.1 粒子滤波（Particle Filter）

当系统强非线性或噪声非高斯时，EKF和UKF可能失效。粒子滤波通过蒙特卡洛方法直接逼近后验概率分布。

基本思想： 用一组带权重的粒子 $\{x_i^k, w_i^k\}_{i=1}^N$ 表示概率分布： $$p(x_k|y_{1:k}) \approx \sum_{i=1}^N w_i^k \delta(x_k - x_i^k)$$ 序贯重要性采样（SIS）：

1. 采样：$x_i^k \sim q(x_k|x_i^{k-1}, y_k)$
2. 更新权重：$w_i^k \propto w_i^{k-1} \frac{p(y_k|x_i^k)p(x_i^k|x_i^{k-1})}{q(x_i^k|x_i^{k-1}, y_k)}$
3. 归一化：$w_i^k = w_i^k / \sum_j w_j^k$

重采样： 解决权重退化问题，当有效粒子数 $N_{eff} = 1/\sum_i (w_i^k)^2$ 过小时触发。

在控制中的应用：

• SLAM（同时定位与地图构建）
• 目标跟踪与多假设处理
• 故障检测与诊断

6.8.2 高斯过程（Gaussian Process）

高斯过程提供了一种非参数的函数逼近方法，特别适合建模未知动力学。

GP动力学模型： $$x_{k+1} = f(x_k, u_k) + w_k$$ 其中 $f \sim GP(m(\cdot), k(\cdot, \cdot))$，均值函数 $m$ 和协方差函数 $k$。

GP-MPC框架：

1. 从数据学习GP模型
2. 传播不确定性through GP
3. 考虑模型不确定性的鲁棒MPC

优势：

• 自动量化模型不确定性
• 在线学习和适应
• 理论保证（如PAC-Bayes界）

6.8.3 深度卡尔曼滤波

结合深度学习和卡尔曼滤波：

可微分卡尔曼滤波： 将卡尔曼滤波嵌入神经网络，端到端学习：

• 系统矩阵 $A, B, C$ 由神经网络参数化
• 噪声协方差 $Q, R$ 可学习
• 通过反向传播优化

应用案例：

• 视觉惯性里程计（VIO）
• 从像素直接估计状态
• 多模态传感器融合

6.9 本章小结

本章系统介绍了随机控制系统的核心理论和方法：

核心概念：

1. 随机系统建模：过程噪声和测量噪声的统计描述
2. 卡尔曼滤波：线性系统最优状态估计，预测-更新递推结构
3. 扩展卡尔曼滤波：通过线性化处理非线性系统
4. LQG控制：分离原理，估计与控制的最优结合
5. BSDE：随机控制的现代数学工具

关键公式汇总：

卡尔曼滤波递推：

• 预测：$\hat{x}_{k+1|k} = A\hat{x}_{k|k} + Bu_k$
• 卡尔曼增益：$K = PC^T(CPC^T + R)^{-1}$
• 更新：$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K(y_k - C\hat{x}_{k|k-1})$

LQG控制律：

• $u_k = -K_{LQR}\hat{x}_k$，其中 $\hat{x}_k$ 来自卡尔曼滤波器

实践要点：

• 模型准确性决定滤波器性能
• 噪声统计参数需要仔细调试
• 计算效率与精度需要平衡
• 鲁棒性考虑不可忽视

6.10 练习题

基础题

习题6.1 一维卡尔曼滤波器 考虑标量系统： $$x_{k+1} = ax_k + w_k, \quad w_k \sim \mathcal{N}(0, q)$$ $$y_k = x_k + v_k, \quad v_k \sim \mathcal{N}(0, r)$$ 推导卡尔曼增益的稳态值，并分析 $q/r$ 比值对滤波器行为的影响。

Hint: 稳态时 $P_{k+1|k} = P_{k|k}$，求解代数Riccati方程。

习题6.2 观测器与卡尔曼滤波器 证明：当过程噪声和测量噪声都为零时，卡尔曼滤波器退化为确定性观测器。

Hint: 考虑 $Q = 0, R = 0$ 的极限情况。

习题6.3 EKF线性化点选择 某非线性系统 $x_{k+1} = x_k^2 + u_k$。比较以下两种EKF实现的差异： a) 在预测值 $\hat{x}_{k+1|k}$ 处线性化 b) 在更新值 $\hat{x}_{k|k}$ 处线性化

Hint: 计算雅可比矩阵 $F = 2x$，分析线性化误差。

挑战题

习题6.4 多传感器融合 三个传感器测量同一个二维位置，测量噪声协方差分别为： $$R_1 = \begin{bmatrix} 1 & 0 \\ 0 & 4 \end{bmatrix}, R_2 = \begin{bmatrix} 4 & 0 \\ 0 & 1 \end{bmatrix}, R_3 = \begin{bmatrix} 2 & 1 \\ 1 & 2 \end{bmatrix}$$ 设计最优融合算法，使得估计误差协方差最小。

Hint: 考虑加权最小二乘或信息滤波形式。

习题6.5 自适应卡尔曼滤波 设计一个自适应算法，在线估计未知的测量噪声协方差 $R$。

Hint: 使用创新序列的统计特性。

习题6.6 BSDE与HJB方程（开放题） 考虑随机LQR问题，其值函数满足随机HJB方程。说明如何用BSDE方法求解，并与确定性Riccati方程对比。

Hint: 考虑值函数的鞅表示。

习题6.7 GPS欺骗检测 设计一个算法检测GPS欺骗攻击（spoofing），攻击者发送虚假GPS信号试图误导接收机。

Hint: 利用INS预测和GPS测量的一致性检验。

习题6.8 分布式卡尔曼滤波 多个节点各自有局部观测，设计分布式算法使每个节点都能估计全局状态，且达到集中式卡尔曼滤波的性能。

Hint: 考虑信息形式的卡尔曼滤波和一致性协议。

6.11 常见陷阱与错误（Gotchas）

6.11.1 数值稳定性问题

问题：协方差矩阵失去正定性

症状：

- 协方差矩阵特征值变负
- 滤波器发散
- 出现NaN或Inf

解决方案：

1. Joseph形式更新： $$P = (I - KH)P(I - KH)^T + KRK^T$$ 保证正定性但计算量大

2. 对称化： $$P = \frac{P + P^T}{2}$$

3. UD分解或平方根滤波器

6.11.2 初始化错误

问题：初始估计偏差过大导致EKF发散

解决方案：

• 使用较大的初始协方差（保守但安全）
• 多假设初始化
• 渐进式启动（先用简单模型）

6.11.3 时间戳问题

问题：传感器时间不同步

影响：

- 100ms延迟 → 100km/h车辆产生2.8m误差
- 时变延迟导致滤波器不稳定

解决方案：

• 硬件时间同步（GPS时钟）
• 软件时间补偿
• 延迟状态增广

6.11.4 模型不匹配

问题：实际系统与模型差异大

诊断方法：

• 检查创新序列是否白噪声
• 监控标准化创新平方（NIS）

解决方案：

• 增加过程噪声（鲁棒但次优）
• 自适应调整参数
• 多模型方法（IMM）

6.11.5 观测野值

问题：传感器偶尔输出错误数据

解决方案：

# 卡方检验
innovation = y - H @ x_pred
S = H @ P_pred @ H.T + R
chi2 = innovation.T @ inv(S) @ innovation
if chi2 > chi2_threshold:
    # 拒绝这个测量
    return x_pred, P_pred

6.12 最佳实践检查清单

设计阶段

• [ ] 系统建模
• [ ] 状态变量选择合理完备
• [ ] 考虑了所有重要噪声源
• [ ] 线性化点选择合适（EKF）

• [ ] 离散化方法正确（采样时间）

• [ ] 可观性分析

• [ ] 检查系统可观性矩阵秩
• [ ] 识别弱可观方向
• [ ] 设计额外测量改善可观性

实现阶段

• [ ] 数值鲁棒性
• [ ] 使用Joseph形式或UD分解
• [ ] 协方差矩阵对称化
• [ ] 避免矩阵求逆，用Cholesky分解

• [ ] 检查条件数

• [ ] 参数调试

• [ ] 过程噪声Q：从物理分析开始
• [ ] 测量噪声R：从传感器规格书开始
• [ ] 用Allan方差分析IMU噪声
• [ ] 留出参数调试接口

测试阶段

• [ ] 仿真测试
• [ ] Monte Carlo分析
• [ ] 极端工况测试
• [ ] 传感器故障模拟

• [ ] 与真值比较

• [ ] 性能监控

• [ ] 创新序列白度检验
• [ ] NIS/NEES一致性检验
• [ ] 计算时间监控
• [ ] 内存使用跟踪

部署阶段

• [ ] 故障处理
• [ ] 传感器失效检测
• [ ] 滤波器发散检测
• [ ] 降级模式设计

• [ ] 故障恢复机制

• [ ] 在线调试

• [ ] 关键变量日志记录
• [ ] 可视化工具准备
• [ ] 参数在线调整能力
• [ ] 性能指标实时监控

优化建议

• [ ] 计算优化
• [ ] 稀疏矩阵利用
• [ ] 固定点/定点实现（嵌入式）
• [ ] SIMD指令优化

• [ ] 多线程并行（多传感器）

• [ ] 精度提升

• [ ] 考虑高阶滤波器（UKF/PF）
• [ ] 自适应噪声估计
• [ ] 多模型融合
• [ ] 约束滤波（如位置非负）