参考资料：

大疆预测https://zhuanlan.zhihu.com/p/38745950

卡尔曼滤波http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

预测追踪

由于相机采集数据、视觉算法执行、数据传输、滤波、PID控制，再到实际云台系统的固有延迟，还要考虑子弹的飞行时间，每个环节都存在不同程度的延时，如果不对云台角度做一定的预测，只能始终跟在目标的后面，无法做到实时跟踪并且命中运动目标

运动学建模

预测方法

运动预测是自瞄击打装甲板的关键一步，如果没能预测目标的移动，那么我们的解算输出用于只会跟在目标的屁股后面，使得打出的弹丸始终落后于目标，扑向空气划过一道淡绿色的曲线。同时，运动预测和运动学建模也是目标跟踪领域的一个研究方向，利用此技术能够提出精确的区域提议，使得候选区域数目降低，加快处理速度。

朴素方法：简单物理规律的应用

模型假设：默认目标做匀速直线运动（CV）或匀加速直线运动（CA）。

核心计算：通过两个时刻的位置差计算速度（一阶差分），或进一步通过速度差计算加速度（二阶差分）；用速度/加速度乘以超前预测时间（超前量），即可得到目标未来时刻的位置。

噪声抑制：仅用两帧数据计算易受噪声干扰，可采用多帧滑窗平均（直接平均）或EMA（指数加权平均）等方法，降低噪声影响。

局限性：仅对运动状态变化缓慢的目标有效。短时间内可将任意运动近似为CV/CA，但一旦目标运动状态突变（如突然转向、加速），或预测时间过长，准确率会大幅下降（所有预测方法均有此类约束）。

卡尔曼滤波

【卡尔曼滤波（Kalman Filter）深入解析：从零开始彻底搞懂预测与修正（适合小白，无需数学基础，一步步深入讲解） - CSDN App】https://blog.csdn.net/navsense_support/article/details/146231513?sharetype=blogdetail&shareId=146231513&sharerefer=APP&sharesource=2401_87616439&sharefrom=link

基于匀速模型的卡尔曼滤波运动学建模(KF)

[5.3.2.2](####5.3.2.2. KF及其优化)中已经介绍了运动学建模的基本方法。这里我们以匀速模型（CV）为例，将加速度视为零均值的高斯噪声，利用卡尔曼滤波器融合观测值和预测值，实现运动学建模。这里假定我们已经得到目标在绝对系下的坐标（已经完成深度估计，通过相机到世界系的变换矩阵将目标点转换到绝对参考系下）。

连续时间状态方程（通俗理解）：

目标的位置和速度随时间变化的关系，用公式表示为：位置的变化率等于速度，速度的变化率等于加速度噪声（因为是匀速模型，理想加速度为0，实际的微小变化用噪声表示）。

$\frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} \mathbf{x} \\ \dot{\mathbf{x}} \end{array}\right]=\left[\begin{array}{ll} 0 & 1 \\ 0 & 0 \end{array}\right]\left[\begin{array}{l} \mathbf{x} \\ \dot{\mathbf{x}} \end{array}\right]+\left[\begin{array}{l} 0 \\ 1 \end{array}\right] \omega(t)$

匀速模型的状态空间建模（一阶微分方程组）

服从正态分布的加速度过程噪声，简单说就是目标实际运动中，偏离“绝对匀速”的微小加速度变化，且这种变化平均为0。

因为我们处理的是相机每帧采集的离散数据，需要把上面的连续方程转换成离散形式（用前后两帧的时间差计算）：

$\mathbf{x}_{k+1}=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \mathbf{x}_{k}+\left[\begin{array}{c} \frac{1}{2} \Delta t^{2} \\ \Delta t \end{array}\right] w_{k}$

下一帧的位置 = 当前帧位置 + 当前帧速度 × 两帧时间差；下一帧的速度 = 当前帧速度 + 加速度噪声 × 两帧时间差（位置还会受加速度噪声的二次影响，因为加速度对位置的影响是“速度×时间”，而速度又受加速度影响，所以是\frac{1}{2}×时间差²）。

