4. 非线性系统估计问题
卡尔曼滤波在线性高斯系统中是最优估计器，但真实工程对象几乎都带有非线性：雷达测距是斜距与角度的非线性组合，惯导的姿态更新涉及三角函数，电池SOC与开路电压呈强非线性曲线。当系统模型 f(·) 或观测模型 h(·) 非线性时，高斯分布经过非线性映射后不再保持高斯形状，这正是非线性估计问题的根源，也是无迹卡尔曼滤波（UKF）要解决的核心矛盾。
一、核心原理：非线性函数传播改变了分布的形状
设状态 x ~ N(μ, P)，经过非线性函数 y = g(x) 传播。线性情况下，均值和协方差按 y = Aμ、P_y = A P Aᵀ 精确传递；而非线性情况下，只有对 g(·) 做泰勒展开才能近似计算。一阶展开得到：
E[g(x)] ≈ g(μ) + ½·tr( Hessian_g · P ) ← 二阶项被一阶EKF丢弃
Cov[g(x)] ≈ J_g · P · J_gᵀ ← J_g 为 g 在 μ 处的雅可比
关键点在于：均值传播存在二阶偏置，协方差传播依赖雅可比。若 g(·) 曲率大、P 又较大，被丢弃的二阶项会显著偏离真实均值，估计结果出现系统性偏差。下图给出非线性传播与EKF线性化近似的对比关系：
二、EKF的线性化误差从何而来
EKF在每个时刻对 f、h 在当前估计点求雅可比，用切线代替曲线。误差来源可归纳为三类：
- 截断误差：泰勒展开只保留一阶，二阶及以上被丢弃，曲率越大误差越大。
- 工作点漂移：雅可比在
μ处计算，但真实状态可能偏离μ，切线不再贴合曲线。 - 协方差失真：
P较大时，状态分布覆盖了曲线的大范围弯曲区域，单点切线无法代表整体。
典型误区：认为“只要雅可比推导正确，EKF就没有误差”。实际上雅可比正确只保证一阶精度，二阶偏置
½·tr(H·P) 依然存在，且随 P 增大而放大。
三、发散风险：从误差累积到滤波器崩溃
线性化误差不会孤立存在，它会通过协方差更新进入增益计算，形成正反馈。常见发散路径如下：
- 初始协方差
P₀设置过大 → 线性化误差显著 → 新息ν = z − h(x̂⁻)异常。 - 雅可比
H失真 → 卡尔曼增益K偏离最优 → 状态修正方向错误。 - 错误修正使估计偏离真值 → 下一时刻工作点更差 → 雅可比更失真。
- 协方差
P被人为压小（过度自信）→ 增益趋零 → 滤波器“拒绝”量测 → 彻底发散。
工程上判断发散的信号包括：新息序列不再零均值白噪声、P 与残差平方不匹配、状态估计持续单向漂移。下表给出EKF与UKF在非线性场景下的适用边界对比：
| 对比维度 | EKF | UKF |
|---|---|---|
| 分布传播方式 | 一阶泰勒线性化 | Sigma点确定性采样 |
| 均值精度 | 一阶，存在二阶偏置 | 可达二阶以上 |
| 协方差精度 | 依赖雅可比，易失真 | 由加权样本统计，更稳健 |
| 雅可比需求 | 必须解析或数值求导 | 无需雅可比 |
| 强非线性表现 | 易发散 | 明显优于EKF |
| 计算量 | 较低 | 约2n+1次函数评估 |
| 适用边界 | 弱非线性、小P、高更新率 | 中强非线性、大初始不确定度 |
四、案例场景：雷达跟踪中的线性化失效
考虑二维雷达跟踪，状态为 [x, y, vx, vy]，量测为斜距与方位角：
r = sqrt(x² + y²)
θ = atan2(y, x)
当目标距离雷达很近（r 小）或方位角接近 ±90° 时，atan2 的雅可比对 x 的偏导 −y/(x²+y²) 会急剧变化。若此时 P 较大，EKF用单点切线近似整个方位角分布，会产生明显偏差，甚至出现“目标穿越雷达原点”时的估计跳变。这正是UKF的典型用武之地：用Sigma点直接穿过 atan2，无需线性化。
实操建议：在决定是否使用UKF前，先做一次“线性化误差体检”——固定
P，用蒙特卡洛采样真实传播 g(x)，比较真实均值与 g(μ) 的偏差。若偏差超过状态标准差的10%，说明EKF已接近适用边界。
五、复盘：非线性估计的三条判断准则
- 看曲率：函数在
μ附近二阶导大，EKF误差大。 - 看不确定度：
P越大，分布覆盖的弯曲区域越广，线性化越不可靠。 - 看更新频率：量测更新越快，工作点越接近真值，线性化误差越小；反之则风险上升。