Kalman 滤波 · Zhejiang University RoboMaster Vision

PnP 解出的位姿是逐帧独立的：带噪声，不含速度，也不知道下一帧目标会在哪。自瞄需要的恰恰是”弹丸飞到时目标在哪”，这中间的差距由滤波填上。本文讲卡尔曼滤波在这条链路上做什么、装甲板跟踪的状态该怎么设计、两个噪声矩阵各自代表什么，以及实现时最容易出错的地方。

文中的参数名、默认值和节点名来自视觉组在用的 25 哨兵自瞄开源，滤波代码集中在 autoaim_predictor 包，参数写在 armor_tracker_params.yaml 和 params.yaml。14-自瞄框架讲过这些参数怎么调，本文补的是它们背后的模型。参数值随版本变动，读的时候以仓库里的当前代码为准。

滤波要补上什么

12-相机标定与位姿解算的产物是每帧一个 tvec，即装甲板相对相机的 $(X, Y, Z)$ 。把它直接送给云台，会暴露四个缺口。

观测带噪声。 噪声源在角点：灯条过曝、轻微形变、网络输出抖动，都会让四个角点在像素级晃动。横向误差不严重， $\Delta X \approx Z\,\Delta u / f$ ，在 $f = 1500$ px、 $Z = 5$ m 处 1 px 只对应 3 mm。深度是另一回事。单块装甲板靠物理宽度 $W$ 与像素宽度 $w$ 定深度， $Z = fW/w$ ，微分得到

$\Delta Z \approx \frac{Z^2}{f W}\,\Delta w$

小装甲板 $W = 0.135$ m，同样取 $f = 1500$ px， $Z = 2$ m 时 1 px 对应 2 cm， $Z = 5$ m 时涨到 12 cm。深度误差按距离平方增长，且始终远大于横向误差，这一条决定了后面观测噪声矩阵不能三个轴填同一个数。单块板解出的 yaw 更差：四个角点近似共面、张角又小，属于病态量，抖几度是常态。

速度测不到。 延迟补偿要知道目标往哪动、多快。两帧差分能算出速度，但会把噪声放大 $1/\Delta t$ 倍， $\Delta t = 20$ ms 就是 50 倍，深度上 12 cm 的抖动会变成 6 m/s 的假速度。

必须往前推。 从曝光到弹丸出膛有处理延迟和执行延迟，出膛后还有飞行时间，合计几十到上百毫秒。这段时间没有观测，位置只能靠模型外推。

会丢帧。 遮挡、目标切换、检测漏检都会让某几帧拿不到观测，此时仍要给出目标位置，对应 14-自瞄框架里的 TEMP_LOST 状态。

滑动平均只能处理第一个缺口，代价是引入滞后，而且给不出速度，也给不出”当前估计有多可信”。卡尔曼滤波同时维护一个运动模型和一份不确定度，按双方的不确定度加权融合观测，把速度作为状态一并估出来，没有观测时还能只跑预测。

flowchart LR
    pnp["PnP 观测<br/>x, y, z"] --> kf["卡尔曼滤波<br/>状态 + 协方差"]
    model["运动模型<br/>匀速 / 整车圆周"] --> kf
    kf --> est["当前状态估计<br/>位置 + 速度"]
    est --> pred["按预测时长外推"]
    pred --> ballistic["弹道解算与开火判断"]

需要的数学准备

推导全程是矩阵运算。公式看不懂，多半卡在线性代数而不是卡尔曼本身。要用到的不多：

矩阵乘法、转置、求逆，以及分块矩阵怎么相乘；
协方差矩阵的含义。对角线是各状态量的方差，非对角线是它们之间的相关性。理解后一半，才能明白为什么修正位置会顺带修正速度；
一维正态分布的均值与方差。

补几何直观可以看 3Blue1Brown 的线性代数系列：线性代数的本质（Bilibili）。

从一维开始：两个不准的数怎么合并

先把矩阵放一边。要测一块装甲板的距离，手里有两个数：模型推出来的 $x_1$ ，不确定度 $\sigma_1$ ；PnP 测出来的 $x_2$ ，不确定度 $\sigma_2$ 。两个都不准，怎么合成一个更准的？

两者都服从正态分布且相互独立时，最优融合是