采样间隔时间（可变），就是相机两帧数据的间隔时间（比如相机每秒拍100帧，\Delta t就是0.01秒）。

离散后的加速度过程噪声，和\omega(t)本质一致，只是适配离散数据。

将上式展开，就得到了我们的过程模型（即预测目标下一时刻状态的模型）：

$\boldsymbol{x}_{k+1}=\boldsymbol{F}_{k} \boldsymbol{x}_{k}+\boldsymbol{\Gamma}_{k} \boldsymbol{w}_{k}, \quad \boldsymbol{w}_{k} \sim N\left(\mathbf{0}_{3 \times 1}, \boldsymbol{Q}_{k}\right)$

$\boldsymbol{F}_{k}$ （状态转移矩阵）：负责用当前状态预测下一时刻状态，核心作用就是实现“位置=位置+速度×时间差”“速度不变”，矩阵形式如下：

$\boldsymbol{F}_{k} = \left[\begin{array}{cccccc} 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right]$

$\boldsymbol{\Gamma}_{k}$ （过程噪声作用矩阵）：负责将3个方向的加速度噪声，转换为对6维状态（3个位置+3个速度）的影响，矩阵形式如下：

$\boldsymbol{\Gamma}_{k} = \left[\begin{array}{ccc} \frac{1}{2} \Delta t^{2} & 0 & 0 \\ 0& \frac{1}{2}\Delta t^{2} & 0 \\ 0 & 0 & \frac{1}{2} \Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t &0 \\ 0 & 0 & \Delta t \end{array}\right]$

而量测模型（即相机等传感器观测目标状态的模型）的获得取决于深度信息的获取：

• 用单目相机通过PnP算深度（z方向距离）：观测误差难建模，因为算法和选点会影响误差。

• 用双目相机或激光雷达：观测模型清晰，误差容易建模（比如双目相机的三角测量，误差规律明确）。

在这里为了方便理解，暂时假定相机系下获得的目标3个坐标（x、y、z）的观测误差相互独立（实际不是，这样会降低模型准确性，得不到最优估计）。

令目标在绝对参考系下的坐标

$\boldsymbol{r_k}=\begin{bmatrix} x_n \\ y_n \\ z_n \end{bmatrix}$ 为观测向量（即传感器实际测到的目标位置），则观测模型为：

$\boldsymbol{z}_{k}=\boldsymbol{H}_{k} \boldsymbol{x}_{k}+\boldsymbol{v}_{k}$

$\boldsymbol{H}_{k}=\left[\begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{array}\right], \boldsymbol{v}_{k} \sim N\left(\mathbf{0}_{3 \times 1}, \boldsymbol{R}_{k}\right)$

通俗理解：传感器观测到的目标位置 = 目标真实位置（从6维状态中提取3个坐标） + 观测噪声，且观测噪声平均为0、有固定波动范围（\boldsymbol{R}_k就是描述观测噪声波动范围的矩阵）。

传感器观测值（3维，仅包含绝对系下的坐标，不直接测速度）。

核心作用是“提取”，从6维状态向量中只拿出3个坐标，忽略3个速度（所以速度对应的列全是0）。

观测噪声，比如相机成像模糊、光线干扰导致的观测位置偏差。

需要从相机系转换到绝对系，步骤如下：

相机系下的观测协方差（假设3个方向误差独立）：

$\boldsymbol{R}^{c}=\left[\begin{array}{ccc} \sigma_{z}^{2} & 0 & 0 \\ 0 & \sigma_{\alpha}^{2} & 0 \\ 0 & 0 & \sigma_{\beta}^{2} \end{array}\right]$

分别是相机系下3个方向观测误差的波动范围平方）

2. 通过k时刻相机系和绝对参考系的旋转矩阵（由IMU数据得到），将相机系的观测协方差转换到绝对系下：

