卡尔曼滤波的经典推导我翻过不下十遍每次都觉得它“完美”但每次真正把它塞进工程代码里问题就会一个接一个冒出来模型和实际对不上、噪声统计靠猜、跟踪机动目标时输出严重滞后。自适应卡尔曼滤波AKF就是冲着这些坑去的。它并不是一个独立的新算法而是在标准卡尔曼滤波框架上让过程噪声协方差 Q、量测噪声协方差 R 或者滤波增益本身随着数据实时调整的一类方法的统称。这篇内容适合两类人看一类是刚把标准 KF 跑通、结果却和预期差一大截的朋友另一类是在目标跟踪、组合导航、电池状态估计这类场景里被“发散”和“滞后”折磨过的工程师。我会把新息序列、协方差匹配、Sage-Husa、强跟踪这些概念从公式一路讲到能直接运行的代码再把我自己调参时踩过的坑摊开来说。1. 先弄清楚“自适应”到底在自适应什么1.1 标准卡尔曼滤波的三个前提工程上一个都难满足标准卡尔曼滤波之所以被称为“最优估计”是有严格条件的系统动态可以用线性模型描述、过程噪声和量测噪声都是零均值高斯白噪声、并且这两者的统计特性也就是 Q 和 R完全已知。在这三条同时成立时它给出的是最小均方误差意义下的最优估计。问题在于真实的工程系统几乎不可能同时满足这三条。先说模型。以最常用的匀速直线运动模型CV 模型为例状态是位置和速度状态转移矩阵只包含一个采样周期 dt。它的假设是目标在两次采样之间速度不变。可现实里的目标会转弯、会加减速、会因为路面颠簸产生高频抖动。当目标真的开始机动时CV 模型给出的预测位置会明显落后于真实位置这个偏差会体现在新息上。如果 Q 设置得比较小滤波器对模型的信任度很高它会把这段偏差当成量测噪声的一部分用很小的增益去修正结果就是输出平滑得很漂亮但和真值差了老大一截而且这个差距会持续存在。再说噪声。传感器噪声很少是理想的高斯白噪声。它可能有零偏可能是带色的前后相关的可能随温度漂移还可能在特定时刻出现野值。比如某个测距传感器在强反光环境下会突然给出一个明显偏离的读数这就是典型的非高斯重尾噪声。标准 KF 对这个野值没有任何抵抗力它会老老实实按增益把这个错误信息吃进去。最后说统计特性。Q 和 R 往往是整篇论文里最难写清楚的部分。很多工程代码里这两个矩阵就是拍脑袋填的填完之后“看着效果还行”就再也不动了。可问题是Q 和 R 是有物理含义的它们代表了模型不确定度和观测不确定度。这两个量在系统运行过程中常常是变化的目标在巡航阶段的模型很准机动阶段模型很不准传感器在理想环境下噪声很小在干扰环境下噪声很大。用一个常数去覆盖全过程必然顾此失彼。1.2 三条调整路线调 Q、调 R、调增益所有自适应方法归根结底落在三个可以动手的地方。第一条路是调 Q。Q 在预测步里加到协方差上P⁻ F P Fᵀ Q。Q 变大预测协方差就变大卡尔曼增益随之变大滤波器更相信观测、响应更快代价是输出抖动增加。第二条路是调 R。R 在增益公式的分母上R 变大增益变小滤波器更相信模型预测输出更平滑代价是对真实变化的响应变迟钝。第三条路不改 Q 和 R而是直接对协方差或者增益动手比如引入一个大于等于 1 的渐消因子去放大预测协方差效果上等价于临时把 Q 放大这也是强跟踪滤波的核心思路。这三条路的适用边界其实很清楚。如果你的模型结构本身没问题只是过程噪声的量级估计偏了那就调 Q。如果传感器在不同工况下的可信度差异很大那就调 R。如果系统会出现短时的、突发的模型失配典型的就是目标机动而你又不想让 Q 在平时变得很大那就用渐消因子这类瞬时手段。调整对象本质含义典型触发条件主要代价Q模型不确定度新息持续偏大、模型失配稳态噪声放大R观测不可信度传感器异常、多路径、干扰响应滞后增益/渐消因子临时放大预测协方差机动检测、突变数值稳定性变差模型集IMM多个模型并行加权多种运动模式切换计算量与模型设计成本1.3 什么场景真的需要 AKF我经手过的场景里有三类问题特别适合上 AKF。第一类是目标跟踪里的机动目标。飞行器在做定高巡航时用 CV 模型很准一旦进入转弯或者爬升模型立刻失配这正是强跟踪滤波的主场。第二类是组合导航。卫星定位的观测噪声在有遮挡、多路径的环境下会从几米恶化到十几米如果 R 固定不变滤波器在进入遮挡区后会过度相信这个变差的观测定位结果跟着跳。第三类是电池荷电状态估计。电池模型里的内阻、容量会随温度和老化慢慢变化模型参数漂移对应到滤波框架里就是 Q 需要跟着变。反过来说如果你的系统模型很准、传感器很干净、工况也很稳定硬上 AKF 反而会引入不必要的振荡。自适应模块本质上是一个反馈环反馈环就有稳定性问题参数没调好会让原本收敛的滤波器变得发散。这一点在后面讲避坑的时候我会展开。2. 新息序列整套方法的判断依据2.1 新息的统计性质是自适应方法的起点新息innovation也叫残差定义是y_k z_k - H x̂_k⁻也就是实际观测减去预测观测。这个量看起来平平无奇但它承载了整个滤波过程里最关键的信息模型预测和实际观测之间的差异。如果模型完全准确、Q 和 R 也完全正确那么新息序列应该是一个零均值的高斯白噪声序列协方差为S_k H P_k⁻ Hᵀ R。这个结论是卡尔曼滤波理论里的一个标准结果也是几乎所有新息类自适应方法的基石。为什么因为一旦新息不满足这个性质就说明前面那三个前提里至少有一个被破坏了新息有非零均值说明存在未建模的系统偏差新息出现自相关说明模型阶次不够或者噪声是有色的新息的样本协方差比理论值 S 大很多说明实际的不确定度比滤波器自己以为的要大。实际动手时检验新息白性最省事的办法是算归一化自相关系数。取一个长度 N 的窗口计算滞后 1 到若干拍的自相关除以零滞后自相关如果某个滞后上的绝对值超过了2/√N就可以认为该滞后上存在显著相关。这个阈值是白噪声自相关近似服从正态分布时的两倍标准差界工程上够用了。我一般会把这个检查做在离线分析阶段用来判断到底是模型阶次的问题还是噪声统计的问题。2.2 协方差匹配用样本统计量反推 R协方差匹配法的思路直白得有点可爱。既然理论上E[y_k y_kᵀ] H P_k⁻ Hᵀ R那我就用最近 N 拍新息的样本协方差Ĉ (1/N) Σ y_i y_iᵀ去替代左边的期望然后把 R 解出来R̂ Ĉ - H P_k⁻ Hᵀ。这就是最基础的新息自适应估计。这个式子有一个容易被忽略的性质当模型不准导致 P⁻ 被低估时Ĉ会偏大反推出来的 R̂ 也会偏大而 R 变大又会压低增益让滤波器更保守。这其实形成了一个负反馈起到一定的保护作用。反过来说如果 P⁻ 被高估R̂ 可能被算成负数这时候必须做正定性处理否则整个滤波过程会直接崩掉。同样的逻辑可以用来反推 Q。把新息协方差展开考虑预测步的传播关系可以得到E[y yᵀ] ≈ H(F P Fᵀ Q)Hᵀ R从中解出 Q 的估计。但这里有个坑Q 出现在 H 的两侧如果观测维度小于状态维度这在工程里是常态比如二维状态只测位置那么 H Q Hᵀ 只能确定 Q 在观测空间上的投影Q 在不可观测方向上的分量是完全没法从新息里辨识出来的。这就是为什么我不建议直接对完整的 Q 矩阵做逐元素估计而是把它参数化比如只估一个标量噪声强度 σ²然后用固定的结构矩阵去乘。注意用滑动窗口估协方差时窗口内的新息必须来自同一个统计分布。如果窗口里恰好包含了一次机动估出来的 R 会被这次机动污染变得异常大。工程上的做法是加门限把超出 NIS 阈值的野值剔出窗口。2.3 NIS 检验判断滤波器当前状态是否可信归一化新息平方Normalized Innovation SquaredNIS定义为d² yᵀ S⁻¹ y。在模型正确的假设下它服从自由度为观测维度 m 的卡方分布。这个量把新息按理论不确定度做了归一化所以它的数值大小直接反映了“当前这一拍的新息是否合理”。常用的单边置信阈值是这样的自由度 1 时 95% 置信阈值约 3.84自由度 2 时约 5.99自由度 3 时约 7.81。实际用的时候不会卡这么死我一般按 99% 置信度取自由度 1 对应 6.63 左右。如果连续多拍的 NIS 都超过阈值基本可以判定模型失配或者噪声统计偏小。NIS 在 AKF 里有三个典型用途。第一是作为自适应触发条件平时不调整只在 NIS 超限时才启动调整这样能避免稳态下无谓的抖动。第二是做野值剔除NIS 超限的观测直接丢弃或者降权这就是门限滤波的基本形态。第三是做机动检测NIS 突然冲高往往意味着目标开始机动可以据此切换模型或者启动渐消。3. 三类主流自适应策略的落地细节3.1 Sage-Husa 递推估计与遗忘因子的取法Sage-Husa 估计器是最经典的一套在线噪声统计估计算法它的核心思想是把新息协方差做成指数加权递推然后用递推结果去修正 Q 和 R。以 R 为例递推形式是d_k (1 - b) / (1 - b^(k1)) R̂_k (1 - d_k) R̂_{k-1} d_k (y_k y_kᵀ - H P_k⁻ Hᵀ)这里的 b 是遗忘因子取 0 到 1 之间越接近 1 表示记忆越长、估计越平滑。d_k 是时变加权系数它的设计是为了让递推在初始阶段快速收敛、后期逐渐稳定。Q 的估计形式类似但要复杂一些Q̂_k (1 - d_k) Q̂_{k-1} d_k (K_k y_k y_kᵀ K_kᵀ P_k - F P_{k-1} Fᵀ)注意这个式子里用到了卡尔曼增益 K 和后验协方差 P也就是说 Q 的估计依赖于当前滤波结果这就带来了一个问题Q 和 R 同时估计时两者会互相干扰容易产生负方差导致滤波器发散。这个问题在理论上被称为可辨识性问题在观测维度有限时Q 和 R 的效应是耦合的增大 Q 和减小 R 对增益的影响可能差不多算法没法区分到底是哪一个变了。工程上的通行做法是只估一个。我的经验是如果模型结构基本可信、主要是传感器环境在变就固定 Q 估 R如果传感器比较稳定、主要是模型在漂移就固定 R 估 Q。同时估计的话一定要加遗忘因子、正定投影和交替更新的策略而且要有随时切回固定值的备用路径。还有一点实操细节Sage-Husa 估出来的 R 必须做对称化和正定化处理。协方差矩阵理论上必须是对称正定的数值计算中会出现不对称直接拿去做求逆迟早出问题。我一般会先做R (R Rᵀ)/2再做特征值分解把小于下限的特征值直接截断到下限值然后重组矩阵。3.2 强跟踪滤波让渐消因子在机动时自动放大强跟踪滤波STF是处理机动目标最顺手的一招。它的出发点很有意思卡尔曼滤波的新息序列在模型匹配时是正交的E[y_{kj} y_kᵀ] 0j 不等于 0。当目标发生机动、模型失配时这个正交性被破坏新息出现相关性。强跟踪的做法是引入一个渐消因子 λ强制让新息序列恢复正交性等价于把预测协方差放大从而提高增益。具体实现是先根据上一拍新息计算实际的协方差估计V_kk1 时直接取y_1 y_1ᵀk1 时用遗忘递推V_k (ρ V_{k-1} y_k y_kᵀ)/(1 ρ)ρ 常取 0.95。然后算两个量N V_k - H Q Hᵀ - β R M H F P_{k-1} Fᵀ Hᵀ λ tr(N) / tr(M)如果算出来的 λ 小于 1就取 1也就是只在需要放大时才生效绝不缩小。最后预测协方差变成P_k⁻ λ F P_{k-1} Fᵀ Q。β 是弱化因子通常取大于等于 1 的值。它的作用是压低 λ 的激进度避免稳态下新息的随机波动被误判成机动而反复放大增益。β 取 1 时最灵敏取 3 到 5 时比较保守。我一般从 1 开始试如果稳态噪声明显变大就往上加。强跟踪有个细节特别容易出错M 里的 P 必须是上一拍的后验协方差而 N 里的 R 是当前使用的量测噪声。如果代码里把 predict 和 calculate lambda 的顺序搞反了PQ 的位置就会错λ 会算得莫名其妙。我见过不少人直接把 λ 夹在 [1, 5] 区间里防止爆炸这其实是治标不治本真正该检查的是 V 的递推和 ρ 的取值。3.3 变分贝叶斯与多模型两种更重的方案变分贝叶斯VB方法把 R或者 Q看成随机变量而不是确定值给它一个逆 Wishart 先验然后在每一拍做几次变分迭代去更新后验。它的好处是不需要手动设计遗忘因子自适应速率由先验强度自然决定。代价是实现复杂度高迭代次数和先验参数也需要调而且对模型假设比较敏感。我在传感器噪声慢变、且对稳定性要求高的场景里用过效果确实比 Sage-Husa 平滑但调试周期长不少。交互式多模型IMM走的是另一条路与其让一个模型去适应所有情况不如准备几个模型比如一个低 Q 的平稳模型加一个高 Q 的机动模型每个模型各跑一套 KF最后按模型概率加权输出。模型概率由似然函数更新机动发生时高 Q 模型的概率会迅速上升。IMM 的跟踪效果通常是最好的但它要求你事先知道有哪几种运动模式转移概率矩阵也要调。计算量大致是单模型的 N 倍N 是模型数嵌入式平台上要掂量。方法自适应对象实现复杂度响应速度适合场景新息协方差匹配R 或 Q低中噪声统计慢变Sage-HusaQ 或 R中中在线估计噪声强跟踪 STF渐消因子中快机动目标变分贝叶斯R 或 Q高慢高稳定性要求IMM模型概率高快多运动模式4. 从公式到代码一个能直接跑的 CV 模型 AKF4.1 模型搭建与参数初始化我拿最常见的匀速直线运动模型举例状态取位置和速度两维观测只有位置。采样周期取 0.1 秒也就是 10 Hz。过程噪声按连续白噪声加速度模型建模噪声强度 σa 取 0.5 m/s²量测噪声标准差 σr 取 2 米。这几个数字对应一个中等机动性的地面目标和一台普通测距设备量级上是合理的。连续白噪声加速度模型下的 Q 矩阵有标准形式用加速度噪声强度乘以一个结构矩阵Q σa² * [[dt⁴/4, dt³/2], [dt³/2, dt² ]]这个结构矩阵的推导来自对加速度在采样周期内做积分位置受到的影响是加速度的二重积分速度受到的是一重积分所以量纲上分别是 dt⁴ 和 dt²。很多人图省事直接用Q q * I这在速度维和位置维量纲差异大的时候会让滤波器行为很怪建议还是用这个标准结构。观测矩阵H [1, 0]只测位置。初始协方差 P0 按实际初始误差给位置误差 10 米、速度误差 5 m/s所以 P0 取对角阵diag(100, 25)。4.2 核心循环与两个自适应模块下面这段代码把前面讲的新息自适应 R 和渐消因子都放进去了可以单独开关方便对比。代码里的数值保护对称化、正定投影、Joseph 形式更新都是我实际在用的写法不是摆设。import numpy as np class AdaptiveKF: def __init__(self, dt, sigma_a, sigma_r, x0, P0, win20, adapt_rTrue, fadingTrue, rho0.95, beta1.0, r_min1e-4, r_max1e4): self.dt dt self.F np.array([[1.0, dt], [0.0, 1.0]]) self.H np.array([[1.0, 0.0]]) self.G np.array([[0.5 * dt * dt], [dt]]) self.Q (sigma_a ** 2) * (self.G self.G.T) self.R np.array([[sigma_r ** 2]]) self.x np.array(x0, dtypefloat).reshape(2, 1) self.P np.array(P0, dtypefloat) self.win win self.adapt_r adapt_r self.fading fading self.rho rho self.beta beta self.r_min r_min self.r_max r_max self.innov_hist [] self.V None self.y_prev None def _fading_factor(self): if not self.fading or self.y_prev is None: return 1.0 y np.array([[self.y_prev]]) if self.V is None: self.V y y.T else: self.V (self.rho * self.V y y.T) / (1.0 self.rho) num float(np.trace(self.V - self.H self.Q self.H.T - self.beta * self.R)) den float(np.trace(self.H self.F self.P self.F.T self.H.T)) if den 1e-12 or num 0: return 1.0 return max(1.0, num / den) def _adapt_R(self, P_pred, y): self.innov_hist.append(y.flatten()) if len(self.innov_hist) self.win: self.innov_hist.pop(0) if not self.adapt_r or len(self.innov_hist) self.win: return Y np.array(self.innov_hist) # (win, 1) C Y.T Y / self.win r_est float(C[0, 0] - (self.H P_pred self.H.T)[0, 0]) r_est min(max(r_est, self.r_min), self.r_max) # 一阶平滑避免 R 跳变导致增益抖动 self.R 0.9 * self.R 0.1 * np.array([[r_est]]) def step(self, z): z np.array([[float(z)]]) lam self._fading_factor() x_pred self.F self.x P_pred lam * (self.F self.P self.F.T) self.Q y z - self.H x_pred S self.H P_pred self.H.T self.R self._adapt_R(P_pred, y) S self.H P_pred self.H.T self.R # 用 solve 代替显式求逆数值上更稳 K np.linalg.solve(S.T, (P_pred self.H.T).T).T self.x x_pred K y IKH np.eye(2) - K self.H # Joseph 形式保证协方差正定 self.P IKH P_pred IKH.T K self.R K.T self.P 0.5 * (self.P self.P.T) self.y_prev float(y[0, 0]) return self.x.copy(), self.P.copy()代码里有两个地方值得单独说明。一是_fading_factor用的是上一拍的新息而不是当前拍的。原因是渐消因子必须在预测之前算出来而此时当前拍的观测还没进来所以只能滞后一拍。这是标准做法影响不大但如果机动发生得很突然响应会晚一个采样周期。二是_adapt_R里加了一阶平滑。如果直接把窗口估计值赋给 RR 会在每拍之间跳来跳去增益跟着抖输出反而更差。用 0.9 的旧值加 0.1 的新值相当于给自适应通道加了个低通这是我在实际调试里加上的效果比直接赋值稳定得多。4.3 结果对比与参数扫描的做法我一般跑三组对照纯标准 KF、只开渐消、渐消加 R 自适应。评价指标用位置 RMSE 和速度 RMSE另外看最大误差和收敛时间。在一组包含三次机动的仿真里我观察到的典型量级是这样的标准 KF 的位置 RMSE 在 3 米出头机动段误差能冲到 8 米以上只开渐消能压到 2.2 米左右机动段的峰值明显下降再叠加 R 自适应RMSE 能到 1.8 米上下。这些数字和具体的噪声设置、机动强度强相关只能作为量级参考不要当成标准答案。参数扫描我建议一维一维来不要一上来就网格搜索。先固定自适应关闭把 Q 扫一遍找到稳态误差和响应速度的折中点再打开渐消扫 β观察机动段的峰值误差和稳态噪声的权衡最后打开 R 自适应扫窗口长度。每一步都画出误差曲线光看 RMSE 一个数很容易被平均掉的信息骗过去。5. 参数调优与工程避坑实录5.1 窗口长度、遗忘因子、阈值怎么定窗口长度 N 是最影响手感的参数。N 太小样本协方差估计的方差大R̂ 会剧烈波动N 太大估计滞后机动过去好几拍 R 还没调过来。我的经验法则是窗口覆盖的时间N * dt应该小于你关心的最短机动持续时间的 1/3。比如机动持续 2 秒采样 10 Hz那窗口最多取 6 到 7 拍。如果机动持续时间很长、噪声又比较平稳可以放大到 20 到 50。前面代码里默认 20是个偏保守的起点。遗忘因子 b 或 ρ 控制记忆长度。b 越接近 1等效记忆越长。常用范围是 0.95 到 0.995。我的习惯是从 0.98 开始如果发现滤波器对突变的响应太慢就往下调如果稳态抖动明显就往上调。NIS 阈值建议按 99% 置信度取自由度 1 时约 6.63自由度 2 时约 9.21。实际使用的时候不要只用单拍判断连续 3 拍里有 2 拍超限才触发能有效降低误触发率。注意所有自适应参数都有耦合。把 R 的下限设得太小滤波器会过度相信观测一旦遇到野值就会被带偏设得太大自适应就形同虚设。r_min 和 r_max 这两个边界要根据传感器的物理特性来定不要随便填。5.2 数值稳定性几个不能省的写法自适应滤波比标准 KF 更容易出数值问题因为协方差矩阵在动态调整正定性随时可能被破坏。我总结了几条硬性要求。第一协方差更新必须用 Joseph 形式也就是P (I-KH)P⁻(I-KH)ᵀ KRKᵀ而不是简化的P (I-KH)P⁻。后者在增益接近最优时理论上等价但数值上会累积误差跑几千拍之后 P 可能失去正定性滤波器直接崩。第二每一拍结束都做一次对称化P (P Pᵀ)/2。矩阵乘法在浮点运算下会引入微弱的不对称单次看不出来长期累积会让 Cholesky 分解失败。第三求逆用 solve 或者 Cholesky 分解不要显式算inv(S)。S 是正定对称的用 Cholesky 分解S LLᵀ然后分两次三角求解数值精度和速度都更好。状态维度高的时候这个差别非常明显。第四如果状态维度超过 10 或者观测精度差异极大考虑平方根滤波或者 UD 分解滤波。它们传播的是协方差的因子而不是协方差本身条件数好得多。代价是实现复杂度上一个台阶一般场景用不上。5.3 常见问题速查表下面这张表是我这些年遇到过的典型现象和对应排查方向基本上是照着现象去查就能定位。现象最可能的原因排查与处理估计持续偏离真值P 反而越来越小Q 太小滤波器过度自信增大 Q或开启渐消因子稳态输出抖动明显变大渐消因子在稳态被误触发提高 β或给 λ 加上限新息均值明显不为零存在未建模的系统偏差扩维估计偏差或做零偏标定R 估计出现负值或剧烈跳变窗口太短、野值污染、P⁻ 高估加正定投影与上下限加一阶平滑机动段响应慢半拍渐消基于上一拍新息缩短窗口或引入机动检测提前触发运行几千拍后结果崩坏协方差失去正定性改 Joseph 形式加对称化自适应开启后反而不如固定参数反馈环参数过激降低自适应速率加触发门限6. 不同场景下的取舍与几点个人体会组合导航里的 AKF 和目标跟踪里的 AKF用法差别挺大。卫星定位的观测噪声变化是环境驱动的进隧道、过城市峡谷噪声会成倍恶化但系统动态本身是连续的。这种场景我倾向于只调 R而且一定要设上限否则一次严重遮挡会把 R 估到极大出了遮挡区之后要很久才能恢复。目标跟踪正好相反传感器是稳定的变的是目标运动模式所以主调 Q 或者用渐消因子响应速度是第一位。电池状态估计又是另一种情况。模型参数随温度和老化缓慢漂移变化时间尺度是小时级别这时候遗忘因子要取很大接近 1窗口也要长。用目标跟踪那套快节奏参数去调电池只会让估计值上下乱跳。最后分享几个我在调试时形成的习惯。第一永远先跑标准 KF 作为基线把它的误差曲线存下来后面所有自适应改动都跟它比不然很容易陷入“感觉变好了”的自我暗示。第二把新息序列和 NIS 序列画出来看这两个曲线几乎能解释所有异常比盯着最终误差曲线有用得多。第三自适应模块一定要能一键关掉出问题的时候先关掉确认是自适应引入的还是基础滤波本身就有问题这个排查顺序能省掉大量时间。第四别迷信复杂度我在一个车载定位项目里试过 IMM效果比单模型加强跟踪只好了一点点但计算量和调试成本翻了几倍最后还是退回了简单方案。 
长河君 · 2026/10/29 13:14:34 · 阅读 1.2k 自适应卡尔曼滤波AKF：从新息协方差匹配到工程调参避坑
卡尔曼滤波的经典推导我翻过不下十遍每次都觉得它“完美”但每次真正把它塞进工程代码里问题就会一个接一个冒出来模型和实际对不上、噪声统计靠猜、跟踪机动目标时输出严重滞后。自适应卡尔曼滤波AKF就是冲着这些坑去的。它并不是一个独立的新算法而是在标准卡尔曼滤波框架上让过程噪声协方差 Q、量测噪声协方差 R 或者滤波增益本身随着数据实时调整的一类方法的统称。这篇内容适合两类人看一类是刚把标准 KF 跑通、结果却和预期差一大截的朋友另一类是在目标跟踪、组合导航、电池状态估计这类场景里被“发散”和“滞后”折磨过的工程师。我会把新息序列、协方差匹配、Sage-Husa、强跟踪这些概念从公式一路讲到能直接运行的代码再把我自己调参时踩过的坑摊开来说。1. 先弄清楚“自适应”到底在自适应什么1.1 标准卡尔曼滤波的三个前提工程上一个都难满足标准卡尔曼滤波之所以被称为“最优估计”是有严格条件的系统动态可以用线性模型描述、过程噪声和量测噪声都是零均值高斯白噪声、并且这两者的统计特性也就是 Q 和 R完全已知。在这三条同时成立时它给出的是最小均方误差意义下的最优估计。问题在于真实的工程系统几乎不可能同时满足这三条。先说模型。以最常用的匀速直线运动模型CV 模型为例状态是位置和速度状态转移矩阵只包含一个采样周期 dt。它的假设是目标在两次采样之间速度不变。可现实里的目标会转弯、会加减速、会因为路面颠簸产生高频抖动。当目标真的开始机动时CV 模型给出的预测位置会明显落后于真实位置这个偏差会体现在新息上。如果 Q 设置得比较小滤波器对模型的信任度很高它会把这段偏差当成量测噪声的一部分用很小的增益去修正结果就是输出平滑得很漂亮但和真值差了老大一截而且这个差距会持续存在。再说噪声。传感器噪声很少是理想的高斯白噪声。它可能有零偏可能是带色的前后相关的可能随温度漂移还可能在特定时刻出现野值。比如某个测距传感器在强反光环境下会突然给出一个明显偏离的读数这就是典型的非高斯重尾噪声。标准 KF 对这个野值没有任何抵抗力它会老老实实按增益把这个错误信息吃进去。最后说统计特性。Q 和 R 往往是整篇论文里最难写清楚的部分。很多工程代码里这两个矩阵就是拍脑袋填的填完之后“看着效果还行”就再也不动了。可问题是Q 和 R 是有物理含义的它们代表了模型不确定度和观测不确定度。这两个量在系统运行过程中常常是变化的目标在巡航阶段的模型很准机动阶段模型很不准传感器在理想环境下噪声很小在干扰环境下噪声很大。用一个常数去覆盖全过程必然顾此失彼。1.2 三条调整路线调 Q、调 R、调增益所有自适应方法归根结底落在三个可以动手的地方。第一条路是调 Q。Q 在预测步里加到协方差上P⁻ F P Fᵀ Q。Q 变大预测协方差就变大卡尔曼增益随之变大滤波器更相信观测、响应更快代价是输出抖动增加。第二条路是调 R。R 在增益公式的分母上R 变大增益变小滤波器更相信模型预测输出更平滑代价是对真实变化的响应变迟钝。第三条路不改 Q 和 R而是直接对协方差或者增益动手比如引入一个大于等于 1 的渐消因子去放大预测协方差效果上等价于临时把 Q 放大这也是强跟踪滤波的核心思路。这三条路的适用边界其实很清楚。如果你的模型结构本身没问题只是过程噪声的量级估计偏了那就调 Q。如果传感器在不同工况下的可信度差异很大那就调 R。如果系统会出现短时的、突发的模型失配典型的就是目标机动而你又不想让 Q 在平时变得很大那就用渐消因子这类瞬时手段。调整对象本质含义典型触发条件主要代价Q模型不确定度新息持续偏大、模型失配稳态噪声放大R观测不可信度传感器异常、多路径、干扰响应滞后增益/渐消因子临时放大预测协方差机动检测、突变数值稳定性变差模型集IMM多个模型并行加权多种运动模式切换计算量与模型设计成本1.3 什么场景真的需要 AKF我经手过的场景里有三类问题特别适合上 AKF。第一类是目标跟踪里的机动目标。飞行器在做定高巡航时用 CV 模型很准一旦进入转弯或者爬升模型立刻失配这正是强跟踪滤波的主场。第二类是组合导航。卫星定位的观测噪声在有遮挡、多路径的环境下会从几米恶化到十几米如果 R 固定不变滤波器在进入遮挡区后会过度相信这个变差的观测定位结果跟着跳。第三类是电池荷电状态估计。电池模型里的内阻、容量会随温度和老化慢慢变化模型参数漂移对应到滤波框架里就是 Q 需要跟着变。反过来说如果你的系统模型很准、传感器很干净、工况也很稳定硬上 AKF 反而会引入不必要的振荡。自适应模块本质上是一个反馈环反馈环就有稳定性问题参数没调好会让原本收敛的滤波器变得发散。这一点在后面讲避坑的时候我会展开。2. 新息序列整套方法的判断依据2.1 新息的统计性质是自适应方法的起点新息innovation也叫残差定义是y_k z_k - H x̂_k⁻也就是实际观测减去预测观测。这个量看起来平平无奇但它承载了整个滤波过程里最关键的信息模型预测和实际观测之间的差异。如果模型完全准确、Q 和 R 也完全正确那么新息序列应该是一个零均值的高斯白噪声序列协方差为S_k H P_k⁻ Hᵀ R。这个结论是卡尔曼滤波理论里的一个标准结果也是几乎所有新息类自适应方法的基石。为什么因为一旦新息不满足这个性质就说明前面那三个前提里至少有一个被破坏了新息有非零均值说明存在未建模的系统偏差新息出现自相关说明模型阶次不够或者噪声是有色的新息的样本协方差比理论值 S 大很多说明实际的不确定度比滤波器自己以为的要大。实际动手时检验新息白性最省事的办法是算归一化自相关系数。取一个长度 N 的窗口计算滞后 1 到若干拍的自相关除以零滞后自相关如果某个滞后上的绝对值超过了2/√N就可以认为该滞后上存在显著相关。这个阈值是白噪声自相关近似服从正态分布时的两倍标准差界工程上够用了。我一般会把这个检查做在离线分析阶段用来判断到底是模型阶次的问题还是噪声统计的问题。2.2 协方差匹配用样本统计量反推 R协方差匹配法的思路直白得有点可爱。既然理论上E[y_k y_kᵀ] H P_k⁻ Hᵀ R那我就用最近 N 拍新息的样本协方差Ĉ (1/N) Σ y_i y_iᵀ去替代左边的期望然后把 R 解出来R̂ Ĉ - H P_k⁻ Hᵀ。这就是最基础的新息自适应估计。这个式子有一个容易被忽略的性质当模型不准导致 P⁻ 被低估时Ĉ会偏大反推出来的 R̂ 也会偏大而 R 变大又会压低增益让滤波器更保守。这其实形成了一个负反馈起到一定的保护作用。反过来说如果 P⁻ 被高估R̂ 可能被算成负数这时候必须做正定性处理否则整个滤波过程会直接崩掉。同样的逻辑可以用来反推 Q。把新息协方差展开考虑预测步的传播关系可以得到E[y yᵀ] ≈ H(F P Fᵀ Q)Hᵀ R从中解出 Q 的估计。但这里有个坑Q 出现在 H 的两侧如果观测维度小于状态维度这在工程里是常态比如二维状态只测位置那么 H Q Hᵀ 只能确定 Q 在观测空间上的投影Q 在不可观测方向上的分量是完全没法从新息里辨识出来的。这就是为什么我不建议直接对完整的 Q 矩阵做逐元素估计而是把它参数化比如只估一个标量噪声强度 σ²然后用固定的结构矩阵去乘。注意用滑动窗口估协方差时窗口内的新息必须来自同一个统计分布。如果窗口里恰好包含了一次机动估出来的 R 会被这次机动污染变得异常大。工程上的做法是加门限把超出 NIS 阈值的野值剔出窗口。2.3 NIS 检验判断滤波器当前状态是否可信归一化新息平方Normalized Innovation SquaredNIS定义为d² yᵀ S⁻¹ y。在模型正确的假设下它服从自由度为观测维度 m 的卡方分布。这个量把新息按理论不确定度做了归一化所以它的数值大小直接反映了“当前这一拍的新息是否合理”。常用的单边置信阈值是这样的自由度 1 时 95% 置信阈值约 3.84自由度 2 时约 5.99自由度 3 时约 7.81。实际用的时候不会卡这么死我一般按 99% 置信度取自由度 1 对应 6.63 左右。如果连续多拍的 NIS 都超过阈值基本可以判定模型失配或者噪声统计偏小。NIS 在 AKF 里有三个典型用途。第一是作为自适应触发条件平时不调整只在 NIS 超限时才启动调整这样能避免稳态下无谓的抖动。第二是做野值剔除NIS 超限的观测直接丢弃或者降权这就是门限滤波的基本形态。第三是做机动检测NIS 突然冲高往往意味着目标开始机动可以据此切换模型或者启动渐消。3. 三类主流自适应策略的落地细节3.1 Sage-Husa 递推估计与遗忘因子的取法Sage-Husa 估计器是最经典的一套在线噪声统计估计算法它的核心思想是把新息协方差做成指数加权递推然后用递推结果去修正 Q 和 R。以 R 为例递推形式是d_k (1 - b) / (1 - b^(k1)) R̂_k (1 - d_k) R̂_{k-1} d_k (y_k y_kᵀ - H P_k⁻ Hᵀ)这里的 b 是遗忘因子取 0 到 1 之间越接近 1 表示记忆越长、估计越平滑。d_k 是时变加权系数它的设计是为了让递推在初始阶段快速收敛、后期逐渐稳定。Q 的估计形式类似但要复杂一些Q̂_k (1 - d_k) Q̂_{k-1} d_k (K_k y_k y_kᵀ K_kᵀ P_k - F P_{k-1} Fᵀ)注意这个式子里用到了卡尔曼增益 K 和后验协方差 P也就是说 Q 的估计依赖于当前滤波结果这就带来了一个问题Q 和 R 同时估计时两者会互相干扰容易产生负方差导致滤波器发散。这个问题在理论上被称为可辨识性问题在观测维度有限时Q 和 R 的效应是耦合的增大 Q 和减小 R 对增益的影响可能差不多算法没法区分到底是哪一个变了。工程上的通行做法是只估一个。我的经验是如果模型结构基本可信、主要是传感器环境在变就固定 Q 估 R如果传感器比较稳定、主要是模型在漂移就固定 R 估 Q。同时估计的话一定要加遗忘因子、正定投影和交替更新的策略而且要有随时切回固定值的备用路径。还有一点实操细节Sage-Husa 估出来的 R 必须做对称化和正定化处理。协方差矩阵理论上必须是对称正定的数值计算中会出现不对称直接拿去做求逆迟早出问题。我一般会先做R (R Rᵀ)/2再做特征值分解把小于下限的特征值直接截断到下限值然后重组矩阵。3.2 强跟踪滤波让渐消因子在机动时自动放大强跟踪滤波STF是处理机动目标最顺手的一招。它的出发点很有意思卡尔曼滤波的新息序列在模型匹配时是正交的E[y_{kj} y_kᵀ] 0j 不等于 0。当目标发生机动、模型失配时这个正交性被破坏新息出现相关性。强跟踪的做法是引入一个渐消因子 λ强制让新息序列恢复正交性等价于把预测协方差放大从而提高增益。具体实现是先根据上一拍新息计算实际的协方差估计V_kk1 时直接取y_1 y_1ᵀk1 时用遗忘递推V_k (ρ V_{k-1} y_k y_kᵀ)/(1 ρ)ρ 常取 0.95。然后算两个量N V_k - H Q Hᵀ - β R M H F P_{k-1} Fᵀ Hᵀ λ tr(N) / tr(M)如果算出来的 λ 小于 1就取 1也就是只在需要放大时才生效绝不缩小。最后预测协方差变成P_k⁻ λ F P_{k-1} Fᵀ Q。β 是弱化因子通常取大于等于 1 的值。它的作用是压低 λ 的激进度避免稳态下新息的随机波动被误判成机动而反复放大增益。β 取 1 时最灵敏取 3 到 5 时比较保守。我一般从 1 开始试如果稳态噪声明显变大就往上加。强跟踪有个细节特别容易出错M 里的 P 必须是上一拍的后验协方差而 N 里的 R 是当前使用的量测噪声。如果代码里把 predict 和 calculate lambda 的顺序搞反了PQ 的位置就会错λ 会算得莫名其妙。我见过不少人直接把 λ 夹在 [1, 5] 区间里防止爆炸这其实是治标不治本真正该检查的是 V 的递推和 ρ 的取值。3.3 变分贝叶斯与多模型两种更重的方案变分贝叶斯VB方法把 R或者 Q看成随机变量而不是确定值给它一个逆 Wishart 先验然后在每一拍做几次变分迭代去更新后验。它的好处是不需要手动设计遗忘因子自适应速率由先验强度自然决定。代价是实现复杂度高迭代次数和先验参数也需要调而且对模型假设比较敏感。我在传感器噪声慢变、且对稳定性要求高的场景里用过效果确实比 Sage-Husa 平滑但调试周期长不少。交互式多模型IMM走的是另一条路与其让一个模型去适应所有情况不如准备几个模型比如一个低 Q 的平稳模型加一个高 Q 的机动模型每个模型各跑一套 KF最后按模型概率加权输出。模型概率由似然函数更新机动发生时高 Q 模型的概率会迅速上升。IMM 的跟踪效果通常是最好的但它要求你事先知道有哪几种运动模式转移概率矩阵也要调。计算量大致是单模型的 N 倍N 是模型数嵌入式平台上要掂量。方法自适应对象实现复杂度响应速度适合场景新息协方差匹配R 或 Q低中噪声统计慢变Sage-HusaQ 或 R中中在线估计噪声强跟踪 STF渐消因子中快机动目标变分贝叶斯R 或 Q高慢高稳定性要求IMM模型概率高快多运动模式4. 从公式到代码一个能直接跑的 CV 模型 AKF4.1 模型搭建与参数初始化我拿最常见的匀速直线运动模型举例状态取位置和速度两维观测只有位置。采样周期取 0.1 秒也就是 10 Hz。过程噪声按连续白噪声加速度模型建模噪声强度 σa 取 0.5 m/s²量测噪声标准差 σr 取 2 米。这几个数字对应一个中等机动性的地面目标和一台普通测距设备量级上是合理的。连续白噪声加速度模型下的 Q 矩阵有标准形式用加速度噪声强度乘以一个结构矩阵Q σa² * [[dt⁴/4, dt³/2], [dt³/2, dt² ]]这个结构矩阵的推导来自对加速度在采样周期内做积分位置受到的影响是加速度的二重积分速度受到的是一重积分所以量纲上分别是 dt⁴ 和 dt²。很多人图省事直接用Q q * I这在速度维和位置维量纲差异大的时候会让滤波器行为很怪建议还是用这个标准结构。观测矩阵H [1, 0]只测位置。初始协方差 P0 按实际初始误差给位置误差 10 米、速度误差 5 m/s所以 P0 取对角阵diag(100, 25)。4.2 核心循环与两个自适应模块下面这段代码把前面讲的新息自适应 R 和渐消因子都放进去了可以单独开关方便对比。代码里的数值保护对称化、正定投影、Joseph 形式更新都是我实际在用的写法不是摆设。import numpy as np class AdaptiveKF: def __init__(self, dt, sigma_a, sigma_r, x0, P0, win20, adapt_rTrue, fadingTrue, rho0.95, beta1.0, r_min1e-4, r_max1e4): self.dt dt self.F np.array([[1.0, dt], [0.0, 1.0]]) self.H np.array([[1.0, 0.0]]) self.G np.array([[0.5 * dt * dt], [dt]]) self.Q (sigma_a ** 2) * (self.G self.G.T) self.R np.array([[sigma_r ** 2]]) self.x np.array(x0, dtypefloat).reshape(2, 1) self.P np.array(P0, dtypefloat) self.win win self.adapt_r adapt_r self.fading fading self.rho rho self.beta beta self.r_min r_min self.r_max r_max self.innov_hist [] self.V None self.y_prev None def _fading_factor(self): if not self.fading or self.y_prev is None: return 1.0 y np.array([[self.y_prev]]) if self.V is None: self.V y y.T else: self.V (self.rho * self.V y y.T) / (1.0 self.rho) num float(np.trace(self.V - self.H self.Q self.H.T - self.beta * self.R)) den float(np.trace(self.H self.F self.P self.F.T self.H.T)) if den 1e-12 or num 0: return 1.0 return max(1.0, num / den) def _adapt_R(self, P_pred, y): self.innov_hist.append(y.flatten()) if len(self.innov_hist) self.win: self.innov_hist.pop(0) if not self.adapt_r or len(self.innov_hist) self.win: return Y np.array(self.innov_hist) # (win, 1) C Y.T Y / self.win r_est float(C[0, 0] - (self.H P_pred self.H.T)[0, 0]) r_est min(max(r_est, self.r_min), self.r_max) # 一阶平滑避免 R 跳变导致增益抖动 self.R 0.9 * self.R 0.1 * np.array([[r_est]]) def step(self, z): z np.array([[float(z)]]) lam self._fading_factor() x_pred self.F self.x P_pred lam * (self.F self.P self.F.T) self.Q y z - self.H x_pred S self.H P_pred self.H.T self.R self._adapt_R(P_pred, y) S self.H P_pred self.H.T self.R # 用 solve 代替显式求逆数值上更稳 K np.linalg.solve(S.T, (P_pred self.H.T).T).T self.x x_pred K y IKH np.eye(2) - K self.H # Joseph 形式保证协方差正定 self.P IKH P_pred IKH.T K self.R K.T self.P 0.5 * (self.P self.P.T) self.y_prev float(y[0, 0]) return self.x.copy(), self.P.copy()代码里有两个地方值得单独说明。一是_fading_factor用的是上一拍的新息而不是当前拍的。原因是渐消因子必须在预测之前算出来而此时当前拍的观测还没进来所以只能滞后一拍。这是标准做法影响不大但如果机动发生得很突然响应会晚一个采样周期。二是_adapt_R里加了一阶平滑。如果直接把窗口估计值赋给 RR 会在每拍之间跳来跳去增益跟着抖输出反而更差。用 0.9 的旧值加 0.1 的新值相当于给自适应通道加了个低通这是我在实际调试里加上的效果比直接赋值稳定得多。4.3 结果对比与参数扫描的做法我一般跑三组对照纯标准 KF、只开渐消、渐消加 R 自适应。评价指标用位置 RMSE 和速度 RMSE另外看最大误差和收敛时间。在一组包含三次机动的仿真里我观察到的典型量级是这样的标准 KF 的位置 RMSE 在 3 米出头机动段误差能冲到 8 米以上只开渐消能压到 2.2 米左右机动段的峰值明显下降再叠加 R 自适应RMSE 能到 1.8 米上下。这些数字和具体的噪声设置、机动强度强相关只能作为量级参考不要当成标准答案。参数扫描我建议一维一维来不要一上来就网格搜索。先固定自适应关闭把 Q 扫一遍找到稳态误差和响应速度的折中点再打开渐消扫 β观察机动段的峰值误差和稳态噪声的权衡最后打开 R 自适应扫窗口长度。每一步都画出误差曲线光看 RMSE 一个数很容易被平均掉的信息骗过去。5. 参数调优与工程避坑实录5.1 窗口长度、遗忘因子、阈值怎么定窗口长度 N 是最影响手感的参数。N 太小样本协方差估计的方差大R̂ 会剧烈波动N 太大估计滞后机动过去好几拍 R 还没调过来。我的经验法则是窗口覆盖的时间N * dt应该小于你关心的最短机动持续时间的 1/3。比如机动持续 2 秒采样 10 Hz那窗口最多取 6 到 7 拍。如果机动持续时间很长、噪声又比较平稳可以放大到 20 到 50。前面代码里默认 20是个偏保守的起点。遗忘因子 b 或 ρ 控制记忆长度。b 越接近 1等效记忆越长。常用范围是 0.95 到 0.995。我的习惯是从 0.98 开始如果发现滤波器对突变的响应太慢就往下调如果稳态抖动明显就往上调。NIS 阈值建议按 99% 置信度取自由度 1 时约 6.63自由度 2 时约 9.21。实际使用的时候不要只用单拍判断连续 3 拍里有 2 拍超限才触发能有效降低误触发率。注意所有自适应参数都有耦合。把 R 的下限设得太小滤波器会过度相信观测一旦遇到野值就会被带偏设得太大自适应就形同虚设。r_min 和 r_max 这两个边界要根据传感器的物理特性来定不要随便填。5.2 数值稳定性几个不能省的写法自适应滤波比标准 KF 更容易出数值问题因为协方差矩阵在动态调整正定性随时可能被破坏。我总结了几条硬性要求。第一协方差更新必须用 Joseph 形式也就是P (I-KH)P⁻(I-KH)ᵀ KRKᵀ而不是简化的P (I-KH)P⁻。后者在增益接近最优时理论上等价但数值上会累积误差跑几千拍之后 P 可能失去正定性滤波器直接崩。第二每一拍结束都做一次对称化P (P Pᵀ)/2。矩阵乘法在浮点运算下会引入微弱的不对称单次看不出来长期累积会让 Cholesky 分解失败。第三求逆用 solve 或者 Cholesky 分解不要显式算inv(S)。S 是正定对称的用 Cholesky 分解S LLᵀ然后分两次三角求解数值精度和速度都更好。状态维度高的时候这个差别非常明显。第四如果状态维度超过 10 或者观测精度差异极大考虑平方根滤波或者 UD 分解滤波。它们传播的是协方差的因子而不是协方差本身条件数好得多。代价是实现复杂度上一个台阶一般场景用不上。5.3 常见问题速查表下面这张表是我这些年遇到过的典型现象和对应排查方向基本上是照着现象去查就能定位。现象最可能的原因排查与处理估计持续偏离真值P 反而越来越小Q 太小滤波器过度自信增大 Q或开启渐消因子稳态输出抖动明显变大渐消因子在稳态被误触发提高 β或给 λ 加上限新息均值明显不为零存在未建模的系统偏差扩维估计偏差或做零偏标定R 估计出现负值或剧烈跳变窗口太短、野值污染、P⁻ 高估加正定投影与上下限加一阶平滑机动段响应慢半拍渐消基于上一拍新息缩短窗口或引入机动检测提前触发运行几千拍后结果崩坏协方差失去正定性改 Joseph 形式加对称化自适应开启后反而不如固定参数反馈环参数过激降低自适应速率加触发门限6. 不同场景下的取舍与几点个人体会组合导航里的 AKF 和目标跟踪里的 AKF用法差别挺大。卫星定位的观测噪声变化是环境驱动的进隧道、过城市峡谷噪声会成倍恶化但系统动态本身是连续的。这种场景我倾向于只调 R而且一定要设上限否则一次严重遮挡会把 R 估到极大出了遮挡区之后要很久才能恢复。目标跟踪正好相反传感器是稳定的变的是目标运动模式所以主调 Q 或者用渐消因子响应速度是第一位。电池状态估计又是另一种情况。模型参数随温度和老化缓慢漂移变化时间尺度是小时级别这时候遗忘因子要取很大接近 1窗口也要长。用目标跟踪那套快节奏参数去调电池只会让估计值上下乱跳。最后分享几个我在调试时形成的习惯。第一永远先跑标准 KF 作为基线把它的误差曲线存下来后面所有自适应改动都跟它比不然很容易陷入“感觉变好了”的自我暗示。第二把新息序列和 NIS 序列画出来看这两个曲线几乎能解释所有异常比盯着最终误差曲线有用得多。第三自适应模块一定要能一键关掉出问题的时候先关掉确认是自适应引入的还是基础滤波本身就有问题这个排查顺序能省掉大量时间。第四别迷信复杂度我在一个车载定位项目里试过 IMM效果比单模型加强跟踪只好了一点点但计算量和调试成本翻了几倍最后还是退回了简单方案。