05｜状态估计：预测不是猜测，融合不是平均

从一个有偏置的加速度计开始

IMU 更新快，却会因偏置和姿态误差逐渐漂移；DVL 直接约束速度，但可能低频或缺测；压力计稳定约束深度，却不提供水平位置。滤波的目的不是把三条曲线平均，而是利用每种观测对不同状态的约束。

先用一维状态 x=[p,v,b]ᵀ，b 为加速度偏置。采样间隔 T 内假定测得加速度 a_m 保持不变，则：

$$\text{p⁺ = p + vT + ½(a\_m−b)T²， v⁺ = v + (a\_m−b)T， b⁺ = b}$$

相应状态转移矩阵是 [[1,T,−T²/2],[0,1,−T],[0,0,1]]。DVL 测速度，H=[0,1,0]；它并不直接测偏置，但多次速度纠正与传播的关联能帮助识别偏置。速度误差与偏置之间的交叉协方差，正是这种间接学习的数学通道。

$$\text{P⁻ = FPFᵀ + Q， r = z−Hx⁻， S = HP⁻Hᵀ+R， K = P⁻HᵀS⁻¹}$$

$$\text{x⁺ = x⁻ + Kr， P⁺ = (I−KH)P⁻(I−KH)ᵀ + KRKᵀ}$$

Q 表示传播过程引入的不确定度，R 表示观测误差，K 决定当前观测怎样纠正各状态。实现时解线性方程求 K，不显式求矩阵逆。Joseph 协方差形式有助于数值稳定。

一个可以手算的融合

预测速度为 0.2 m/s，方差 0.04；DVL 为 0.1 m/s，方差 0.01。标量增益 K=0.04/(0.04+0.01)=0.8，所以更新速度是 0.12 m/s，方差为 0.008。更精确的观测权重更大，但不是简单固定占比；随着传播时间和观测质量变化，权重会改变。

实现与实验

先产生一条真实速度曲线，加固定 0.02 m/s² IMU 偏置和随机噪声，再每隔若干传播步加入 DVL。分别运行不含偏置状态和含偏置状态的滤波器，观察长期速度误差。随后删除一段 DVL：状态继续传播，而协方差增长。这不是滤波器“坏了”，而是它在表达缺少新信息。

把 R 随意调大能让曲线更平滑，却会降低纠正速度；把 Q 调得过小会让估计器过分相信模型。合理的调节顺序是先确认单位、时间、模型与噪声来源，再看创新是否长期偏向一侧，而不是只追求输出看上去平滑。