$\boldsymbol{R}_{k}^{n}=\boldsymbol{C}_{c \mid k}^{n} \boldsymbol{R}_{k}^{c} \boldsymbol{C}_{c \mid k}^{n T}$

有同学可能会问，坐标变换不是还有一个平移变换吗？为什么不用考虑进来？这是因为平移变换不会改变协方差阵。通俗理解：方差描述的是数据点的“分散程度”（比如10个观测点偏离真实位置的平均距离），平移只是把所有点一起挪个位置，分散程度没变；而旋转会改变坐标方向，需要重新计算分散程度，所以要用到旋转矩阵。

卡尔曼滤波公式

这样就拥有了量测更新的全部信息，下面看看整个滤波器更新的流程（卡尔曼滤波“黄金六式”，通俗版解释附后）：

“黄金六式”通俗解释：

用前一帧的最优状态（位置+速度），预测当前帧的目标状态（不考虑观测值）。
预测当前帧状态的“不确定性”（比如预测位置可能偏离真实位置多少），同时加入过程噪声的影响。
获取当前帧传感器的观测值（实际测到的目标位置）。
计算“卡尔曼增益”：平衡预测值和观测值的可信度（观测越准，增益越大，越相信观测值；预测越准，增益越小，越相信预测值）。
用观测值修正预测值，得到当前帧的最优状态（既考虑了历史运动规律，又结合了实际观测）。
更新当前帧最优状态的“不确定性”，为下一帧预测做准备。

一轮更新就结束了。你只需要把这些矩阵运算用代码实现，就可以获得自己的运动模型。OpenCV在video/tracking模块下提供了卡尔曼滤波器的实现，详情见cv::KalmanFilter Class Reference；它还提供了包括粒子滤波、增强/无迹卡尔曼滤波等的实现，有需求的同学自行查阅文档。

关于运动学建模，还有一些细节需要注意：

• 异步更新：IMU的采样频率一般为1kHz或500Hz（比如C型开发板的BMI088），而装甲板识别和解算的帧率（100~200Hz）远低于前者。因此，没有新观测值（相机帧）时，仅执行“状态预测+协方差预测”，让目标状态实时更新（插帧平滑）；有新观测值时，再执行完整的6步更新。同时要对协方差矩阵设上限，防止状态偏离真实值越来越远（状态发散）。

• 时间对齐：若没有采用相机+IMU的硬触发，需要对齐两者数据的时间戳，确保相机系到绝对系的位姿变换

准确

至于采用其他的运动学模型（比如匀加速模型），对于滤波器的设计来说，只是过程模型（状态状态转移矩阵和过程噪声作用矩阵）有所不同。为了得到更精确的量测更新，应该根据你使用的解算方法和传感器，对相机成像模型、测距的观测噪声作进一步的分析。

扩展卡尔曼滤波(EKF)

KF仅适用于线性系统（状态方程、观测方程均为线性），而本场景中，装甲板位置需通过“机器人中心坐标+距离+朝向角”推导，存在cos(yaw)、sin(yaw)等非线性项，属于非线性系统。EKF通过“雅可比矩阵线性化非线性模型”，适配该场景，核心逻辑与KF一致（预测+更新），仅新增“雅可比矩阵计算”步骤。

目标的状态（机器人中心位置、速度、装甲板高度、朝向角等）随时间变化的关系，用公式表示为：位置的变化率等于速度，速度/角速度/距离的变化率等于对应噪声（匀速模型下，理想变化率为0，实际微小波动用噪声表示）。

$\frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} x_c \\ \dot{x}_c \\ y_c \\ \dot{y}_c \\ z_a \\ \dot{z}_a \\ yaw \\ \dot{yaw} \\ r \end{array}\right]=\left[\begin{array}{lllllllll} 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]\left[\begin{array}{l} x_c \\ \dot{x}_c \\ y_c \\ \dot{y}_c \\ z_a \\ \dot{z}_a \\ yaw \\ \dot{yaw} \\ r \end{array}\right]+\left[\begin{array}{lllllllll} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \end{array}\right] \omega(t)$