$\hat{x} = \frac{\sigma_2^2\,x_1 + \sigma_1^2\,x_2}{\sigma_1^2 + \sigma_2^2}, \qquad \frac{1}{\hat{\sigma}^2} = \frac{1}{\sigma_1^2} + \frac{1}{\sigma_2^2}$

权重按方差的倒数分配，谁更确定谁的话更算数。融合后的 $\hat{\sigma}$ 同时小于 $\sigma_1$ 和 $\sigma_2$ ，两个不准的数合起来比其中任何一个单独用都准。

把第一个式子改写成增量形式：

$\hat{x} = x_1 + K\,(x_2 - x_1), \qquad K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}$

卡尔曼滤波的全部思想就在这里：先信模型，再用观测与模型之差去修正，修正多少由双方的不确定度决定。 $K$ 即卡尔曼增益，取值在 0 到 1 之间。模型很准（ $\sigma_1 \to 0$ ）时 $K \to 0$ ，观测被忽略；观测很准（ $\sigma_2 \to 0$ ）时 $K \to 1$ ，直接采信观测。

从这里到完整的卡尔曼滤波只差两步：把标量换成向量、方差换成协方差矩阵；补上”模型往前走一步，不确定度会变大”这个环节。

五个公式

系统由两个方程描述。状态转移方程说明状态如何随时间演化，观测方程说明状态如何被测到：

$x_k = A\,x_{k-1} + w_k, \qquad w_k \sim N(0,\,Q)$

$z_k = H\,x_k + v_k, \qquad v_k \sim N(0,\,R)$

$A$ 是状态转移矩阵， $H$ 是观测矩阵，负责把状态映射到能测量的量。 $w_k$ 是过程噪声，代表模型不完美； $v_k$ 是观测噪声，代表传感器不准。协方差矩阵 $Q$ 和 $R$ 描述这两种噪声的大小，也是工程上真正需要反复确定的东西。

每来一帧，滤波器跑一次预测、一次更新。预测部分（上标 $-$ 表示还没用上本帧观测）：

$\hat{x}_k^- = A\,\hat{x}_{k-1}, \qquad P_k^- = A\,P_{k-1}\,A^{\mathsf{T}} + Q$

更新部分：

$K_k = P_k^-H^{\mathsf{T}}\left(H\,P_k^-H^{\mathsf{T}} + R\right)^{-1}$

$\hat{x}_k = \hat{x}_k^- + K_k\left(z_k - H\,\hat{x}_k^-\right)$

$P_k = \left(I - K_kH\right)P_k^-$

五个式子各自的作用值得逐个看。

$\hat{x}_k^- = A\hat{x}_{k-1}$ 按运动模型把状态往前推一步。它不需要观测，所以丢帧时这一步照样能跑。

$P_k^- = AP_{k-1}A^{\mathsf{T}} + Q$ 把不确定度也往前推。前一项是原有不确定度经模型传播的结果，后一项 $+Q$ 承认模型不可能完美，每推一步就要多加一份不确定度。缺了它，滤波器会越来越自信，最终彻底无视观测。

$z_k - H\hat{x}_k^-$ 是观测与预测之差，称残差或新息。它是滤波器唯一能获得的新信息，调试时最该盯的量也是它。

$K_k$ 是增益。对照一维情形， $P_k^-$ 相当于 $\sigma_1^2$ ， $R$ 相当于 $\sigma_2^2$ ；分母 $HP_k^-H^{\mathsf{T}} + R$ 是残差的协方差，通常记作 $S$ ，后面判断异常观测要用。

$P_k = (I - K_kH)P_k^-$ 让不确定度在吸收观测后下降。整个循环就是 $Q$ 不断把 $P$ 撑大、观测不断把 $P$ 压小，两者平衡后增益趋于稳定。

稳态增益只取决于 $Q$ 与 $R$ 的比例。两者同乘 10 不改变滤波行为，因此调参时固定一个、只动另一个就够了。

装甲板常速模型

最简单的可用模型：假设装甲板匀速直线运动，只跟踪它的位置。状态取位置和速度，观测就是 PnP 的三个坐标：

$x = \begin{bmatrix} x & y & z & v_x & v_y & v_z \end{bmatrix}^{\mathsf{T}}, \qquad z = \begin{bmatrix} x & y & z \end{bmatrix}^{\mathsf{T}}$

$A = \begin{bmatrix} I_3 & \Delta t\,I_3 \\ 0 & I_3 \end{bmatrix}, \qquad H = \begin{bmatrix} I_3 & 0 \end{bmatrix}$