服从正态分布的过程噪声，包含X/Y/Z方向加速度噪声、角速度噪声、距离噪声，代表目标偏离“绝对匀速”的微小波动，平均为0。

离散时间状态方程（适配相机帧数据）

由于我们处理的是相机每帧采集的离散数据，需将连续方程转换为离散形式（用前后两帧的时间差计算）：

$\boldsymbol{x}_{k+1}=\left[\begin{array}{lllllllll} 1 & \Delta t & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & \Delta t & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & \Delta t & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right] \boldsymbol{x}_{k}+\left[\begin{array}{ccccc} \frac{1}{2} \Delta t^{2} & 0 & 0 & 0 & 0 \\ \Delta t & 0 & 0 & 0 & 0 \\ 0 & \frac{1}{2} \Delta t^{2} & 0 & 0 & 0 \\ 0 & \Delta t & 0 & 0 & 0 \\ 0 & 0 & \frac{1}{2} \Delta t^{2} & 0 & 0 \\ 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & \frac{1}{2} \Delta t^{2} & 0 \\ 0 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & \Delta t^{2} \end{array}\right] w_{k}$

下一帧状态 = 当前帧状态（按匀速推导） + 过程噪声影响，核心分量变化如下：

离散状态方程标准形式

$\boldsymbol{x_k}=\begin{bmatrix} x_{c,k} & \dot{x}_{c,k} & y_{c,k} & \dot{y}_{c,k} & z_{a,k} & \dot{z}_{a,k} & yaw_k & \dot{yaw}_k & r_k\end{bmatrix}^T$ ：包含当前帧绝对系下的机器人中心位置、速度，装甲板高度、速度，目标朝向角、角速度，以及中心到装甲板的距离。

核心作用是“用当前状态预测下一时刻状态”，完全匹配匀速模型，矩阵形式与上述离散方程左侧系数矩阵一致，特点为：

位置分量（x_c, y_c, z_a, yaw）对应的行：“1”对应自身，“\Delta t”对应自身速度（如x_c行对应\dot{x}_c列）；

速度/角速度/距离分量对应的行：仅自身位置为“1”（速度/角速度/距离不变，仅受噪声影响）。

负责将5类过程噪声（X/Y/Z加速度、角速度、距离噪声）转换为对9维状态的影响，矩阵形式与上述离散方程右侧系数矩阵一致，核心是“将噪声与状态分量的影响关系量化”（如X加速度噪声仅影响x_c和\dot{x}_c）。

描述过程噪声的波动范围，代码中采用动态调整策略（非固定值），核心逻辑：

位置相关噪声与目标合线速度负相关，速度越快，噪声越大（运动越剧烈，预测不确定性越高）；

朝向角相关噪声与目标合线速度正相关，速度越快，噪声越小:（平移时旋转波动更小）；

距离噪声（r）：固定值（代码中为80.0），因距离波动极小

矩阵为对角矩阵，对角线元素为各状态分量的噪声方差，非对角线元素为0（假设噪声相互独立）。

量测模型（传感器观测与状态的关系）

量测模型描述“传感器观测值”与“目标真实状态”的关系，本场景中传感器（相机）直接观测到的是“装甲板在绝对系下的X/Y/Z坐标”和“朝向角”，需通过非线性关系从9维状态中推导。

量测方程（非线性）：

$\boldsymbol{z}_{k}=\boldsymbol{h}(\boldsymbol{x}_{k})+\boldsymbol{v}_{k}$

$\boldsymbol{h}(\boldsymbol{x}_k) = \begin{bmatrix} x_{c,k} - r_k \times \cos(yaw_k) \\ y_{c,k} - r_k \times \sin(yaw_k) \\ z_{a,k} \\ yaw_k \end{bmatrix}$

通俗理解：

装甲板X坐标：机器人中心X坐标 - 中心到装甲板距离 × 朝向角余弦（沿朝向角反方向）；
装甲板Y坐标：机器人中心Y坐标 - 中心到装甲板距离 × 朝向角正弦；
装甲板Z坐标：直接等于状态中的装甲板高度；
朝向角：直接等于状态中的目标朝向角。

包含相机观测到的装甲板绝对系X/Y/Z坐标、观测朝向角（从相机姿态四元数转换而来）。

包括相机成像模糊、光线干扰导致的位置偏差，以及姿态解算导致的朝向角偏差，平均为0。

描述观测噪声的波动范围，代码中采用动态调整策略，核心逻辑：

与装甲板观测距离成正比，距离越远，传感器误差越大，噪声越大；

朝向角噪声：固定值（代码中为0.02），因朝向角观测精度受距离影响较小。

矩阵为对角矩阵，对角线元素为各观测分量的噪声方差，非对角线元素为0。

EKF关键补充：雅可比矩阵（线性化非线性模型）

EKF的核心是“用雅可比矩阵将非线性观测函数\boldsymbol{h}(\boldsymbol{x}_k)线性化”，替代KF中的线性观测矩阵\boldsymbol{H}_k，同时状态转移函数虽为线性（匀速模型），仍需计算其雅可比矩阵（与状态转移矩阵\boldsymbol{F}_k完全一致）。

：

因状态转移函数为线性，其雅可比矩阵与状态转移矩阵\boldsymbol{F}_k完全相同，作用是“线性化状态转移过程”（本场景中可直接复用\boldsymbol{F}_k）。

：

将非线性观测函数\boldsymbol{h}(\boldsymbol{x}_k)线性化，核心是计算“观测值对各状态分量的偏导数”，矩阵形式如下：

$\boldsymbol{J}_h = \left[\begin{array}{lllllllll} 1 & 0 & 0 & 0 & 0 & 0 & r_k \times \sin(yaw_k) & 0 & -\cos(yaw_k) \\ 0 & 0 & 1 & 0 & 0 & 0 & -r_k \times \cos(yaw_k) & 0 & -\sin(yaw_k) \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \end{array}\right]$

各关键偏导数含义（通俗理解）：

朝向角变化对装甲板X坐标的影响，距离越远、朝向角越大，影响越明显；

中心到装甲板距离变化对X坐标的影响，沿朝向角反方向；

朝向角变化对装甲板Y坐标的影响，与X坐标相反；

距离变化对Y坐标的影响，沿朝向角反方向；

EKF更新流程

EKF流程与KF一致，仅将“观测矩阵\boldsymbol{H}_k”替换为“观测雅可比矩阵\boldsymbol{J}_h”，核心分为“预测阶段”和“更新阶段”，步骤如下：

状态预测：用历史状态推当前帧预测状态

$\boldsymbol{x}_{k}^{-}=\boldsymbol{F}_{k} \hat{\boldsymbol{x}}_{k-1}$

用前一帧的最优状态（9维），通过状态转移矩阵，预测当前帧的目标状态（不考虑观测值）。

协方差预测：量化预测状态的不确定性

$\boldsymbol{P}_{k}^{-}=\boldsymbol{F}_{k} \boldsymbol{P}_{k-1} \boldsymbol{F}_{k}^{T}+\boldsymbol{\Gamma}_{k} \boldsymbol{Q}_{k} \boldsymbol{\Gamma}_{k}^{T}$

预测当前帧状态的“不可信程度”（如预测位置可能偏离真实位置多少），同时加入过程噪声的影响（噪声越大，不确定性越高）。

• 补充：代码中会对“中心到装甲板距离”设范围（0.12~0.4米），防止协方差过大导致状态发散

观测方程：获取当前帧传感器观测值

$zk=h(xk)+vk$

读取相机检测到的装甲板位置、朝向角（4 维观测值），包含观测噪声

卡尔曼增益：平衡预测值与观测值的可信度

$Kk=JhPk−JhT+RkPk−JhT$