速度不在观测里，靠 $P$ 的非对角元素被间接估出来：位置与速度相关，修正位置时速度跟着被修正。滤波器比两帧差分强，就强在这里。

坐标系要先转对

滤波不能在相机系里做。相机装在云台上，云台一动，静止的目标在相机系里也在动，滤波器会把云台运动当成目标速度，预测随之跑偏。正确做法是先用下位机回传的云台姿态，把 PnP 结果转到一个近似惯性的坐标系（通常是云台底座系或 odom 系），在那里滤波，出了结果再转回去算角度。这个变换由自瞄控制器负责，姿态或外参出错的典型表现是云台一动预测就飘。

本文统一取水平面内 $x$ 向前、 $y$ 向左、 $z$ 向上的车体约定，哨兵自瞄的实际轴向定义以代码为准。

dt 取时间戳差

$A$ 里的 $\Delta t$ 必须是本帧与上一帧图像时间戳之差，不能写成 1.0 / fps。检测耗时本身就在波动，哨兵自瞄的耗时记录里神经网络推理占 15~30 ms，再叠加丢帧，实际间隔和标称值能差出一倍。用固定值会让速度估计带上系统性偏差，且帧率一变表现就忽好忽坏。时间戳要取图像采集时刻，不是处理完成时刻。

Q 由加速度推出来

$Q$ 描述匀速假设错得有多厉害，而模型误差的来源正是目标真实的加速度，所以它可以推导，不必凭感觉填。

设一帧内加速度是常量随机量 $a \sim N(0, \sigma_a^2)$ ，它给位置带来 $\tfrac{1}{2}a\Delta t^2$ 、给速度带来 $a\Delta t$ ，单个轴上于是有

$Q_{\text{axis}} = \sigma_a^2 \begin{bmatrix} \Delta t^4/4 & \Delta t^3/2 \\ \Delta t^3/2 & \Delta t^2 \end{bmatrix}$

这样只剩一个待定参数 $\sigma_a$ ，量纲是 m/s²，可以凭物理量级估：步兵平移的加速度在几 m/s²，取 5 左右作起点是合理的。 $\Delta t$ 变化时 $Q$ 自动跟着变，不用另行处理。

实际代码里的 $Q$ 常常是手写的对角矩阵，和上式形式不同。哨兵自瞄的 armor_tracker_params.yaml 里，kf_main_observing_armor 的位置过程噪声（2e+1）比速度项（1e-0）大一个量级，而按上式在 $\Delta t = 0.02$ s 时位置项应远小于速度项。这类数值是把模型误差、时间戳误差、坐标变换误差一起塞进位置项后调出来的经验值，照抄可以作起点，但它不是从运动模型推出来的，换了帧率或坐标系未必还合适。

参数名里的 main observing armor 指主观测装甲板，说明项目里不止一组滤波器：主观测板一组，整车观测器一组，前哨站观测器（outpost_observer）又是一组。动手前先确认改的是哪一组。

R 直接测

$R$ 不用猜。把装甲板固定在典型距离上，录 200 帧以上的 PnP 输出，算 $x, y, z$ 各自的标准差，平方后填进 $R$ 的对角线。五分钟就能做完，比盲调有效得多。

测出来的深度项一定明显大于横向项，与前面 $Z^2/(fW)$ 的估算一致。更进一步可以让 $R$ 随距离变化，按 $Z^2$ 缩放深度项，近距离锁得紧、远距离更依赖模型，比固定值稳。

$R$ 属于传感器：标定和检测不变，它就不该动；要改滤波行为请动 $Q$ 。把 $R$ 当平滑强度来调，等于虚报传感器精度，静止时看着舒服，目标一机动必然滞后。

最小实现

用 Eigen 写一个通用的线性卡尔曼滤波，核心就是那五个式子：

#include <Eigen/Dense>

// N 维状态、M 维观测的线性卡尔曼滤波
template <int N, int M>
class KalmanFilter
{
public:
  using StateVec = Eigen::Matrix<double, N, 1>;
  using StateMat = Eigen::Matrix<double, N, N>;
  using MeasVec = Eigen::Matrix<double, M, 1>;
  using MeasMat = Eigen::Matrix<double, M, M>;
  using ObsMat = Eigen::Matrix<double, M, N>;