计算 “权重系数”，观测越准（Rk越小），增益越大，越相信观测值；预测越准（Pk−越小），增益越小，越相信预测值。

状态更新：用观测值修正预测值，得到最优状态

$^k=xk−+Kk(zk−h(xk−))$

用观测值与 “预测状态推导的观测值” 的差值，修正预测状态，得到当前帧的最优状态（既符合历史运动规律，又贴合实际观测）

协方差更新：量化最优状态的不确定性

$Pk=(I−KkJh)Pk−$

更新当前帧最优状态的 “不可信程度”，为下一帧预测做准备（修正后，不确定性通常会降低）

细节补充

动态噪声调整

根据目标运动状态调整（速度越快，位置噪声越大；平移越快，旋转噪声越小），提升预测精度

根据装甲板距离调整（距离越远，位置噪声越大），适配传感器精度变化

异步更新与时间对齐

异步更新：控制频率（如 1kHz）高于相机帧率（如 100Hz），无新观测值时，仅执行 “状态预测 + 协方差预测”（插帧平滑状态）；有新观测值时，执行完整 6 步更新；
时间对齐：通过时间戳对齐相机与 IMU 数据，确保坐标变换（相机系→绝对系）和Δt计算准确。

特殊场景处理（装甲板跳转 / 短暂丢失）

装甲板跳转（小陀螺模式）：朝向角突变时，直接用观测朝向角更新状态，交换 “中心到装甲板距离”（适配 4 块装甲板场景），避免 EKF 发散；
短暂丢失：连续数帧未匹配到装甲板时，仅执行预测阶段，维持状态；丢失超阈值（如 0.3 秒），重置 EKF，重新初始化追踪目标。

超前量估计

前一小节我们对目标的运动进行了建模，根据得到的运动学模型，我们假定现在已经能够获取目标运动的较为可靠的信息。首先我们需要对所有可能产生滞后和延时的环节进行统计。列表如下：

暂不支持电子表格: Lyuis5RN1hrtkvtmvIscYrhBn45_U2T6io

子弹飞行时间最少将占据2/3以上的时间，因此距离的解算和弹道模型是否准确是超前量估计最重要的问题。可以选择利用下位机的定时器、C++中的chrono库对软件部分的时间开销进行定量测算，然后实时更新需要超前的时间。也可以对“忽略”和“不重要”的项目总耗时进行拟合，而其他的部分还是通过计算得到。最暴力的方法是只考虑子弹飞行时间，将剩下所有开销视作黑箱，通过实弹测试获取数据，把超前量当作距离目标的函数进行拟合。

获取超前量后，根据前面得到的速度，用超前时间乘以速度获取这段时间将发生的位移$\Delta x$，当前时刻的模型输出中加上$\Delta x $得到最终打击位置在绝对系下的坐标。此信息就是我们需要的控制信息，将其转化为云台的pitch和yaw，发送给电控。

如果追求击杀效率，则通信、电机动作、弹链空程的延时都不需要考虑。这时我们假设发射机构和目标之间的弹丸速度极快（想象成水枪！），那么只需要我们的枪管一直指向准确的预计位置，就可以保证“水”始终冲刷目标装甲板。若你对命中率的要求较高，则仍需要考虑上面的因素。

实际上，超前时间的计算依赖与云台和目标的距离(由距离计算弹丸飞行时间)，然而为了得到距离（超前时间*目标速度），我们又需要知道弹丸的总飞行时间，似乎陷入了死循环。上面介绍的方法只是一种近似，不过好在目标的运动速度不是特别快，误差还算可以接收。关于子弹飞行时间的计算，我们将在弹道预测里介绍。

跟踪器

在一帧图像中往往会识别到多个目标，我们需要确认这些装甲板在不同帧之间的继承关系以建立运动模型；对于能量机关的圆拟合、旋转函数拟合我们也想选取多片扇叶计算取平均获得更精确的结果，同样需要追踪每片扇叶的状态。因此，对每个目标进行跟踪是非常有必要的。不过，由于在我们的应用场景下，目标数量较少，且识别算法帧率较高，帧间物体在图像上的位移并不会很大，使用一些简单的跟踪策略即可。并且因为我们在每帧都会进行检测，实际上采用的是track by detection的方法。至于5.4中介绍的目标跟踪算法，我们有别的用途。本小节主要介绍跟踪器的构造和目标切换时的策略。