  void predict(const StateMat & A, const StateMat & Q)
  {
    x_ = A * x_;
    P_ = A * P_ * A.transpose() + Q;
  }

  // 返回残差，调用方可以据此做异常门限
  MeasVec update(const MeasVec & z, const ObsMat & H, const MeasMat & R)
  {
    MeasVec y = z - H * x_;                   // 残差（新息）
    MeasMat S = H * P_ * H.transpose() + R;   // 残差协方差
    Eigen::Matrix<double, N, M> K = P_ * H.transpose() * S.inverse();
    x_ += K * y;
    P_ = (StateMat::Identity() - K * H) * P_;
    return y;
  }

  StateVec x_ = StateVec::Zero();
  StateMat P_ = StateMat::Identity();
};

常速模型对应的 $A$ 与 $Q$ ：

Eigen::Matrix<double, 6, 6> makeA(double dt)
{
  auto A = Eigen::Matrix<double, 6, 6>::Identity().eval();
  A(0, 3) = dt;
  A(1, 4) = dt;
  A(2, 5) = dt;
  return A;
}

Eigen::Matrix<double, 6, 6> makeQ(double dt, double sigma_a)
{
  const double t2 = dt * dt, t3 = t2 * dt, t4 = t3 * dt;
  auto Q = Eigen::Matrix<double, 6, 6>::Zero().eval();
  for (int i = 0; i < 3; ++i) {
    Q(i, i) = t4 / 4;
    Q(i, i + 3) = t3 / 2;
    Q(i + 3, i) = t3 / 2;
    Q(i + 3, i + 3) = t2;
  }
  return Q * sigma_a * sigma_a;
}

初始化容易被忽略。第一帧拿到观测后，位置直接填观测值，速度填 0，但 $P$ 的速度部分要给一个足够大的初值，例如 $(10\ \text{m/s})^2$ 。速度初值本来就是瞎猜的， $P$ 必须如实反映这一点，否则滤波器会死守错误的零速度，收敛很慢。哨兵自瞄的 max_converging_frames（车 10 / 前哨站 15）就是为此存在：跟踪器刚看到目标时进入 CONVERGING，攒够这么多帧才转 TRACKING，因为收敛之前的结果不可信，先不开火。

小陀螺与整车观测：为什么要用 EKF

目标打小陀螺时常速模型会崩。装甲板此时做圆周运动，向心加速度 $a = \omega^2 r$ ，按哨兵自瞄的 initial_radius = 0.26 m（常规步兵两装甲板间距）计算， $\omega = 10$ rad/s 对应 26 m/s²，远超平移时的几 m/s²。把这么大的加速度塞进过程噪声，相当于告诉滤波器模型基本没用，增益被推到接近 1，输出退化成跟着观测抖。

更根本的问题是跟错了对象。小陀螺时四块装甲板轮流出现，板的位置在跳变，而整车中心是稳的。该跟的是车，不是当前看到的那块板。

整车状态把车体中心、朝向、转速和半径一起放进去：

$x = \begin{bmatrix} x_c & v_{x} & y_c & v_{y} & z_c & v_{z} & \theta & \omega & r \end{bmatrix}^{\mathsf{T}}$

观测仍是 PnP 给出的装甲板位置，外加它的 yaw：

$z = h(x) = \begin{bmatrix} x_c - r\cos\theta \\ y_c - r\sin\theta \\ z_c \\ \theta \end{bmatrix}$

三角函数一出现， $h$ 就不再是矩阵乘法，线性卡尔曼的前提没了。扩展卡尔曼滤波（EKF）的做法是在当前估计点把非线性函数一阶展开，用雅可比矩阵代替 $A$ 和 $H$ ：

$F = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1}}, \qquad H = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_k^-}$

五个公式的结构不变，只有两处替换：预测状态用 $f(\hat{x})$ 而非 $A\hat{x}$ ，残差用 $z - h(\hat{x}^-)$ 而非 $z - H\hat{x}^-$ ；协方差传播和增益计算里的 $A$ 、 $H$ 换成 $F$ 、 $H$ 。

上面这个 $h$ 的非零偏导只有几项，其余为零：

偏导	值
$\partial x_a / \partial x_c$ ， $\partial y_a / \partial y_c$ ， $\partial z_a / \partial z_c$	$1$
$\partial x_a / \partial \theta$	$r\sin\theta$
$\partial y_a / \partial \theta$	$-r\cos\theta$
$\partial x_a / \partial r$	$-\cos\theta$
$\partial y_a / \partial r$	$-\sin\theta$
$\partial \theta_m / \partial \theta$	$1$

手推一遍再和代码对照，是检查 EKF 实现最有效的办法。雅可比写错的表现是大部分时候能用、某些角度就发散，单看现象很难查。

这套状态还顺带改善了 yaw 病态的问题：半径 $r$ 在线估计，四块板的观测共同约束同一个中心，单块板 yaw 的抖动被多帧、多板的信息摊薄。哨兵自瞄用 enter_antispin_palstance / exit_antispin_palstance（2.5 / 1.0 rad/s）以迟滞方式判断何时切到整车圆周模型，用 switch_armor_angle（1.0 rad）决定何时更换主观测板，都是围绕这套状态设计的。前哨站转速高且稳定，项目里交给 outpost_observer 单独处理，参数独立。

实现 EKF 有两处细节必须处理。

角度残差要归一化。 $\theta$ 的残差必须折回 $[-\pi, \pi]$ 。估计值 3.14、观测值 −3.14 实际只差 0.003 rad，不归一化会算成 6.28 rad 的巨大残差，一帧就能把滤波器打飞。

inline double normalizeAngle(double a)
{
  while (a > M_PI) a -= 2 * M_PI;
  while (a < -M_PI) a += 2 * M_PI;
  return a;
}

换板时要补偏移。 主观测板切到相邻一块， $\theta$ 的观测会跳约 $\pi/2$ 。这不是目标转了 90°，是观测对象换了。正确处理是在换板时给状态里的 $\theta$ 加上对应偏移，而不是让滤波器把这个跳变当成真实运动吸收进去。

从状态估计到开火点

滤波器给出的是当前时刻的状态，开火需要的是弹丸到达时刻的位置。把状态按预测时长 $t_p$ 往前外推，只跑预测、不做更新：

$\hat{x}(t + t_p) = f\big(\hat{x}(t),\, t_p\big)$

常速模型下就是位置加上速度乘 $t_p$ ；整车模型下是把 $\theta$ 推进 $\omega t_p$ ，再算那一刻哪块板正对我方，配合 antispin_shoot_angle（默认 0.35 rad）判断是否落在开火窗口内。

$t_p$ 由三段构成：图像延迟（采集到处理完）、执行延迟（指令下发到弹丸出膛）、弹丸飞行时间。哨兵自瞄把前两段显式相加，写作 now() - img_time + control_to_fire_time，其中 control_to_fire_time_armor 与 control_to_fire_time_buff 默认 0.03 / 0.047 s。飞行时间取决于预测点的距离，而预测点又取决于飞行时间，因此放在弹道解算里迭代。

外推会把速度误差线性放大：速度估计偏 0.3 m/s，外推 150 ms 就是 4.5 cm 的落点偏差。弹速越低、延迟越大，对速度估计精度的要求越高。这也是 bullet_speed（默认 24.0 m/s，运行时由电控经串口发来）必须与实测一致的原因，它错了，所有延迟补偿一起错。

预测只能补偿已知的延迟和已建模的运动。目标突然变向、突然加减速，预测必然打空，调参解决不了这件事。

确定参数与排查问题

顺序上先定 $R$ ，再调 $Q$ ，一次只动一个。

$R$ 按前面的方法实测填入，之后不再动。 $Q$ 用几个场景交叉验证：

场景	看什么	偏离时的表现
静止目标	估计位置的抖动幅度	$Q$ 偏大，观测噪声被原样放过来，输出跟着抖
匀速横移	估计位置相对真实位置的滞后	$Q$ 偏小，滤波死守模型，跟不上机动
突然变向	恢复跟上所需的帧数	$Q$ 偏小恢复慢，偏大平时不干净

平滑和跟随此消彼长，没有同时最优的取值，只有针对具体场景的折中。

判断参数是否合适，看残差比看轨迹曲线更直接。参数合适时残差序列应当零均值、看不出趋势、幅度大致落在 $\pm 3\sqrt{\operatorname{diag}(S)}$ 以内。残差持续同号说明模型有系统性偏差，优先查坐标变换、时间戳和 $\Delta t$ ；残差幅度明显超出 $S$ 给出的范围，说明 $Q$ 或 $R$ 偏小。把残差和 $S$ 一起发出来画曲线，调参才有依据。