跟踪器构造

1. 跟踪器初始化（`Tracker::Tracker` + `Tracker::init` + `Tracker::initEKF`）

Tracker::Tracker：作为构造函数，初始化跟踪器基础状态（如初始状态设为 “丢失”LOST）、核心参数（最大匹配距离、最大偏航角差），以及 EKF 所需的状态向量、观测向量等基础数据结构。 .
Tracker::init：接收装甲板检测数据，优先选择 “距离图像中心最近” 的装甲板作为初始跟踪目标，调用initEKF初始化 EKF，同时记录目标 ID（就是指的装甲板上面的数字）、更新装甲板数量，并将跟踪状态切换为 “检测中”DETECTING。
Tracker::initEKF：基于初始跟踪装甲板的位姿（位置、朝向角），计算机器人中心坐标等关键参数，初始化 EKF 的 9 维状态向量（含位置、速度、高度、角速度等），并设置 EKF 的初始状态。

2. 跟踪状态更新（`Tracker::update`）

这是跟踪器的核心函数，包含 EKF 预测、目标匹配、状态修正和状态机切换四大步骤：

EKF 预测：调用ekf.predict()生成目标状态的预测值（如预测位置、速度）。
目标匹配：在检测到的装甲板中，筛选出与当前跟踪目标 ID 一致的装甲板，计算其与预测位置的距离差、偏航角差，找到 “最接近预测值” 的装甲板。
EKF 更新：若匹配装甲板的距离差、偏航角差在阈值内（是上一时刻预测的当前的值和当前观测的），调用ekf.update()用观测值修正预测状态；若未匹配到目标，则保留预测状态。
状态机切换：根据 “是否匹配到目标”“连续匹配帧数”“连续丢失帧数”，在DETECTING（检测中）、TRACKING（跟踪中）、TEMP_LOST（临时丢失）、LOST（丢失）四种状态间切换。

3. 异常处理（`Tracker::handleArmorJump` + 半径约束逻辑）

Tracker::handleArmorJump：针对 “仅存在 1 个同 ID 装甲板，但偏航角突变” 的场景（即装甲板跳变），修正目标朝向角，更新装甲板数量，若位置偏差过大则重置 EKF 状态，避免跟踪漂移。
半径约束逻辑：在update函数中，对 EKF 输出的 “机器人中心到装甲板距离”（状态向量第 8 维）进行范围约束（0.12~0.4 米），防止距离值异常扩散。

4. 辅助计算（`Tracker::orientationToYaw` + `Tracker::getArmorPositionFromState` + `Tracker::updateArmorsNum`）

Tracker::orientationToYaw：将装甲板的四元数姿态转换为偏航角（yaw），并处理偏航角的连续性（避免从 π 突变到 -π）。
Tracker::getArmorPositionFromState：根据 EKF 的预测状态向量（如机器人中心坐标、偏航角、距离），反推装甲板的预测位置，用于与实际检测位置对比匹配。
Tracker::updateArmorsNum：根据跟踪目标的装甲板类型（大 / 小）和 ID，判断目标的装甲板数量（如普通目标 4 个、平衡机器人 2 个、前哨站 3 个），为可视化和后续跟踪提供基础信息。

图片处理失败: Tn5kbBcnFoZvhwxTRLYcfMnan3e Request failed with status code 400

目标切换策略

• 核心需求：应对目标短暂丢失场景（如对方机器人“小陀螺”模式、装甲板击打闪烁（0.1s后亮起）、目标短暂遮挡），需区分“目标丢失”与“控制补帧异步更新”（前者因目标未检测到，后者因控制频率高于采样频率）。