PnP 偶尔会因误检或角点错乱吐出离谱的值，用马氏距离挡一道：

$d^2 = y^{\mathsf{T}}S^{-1}y$

$y$ 是残差。三维观测下 $d^2$ 服从自由度 3 的卡方分布，95% 分位点 7.81，99% 分位点 11.34，超过阈值就跳过这次更新、只保留预测。代价是必须限制连续跳过的帧数，否则目标真的变向时，滤波器会一直拒绝正确的观测。

按现象定位问题时可以查下表，前几条在上文已经展开，后几条是实现时另外几个常见疏漏：

现象	原因	处理
速度估计有系统偏差，帧率波动时忽好忽坏	$\Delta t$ 用了固定帧率	改用图像时间戳差，取采集时刻
云台一动预测就飘	在相机系里滤波	先转到云台底座系或 odom 系
$\pm\pi$ 附近偶发发散	角度残差没归一化	残差折回 $[-\pi, \pi]$
每次换板预测点跳一下	换板没补角度偏移	给 $\theta$ 加对应偏移
收敛慢，前期预测不可信	$P$ 的速度项初值太小	给大初值，并等够 `max_converging_frames`
静止时好看，机动明显滞后	拿 $R$ 当平滑强度调	$R$ 由传感器定，改行为动 $Q$
锁定新车时预测点从旧车位置飞过来	目标切换没重置	换目标必须重建滤波器
长时间只预测不更新，输出越来越离谱	$P$ 持续膨胀	限制 `max_temp_lost_frames`（车 10 / 前哨站 30），超时转 `LOST`
数值差 1000 倍，调什么都不对	单位混用	全流程统一 SI，PnP 的 mm 在入口换成 m
长时间运行后突然发散	$P$ 失去对称正定	每步强制 $P \leftarrow (P + P^{\mathsf{T}})/2$ ，必要时改 Joseph 形式更新

练习与资料

公式推导推荐 DR_CAN 的卡尔曼滤波器系列，从递归思想讲到 EKF，是中文资料里讲得最完整的一套：卡尔曼滤波器 01 · 递归算法（Bilibili）。建议跟着手推一遍，尤其是增益 $K$ 的推导，即最小化后验估计误差协方差的迹。最后几讲的 EKF 不要跳过，整车观测器就建在上面。

只看推导建立不起手感，写两个小程序补上：

一维仿真。造一个匀速运动的目标，加正态噪声当观测，实现完整的预测与更新循环，把真值、观测、估计画在一张图上。再把 $Q$ 分别放大和缩小 100 倍，各看一遍曲线形状。
接实际数据。录一段静止装甲板的 PnP 输出，先实测出 $R$ ，再跑常速模型，对比滤波前后的抖动幅度。

做完这两个练习再去读 autoaim_predictor。读代码的顺序建议是：先找滤波器类，对着五个公式认出预测和更新；再看状态向量的定义，判断它用的是常速模型还是整车模型；最后翻 armor_tracker_params.yaml，把每个噪声参数对应到 $Q$ 或 $R$ 的具体位置。

看完你需要掌握什么

能说清滤波补上的四个缺口：降噪、估速、外推、丢帧续航；
能默写预测与更新的五个公式，并解释 $+Q$ 和 $(I - KH)$ 各自在做什么；
能为装甲板跟踪写出 $x$ 、 $A$ 、 $H$ ，并说明 $\Delta t$ 怎么取、滤波该在哪个坐标系里做；
能说出 $Q$ 和 $R$ 各自代表什么、 $R$ 怎么实测、为什么不该拿 $R$ 当平滑参数；
能解释小陀螺为什么要换成整车模型，以及 EKF 相对 KF 改动了哪两处；
能按现象定位故障：抖动、滞后、发散、换板跳变，分别先怀疑哪个环节；
能在 armor_tracker_params.yaml 里找到本文提到的每个参数，说出它对应模型的哪一部分。

下一步读 14-自瞄框架，看这些状态和参数在完整工程里如何组织。

参考资料

25 哨兵自瞄开源：本文参数名、默认值与节点名的来源，滤波代码在 autoaim_predictor；
DR_CAN 卡尔曼滤波器系列（Bilibili）：公式推导主线；
站内 12-相机标定与位姿解算（观测从哪来）、14-自瞄框架（参数在整体框架里怎么调）。