• 基础规则：设置帧阈值，连续多帧未检测到目标则删除跟踪器；即使未检测到目标，仍需执行状态转移，若为短暂丢失，可视为识别帧率下降，插入仅做状态转移的模型更新。

• 能量机关特殊处理：扇叶被击中后，延迟一段时间会变为激活状态，为加速模型收敛，避免重新初始化跟踪器，让该扇叶继承历史跟踪信息。

补充

• ROI结合优化：针对哨兵（运动状态单一，仅1个跟踪器），下一帧仅在ROI区域运行检测算法，传统算法可直接降低时间开销（与图像大小正相关），神经网络可搭配轻量化ROI网络提速。

• 其他机器人ROI生成：对同一号码的装甲板，取四角点四向最大值作为边界，以边界bbox几何中心为参考，将bbox扩大1.5~2倍（确保“小陀螺”模式下所有装甲板可检测）。

• 多装甲板跟踪目的：为后续“反陀螺”功能提供数据支撑，需为同一机器人的多个装甲板分别维护跟踪器。

优化建议

君瞄代码流程图

图片处理失败: KpSOba1AToAihJxGDVGcN7Qcnog Request failed with status code 400

整车构建

第一步：确定整车装甲板数量

通过当前跟踪装甲板的 ID 和类型，判断目标车型（普通机器人 / 平衡机器人 / 前哨站），进而确定装甲板总数，为后续几何分布计算奠定基础。

普通机器人（如 ID 为 1、2、6 等）：tracked_armors_num = NORMAL_4（4 块装甲板，两两对称分布）。
平衡机器人（类型为 “large” 且 ID 为 3/4/5）：tracked_armors_num = BALANCE_2（2 块装甲板，对称分布）。
前哨站（ID 为 “outpost”）：tracked_armors_num = OUTPOST_3（3 块装甲板，均匀分布）。

获取整车核心状态（EKF 估计 + 跟踪数据）

通过 Tracker::update 中的 EKF 预测与更新，得到整车几何计算所需的 6 个核心参数

暂不支持电子表格: Lyuis5RN1hrtkvtmvIscYrhBn45_qj70ge

几何计算反推所有装甲板位置

根据装甲板数量和几何分布规律，逐一计算每块装甲板的 3D 坐标（x, y, z），核心公式基于 “机器人中心 + 极坐标转换”：

(1)通用角度分布计算

每块装甲板的朝向角 = 基准朝向角（yaw） + 均匀分布角度差，公式：

$tmp_yaw = yaw + i * (2 * M_PI / tracked_armors_num)$

（i 为装甲板序号，从 0 到 “装甲板数量 - 1”，确保均匀分布）

(2)分车型坐标计算

4 块装甲板（普通机器人）：两两为一组，上下分布（有高度差 dz），两组距离不同（r1/r2）。

第 1、3 块（上组）：z = za，距离 = r1；第 2、4 块（下组）：z = za + dz，距离 = r2。

坐标公式：

2 块装甲板（平衡机器人）：对称分布，无高度差，距离统一为 r1。

坐标公式：

3 块装甲板（前哨站）：均匀分布，无高度差，距离统一为 r1，且装甲板有固定倾斜角（-0.26 弧度）

坐标公式同平衡机器人，仅在设置装甲板姿态时增加倾斜角

生成可视化标记并显示

将计算出的每块装甲板坐标，封装为 ROS2 可视化消息（Marker::CUBE 红色立方体），同时添加整车位置（绿色球体）、速度（箭头）等辅助标记，最终发布到 /tracker/marker 话题：

初始化标记属性：设置装甲板为红色立方体，尺寸根据类型（小装甲板 0.135m，大装甲板 0.23m）调整。
循环生成标记：为每块装甲板分配唯一 ID，填入计算出的坐标和姿态，加入标记数组。
发布与显示：RViz 订阅 /tracker/marker 话题后，自动渲染所有标记，在屏幕上显示完整的整车装甲板分布。

代码示例

rm_vision