armor_processor

0.自瞄预测逻辑

预测节点接收检测节点发布的目标装甲板信息，根据目标的状态（目标是否丢失/目标是否处于小陀螺/目标是否发生切换）调整预测器状态（追踪/预测/丢失预测/丢失），将目标位姿信息输入预测器，输出目标预测位置后解算出pitch和yaw轴角度并发布消息。

• 预测器
  • 基于两个模型，分别为singer模型、整车模型（还有IMM模型，当前未使用），各模型详解见下一部分Update log。
  • 预测器分为4个状态，分别为Tracking（追踪）、Predicting（预测）、Losting（丢失预测）、Lost（完全丢失）。预测器4个状态的切换条件为：
    • 目标上帧存在，此帧丢失则进入Losting状态；
      • 此状态下，预测器仅对目标当前位置进行状态估计。
    • 目标上帧不存在且当前帧丢失或预测器处于Losting状态的帧数大于等于5，则切入Lost状态；
      • 此状态直接重置预测器。
    • 车辆发生切换或目标未完成初始化（首次识别到），则切入Tracking状态；
      • 此状态对预测器进行初始化，给状态量赋初值。
    • 目标已完成初始化则进入Predicting状态。
      • 此状态下对目标当前位置进行估计后，对目标在延迟时间后（结合发弹延迟、弹丸飞行时间、串口通信延迟、云台响应延迟等）做超前预测。

1.Update log

Date:2023-05-03

从车辆中心的运动状态看，只有各个方向的平动，因此机动模型应用于车辆中心则可规避对小陀螺状态下单个装甲板的预测出现超调的问题，从而提高预测效果的上限。
整车状态预测思路分为前后两个阶段：车辆中心的位置估计和车辆中心的位置预测。

• 对车辆中心建模

  车辆中心和装甲板的位置关系对应于圆心和圆上的点，而PnP解算出的装甲板的姿态则包含装甲板相对于车辆中心的角度信息。
  整车模型如下： \(\begin{cases} x=x_c+r*sin({\omega}t+{\theta}) \\ y=y_c-r*cos({\omega}t+{\theta}) \\ z=z_c \end{cases}\)

  模型中已知$x，y，z，\theta$，我们需要估计的状态量有车辆中心$(x_c,y_c,z_c)$和车辆半径$r$以及车辆旋转角速度$\omega$。

• 基于扩展卡尔曼滤波的车辆中心状态估计:
  • 滤波的状态量 \(X= \begin{bmatrix} x_c \\ y_c \\ z_c \\ r \\ \theta \\ \omega \\ \end{bmatrix}\)
  • 状态转移矩阵 \(F= \begin{bmatrix} 1 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \\ 0 \ \ 1 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \\ 0 \ \ 0 \ \ 1 \ \ 0 \ \ 0 \ \ 0 \\ 0 \ \ 0 \ \ 0 \ \ 1 \ \ 0 \ \ 0 \\ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 1 \ \ dt \\ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 1 \\ \end{bmatrix}\)

    dt为图像两帧之间的时间间隔。

  • 滤波的观测量 \(Z= \begin{bmatrix} x \\ y \\ z \\ \theta \\ \end{bmatrix}\)
  • 测量矩阵 \(H= \begin{bmatrix} 1 \ 0 \ 0 \ \ \ sin\theta \ \ \ 0 \ 0 \\ 1 \ 0 \ 0 \ -cos\theta \ 0 \ 0 \\ 0 \ 0 \ 1 \ \ \ \ \ \ 0 \ \ \ \ \ \ 0 \ 0 \\ 0 \ 0 \ 0 \ \ \ \ \ \ 0 \ \ \ \ \ \ 1 \ 0 \\ \end{bmatrix}\)
  • 测量矩阵的雅克比形式 \(J_H= \begin{bmatrix} 1 \ 0 \ 0 \ \ \ sin\theta \ \ \ rcos\theta \ 0 \\ 1 \ 0 \ 0 \ -cos\theta \ rsin\theta \ 0 \\ 0 \ 0 \ 1 \ \ \ \ \ \ \ \ 0 \ \ \ \ \ \ 0 \ \ \ \ \ \ \ 0 \\ 0 \ 0 \ 0 \ \ \ \ \ \ \ \ 0 \ \ \ \ \ \ 1 \ \ \ \ \ \ \ 0 \\ \end{bmatrix}\)

    首次检测到装甲板目标后会对滤波进行初始化，即对滤波状态量赋初值，此时的圆心直接通过模型计算给出；之后则不断通过滤波的预测+更新步骤迭代收敛中心坐标$(x_c,y_c,z_c)$和车辆半径$r$。
    装甲板发生切换处理：考虑到车辆中心、半径和角速度不变，因此我们只重置滤波状态量中的角度$\theta$，从而使滤波更快收敛。
    车辆发生切换：重置滤波器。

• 基于卡尔曼滤波的车辆中心状态预测:

  将上一阶段的输出量作为singer模型的输入量，对车辆中心进行预测。（singer模型见下面基于‘当前’统计模型（CS model）的卡尔曼滤波算法）

• 目标点预测 上一阶段的输出量即为目标中心的预测点$(x_c,y_c,z_c)$，结合时间延迟量计算当前装甲板的预测旋转角度， \({\theta}_{prediction}={\theta}+{\omega}*{t_{prediction}}\) 代入模型可计算出装甲板预测点， \(\begin{cases} x_{prediction}=x_c+r*sin(\theta_{prediction}) \\ y_{prediction}=y_c-r*cos(\theta_{prediction}) \\ z_{prediction}=z_c \end{cases}\) 车辆相邻装甲板之间旋转角度相对固定$(\frac{\pi}{2})$，因此车辆4块装甲板的预测点为

        for(int ii = 0; ii < 4; ii++)
        {
            x_pred=x_c+r*sin(θ_pred+ii*(PI/2));
            y_pred=y_c-r*cos(θ_pred+ii*(PI/2));
            z_pred_z_c;
        }
    

> 至此，我们已经得到了整个车辆的运动轨迹（车辆中心和4个装甲板中心的位置），接下来我们要从4个预测的装甲板位置中心选出最佳的点进行瞄准打击。\
> 我们相机能看到装甲板的旋转角度范围在0.xrad到2.xrad之间，也是我们打击的有效范围，因此只需加入条件判断出在这个范围内的点即可，然后将坐标转换到相机系下计算出pitch和yaw角度发送给下位机。 * 自动开火逻辑
考虑到控制上滞后的问题，定点打击更合理，我们只需要发送开火标志位给下位机。\
我们的滤波输出包含车辆中心点（变化幅度最小），因此我们的定点选择车辆中心点，当预测的装甲板位置在开火的范围内的时候，我们将此点的角度信息和开火标志位发送给下位机，下位机接收到开火标志位后，控制云台摆动到给定角度位置（实际上小于某一阈值）后开火。

Date:2023-04-02

传统基于单步量测的新息序列不等式

\({V_{k}V_{k}^T}{\leqslant}{r{\cdot}{tr(P_{Kzk})}}={r{\cdot}{tr(E[V_{k}{V_{k}^T}])}}\) 式中r为储备系数（r>=1），tr表示矩阵迹的符号。上式的左端为新息序列的平方，包含实际估计误差信息，右端与新息序列协方差矩阵的迹有关，协方差矩阵的理论值计算如下： \(P_{Kzk}=E[V_{k}V_{k}^T]=H_{k}P_{k|k-1}H_{k}^T+R_{k}\)

Date:2022-12-18

1.小陀螺的规律性首先会考虑其运动模型。
    先考虑原地小陀螺相对简单的情况，单看车辆上一个装甲板的小陀螺轨迹无疑是圆周运动，这时首先想到的是圆的参数方程，但是对于车上的相机而言，其在车辆小陀螺的每个周期内看到的只有四分之一周期内的装甲板信息（而且每个周期还是相同的一段轨迹），这对于考虑整个圆轨迹的运动模型而言势必会造成很大误差，结果不会太理想。
    所以得重新审视小陀螺运动，小陀螺状态下每隔一定周期会出现两个装甲板同时被检测到的情况，在程序逻辑上，每次出现两个装甲板就会切换至新增的（也就是旋转方向上靠后的），因此我们可以根据两次切换的时间差计算周期（实际上近似等于小陀螺旋转周期的四分之一）。此时我们不单看车辆一个装甲板的运动轨迹了，而是相邻的两个装甲板，显然，两个装甲板的轨迹不再是圆（两个装甲板到车辆轴心的距离不一样），看起来更像是椭圆（把圆轨迹方程中的半径r换成长轴a和短轴b即可），好像合理一点了。。。
    但是问题依然没有解决，我们还是纠结如何求车辆整个运动周期内的轨迹。
    我们最终还是把目光聚集到一个装甲板上，并仅考虑一个装甲板在视野内的四分之一周期内的运动轨迹，测试后发现x轴的轨迹近似于一条直线，那么问题就变简单了，我们的模型只是一次函数。
2.确定小陀螺状态下装甲板切换周期
    装甲板切换周期也是我们预测的周期。一般小陀螺状态下转速比较稳定，因此我们多次记录装甲板切换周期，同时剔除掉跳变值，然后取其平均值作为预测周期，以降低异常值的影响。实际上根据装甲板切换周期计算预测周期误差有点大，考虑通过前后装甲板正对时的时间差计算预测周期（弃，不合理）。
3.移动小陀螺
    x方向依然满足一次函数模型，只是每个周期的起点不同，此种情况稍微有点复杂。我们每次预测至少记录当前跟随装甲板的3帧信息，再少会出问题，因此带来的问题就是前三帧没有预测，这就会浪费掉一部分击打的机会。考虑对前后两个周期内的直线斜率进行继承，改变的是一次函数中的常数项。
    从小陀螺的轨迹图中可以看出，每个周期的起点之差几乎是恒定的（当目标往某一方向平移+小陀螺），我们可以记录最近两个周期的起点之差，进而估算出当前周期的起点值，上述模型中我们需要估计的状态量有车辆中心$(x_c,y_c,z_c)$和车辆半径$r$，
2.位置/速度繼承
    通過繼承上一階段的位置/速度來加快當前時刻曲線的收斂。
3. 反向速度/位置

Date:2022-11-27

说明：此算法是对其他模型（包括CV、CA、CT、CTRV、CTRA等运动模型）的融合，故主要介绍算法融合思路。 假设有n个模型，当前时刻处于各运动模型的概率取决于上一时刻的状态，算法用马尔可夫概率转移矩阵$\Pi$来表示各模型之间的概率转移，即: $$
\Pi=
\begin{bmatrix}
    \pi_{11} & \dots  & \pi_{1n} \\
    \vdots & \ddots & \vdots \\
    \pi_{n1} & \dots  & \pi_{nn} 
\end{bmatrix} \\ $$ 其中$\pi_{ij}$表示模型$i$到模型$j$的转移概率，显然 $$
\sum_{j=0}^{n}{\pi_{ij}}=1 $$

已知目标上一时刻在第i个模型下的状态后验$\hat{x}{k-1}^i$、协方差后验$P{k-1}^i$、模型概率$\mu_{k-1}^i$和马尔可夫概率转移矩阵$\Pi_{k-1}^i$。

IMM算法步骤：

1）模型交互；
2）滤波预测更新；
3）模型概率更新；
4）模型融合。

各步骤实现细节：

1）输入交互 $$
C_{k-1}^j=\sum_{i=1}^n{\Pi_{ij}}{\mu_{k-1}^i} \\

\mu_{k-1}^{ij} = \frac{\Pi_{ij}\mu_{k-1}^i}{C_{k-1}^{j}} $$

$$ \hat{X}{k-1}^{j} = \sum{i=1}^n{\hat{X}{k-1}^j}\mu{k-1}^{ij} \

P_{k-1}^{j}=\sum_{i=1}^n{\mu_{k-1}^{ij}{[P_{k-1}^i+(\hat{X}_{k-1}^i-\hat{X}_{k-1}^j)(\hat{X}_{k-1}^i-\hat{X}_{k-1}^j)^T]}} $$
2)滤波预测更新 $$
Predict
\begin{cases}
    \hat{x}_{k}^{}=J_{k-1}^{j}\hat{X}_{k-1}^{j} \\
    P_{k}^{}=J_{k-1}^{j}P_{k-1}^{j}(J_{k-1}^{j})^T+Q_{k-1}^{j}
\end{cases}  $$
模型中的状态转移方程进行泰勒展开取一阶项得到雅可比矩阵J。 $$
Update
\begin{cases}
    K_{k}^{}=P_{k}^{j}(H_{k}^{j})^T(H_{k}^jP_{k}^{j}(H_{k}^{j})^T+P_{k}^{j})^{-1} \\
    \hat{x}_{k}^{}=\hat{x}_{k}^{}+K_{k}^{}(z_{k}-H_{k}^{j}\hat{x}_{k}^{}) \\
    P_{k}^{}=P_{k}^{}-K_{k}^{}H_{k}^{j}P_{k}^{}
\end{cases} $$
3)模型概率更新 计算测量值与预测值之间的残差： $$
V_{k}^{j}=z_{k}-H\hat{X}_{k|k-1}^{0,j}  $$ 残差的协方差矩阵为： $$
S_{k}^{j}=HP_{k|k-1}^{j}H^T+R_{k}^{j} $$ 计算各模型的似然值： $$
\Lambda_{k}^{i}=\frac{e^{-\frac{(V_{k}^{i})^T{(S_{k}^{i})^{-1}V_{k}^{i}}}{2}}}{\sqrt{(2\pi)^{M}\lVert S_{k}^{i} \rVert}} \\ $$
    其中M为测量模型维数。 更新模型概率 $$
C_{k}=\sum_{i=1}^{n}{\Lambda_{k}^{i}C_{k}^{i}} \\
\mu_{k}^{i}=\frac{\Lambda_{k}^{i}C_{k}^{i}}{C_{k}} $$

4)融合各模型的状态`计和协方差 $$
\hat{x}_{k}=\sum_{i=1}^{n}{\hat{X}_{k}^{i}}\mu_{k}^{i} \\
P_{k}=\sum_{i=1}^{n}{\mu_{k}^{i}[P_{k}^{i}+(\hat{X}_{k}^{i}-\hat{x}_{k})(\hat{X}_{k}^{i}-\hat{x_{k}})^T]} $$
TODO:说明：
目前加入三个模型（CV、CA、CT），CT模型包含左转弯和右转弯两种情况。参数量比较多，调试是个复杂而有趣的事情。

Date:2022-11-23

基于‘当前’统计模型（CS model）的卡尔曼滤波算法 Singer模型 机动模型把机动控制项作为相关噪声（有色噪声）进行建模。 目标加速度a(t)作为指数自相关的零均值随机过程，即： \(R(\tau)=E[a(t)a(t+\tau)]=\sigma_m^2e^{-\alpha\lVert\tau\rVert}\)

$\sigma_m^2$为目标机动加速度的方差，根据目标机动加速度的概率密度函数计算，$\alpha$为目标机动频率。

#### 机动目标加速度分布作如下假定：

1.目标机动加速度处于极大值$a_{M}$的概率为$P_{M}$，处于极小值$-a_{M}$的概率也为$P_{M}$；

2.机动加速度等于0的概率为$P_{0}$(非机动)；

3.机动加速度在区间[$-a_{M}$, $a_{M}$]上近似服从均匀分布。

由以上假设可推出目标加速度的概率密度函数，并进而计算得到目标机动加速度方差为： $$
\sigma_m^2=\frac{a_{M}^2}{3}(1+4P_{M}-P_{0}) $$

应用于卡尔曼滤波时要对加速度a(t)“白化”。
白化后得到如下关系式： $$
\dot{a_{t}}=-\alpha{a_{t}}+w_{t} $$

其中$a_{t}$输入为白噪声的一阶时间相关模型，$w_{t}$是均值为0、方差为$2\alpha{\sigma_m^2}$的高斯白噪声，即： \(E[w(t)w(\tau)]=2\alpha\sigma_m^2\delta(t-\tau)\)

根据以上理论，假设目标的状态向量$\chi=[x,\dot{x},\ddot{x}]$，$\ddot{x}$=$a$，则上述一阶时间相关模型如下：
\(\dot{\chi_{t}}=A\chi_t+w_t\) 其中 \(A= \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & -\alpha \end{bmatrix} ，w=[0, 0, v]\) 将A和$w$代入上式得： $\dot\chi_t$=$\begin{bmatrix}0 & 1 & 0 \ 0 & 0 & 1 \ 0 & 0 & -\alpha \end{bmatrix}$ $\begin{bmatrix}\chi_t \ \dot{\chi_t} \ \ddot{\chi_t}\end{bmatrix}$+$\begin{bmatrix}0 \ 0 \ v\end{bmatrix}$

离散状态采样时间间隔T有： \(\chi_{k+1}=F_k\chi_{k}+w_k\)

式中： \(F_k=e^{AT}=\begin{bmatrix} 1 & T & \frac{\alpha{T}-1+e^{-\alpha{T}}}{\alpha^2} \\ 0 & 1 & \frac{1-e^{-\alpha{T}}}{\alpha} \\ 0 & 0 & e^{-\alpha{T}} \end{bmatrix}, w_k的噪声协方差矩阵Q_k=2\alpha\sigma_m^2\begin{bmatrix} q_{11} & q_{12} & q_{13} \\ q_{21} & {q_22} & q_{23} \\ q_{31} & q_{32} & q_{33} \end{bmatrix}\) 其中Q为对称矩阵，且 \(\begin{cases} q_{11}=(\frac{[1-e^{-2\alpha{T}}+2\alpha{T}+\frac{2\alpha^3T^3}{3}-2\alpha^2T^2-4\alpha{T}e^{-\alpha{T}}]}{2\alpha^{5}}) \\ q_{12}=\frac{[e^{-2\alpha{T}}+1-2e^{-\alpha{T}}+2\alpha{T}e^{-\alpha{T}}-2\alpha{T}+\alpha^2{T}^2]}{2\alpha^4} \\ q_{13}=\frac{[1-e^{-2\alpha{T}}-2\alpha{T}e^{-\alpha{T}}]}{2\alpha^3} \\ q_{22}=\frac{[4e^{-\alpha{T}}-3-e^{-2\alpha{T}}+2\alpha{T}]}{2\alpha^3} \\ q_{23}=\frac{[e^{-2\alpha{T}}+1-2e^{-\alpha{T}}]}{2\alpha^2} \\ q_{33}=\frac{[1-e^{-2\alpha{T}}]}{2\alpha} \end{cases}\) 若目标机动频率$\alpha\to\infty$时，此时状态转移矩阵为 \(F=e^{AT}=\begin{bmatrix} 1 & T & \frac{T^2}{2} \\ 0 & 1 & T \\ 0 & 0 & 1 \end{bmatrix}\) 过程噪声协方差矩阵为 \(Q=q\begin{bmatrix} \frac{T^5}{20} & \frac{T^4}{8} & \frac{T^3}{6} \\ \frac{T^4}{8} & \frac{T^3}{3} & \frac{T^2}{2} \\ \frac{T^3}{6} & \frac{T^2}{2} & T \end{bmatrix}\) 模型此时等效于为匀加速运动模型，q取一个较小值，如q=0.05。

若目标机动频率$\alpha\to0$，此时状态转移矩阵 \(F=\begin{bmatrix} 1 & T & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\) 此时模型等效于匀速运动模型。

以上可以看到匀加速运动模型和匀速运动模型是Singer模型的两个特例，即Singer模型针对的是处于匀加速运动和匀速运动之间的机动目标。但是Singer模型认为目标机动加速度符合零均值的均匀分布并不符合大多数的实际情况。

基于此，出现了对Singer模型的改进算法——“当前”统计模型（CS Model）

CS模型提出两条新的假设：
1.加速度非零均值；
2.加速度的概率密度服从修正的瑞利分布。 类似于Singer模型，CS模型的离散时间状态方程可以表示如下： $\\
X_{k+1}=F_{k}X_{k}+G_{k}\overset{-}{a_{k}}+W_{k} $

其中，$G_{k}$为加速度输入控制矩阵，$\overset{-}a_{k}$为目标加速度均值。 \(G_{k}=\begin{bmatrix} \frac{(-T+\frac{\alpha{T^2}}{2}+\frac{1-e^{-\alpha{T}}}{\alpha})}{\alpha} \\ T-\frac{(1-e^{-\alpha{T}})}{\alpha} \\ 1-e^{-\alpha{T}} \end{bmatrix}\) $\overset{-}a_k$可用当前时刻状态估计中的预测加速度代替，即 $ \overset{-}a_k=\hat{\ddot{\chi}}_{k|k-1} $

噪声$W_k$的协方差矩阵和Singer模型的$Q$相同，但是机动加速度的方差$\sigma_a^2$计算公式为： \(\sigma_a^2=\begin{cases} \frac{4-\Pi}{\Pi}[a_{max}-\overline{a}_{k}]^2，\overline{a}_{k}>0 \\ \frac{4-\Pi}{\Pi}[a_{-max}+\overline{a}_{k}]^2，\overline{a}_{k}<0 \end{cases}\) 其中$a_{-max}$为负加速度上限，不必等于$-a_{max}$。

可以看出模型可以利用目标加速度的预测值自适应地调整加速度方差$\sigma^2$，对目标有更快的响应速度。

TODO:预测部分调试说明：
程序预测部分卡尔曼滤波的状态向量为X=[x,v,a]，输入仅为目标x轴或y轴的位置信息,v和a由卡尔曼滤波预测得出(可能会造成卡尔曼滤波发散，初始化滤波时可以考虑传入目标的速度和加速度信息，但考虑到通过帧差法计算出的速度和加速度误差较大，不可避免地会造成预测抖动，因此并未使用)，Siner模型待调的参数包括目标机动频率α、目标加速度方差σ^2、采样周期T。CS模型待调的参数包括目标机动频率α、采样周期T、机动最大加速度a_max、机动最大负加速度a_-max。
TODO:调试经验：
比赛场上目标不会机动过快，可能的就是1s机动一次，可以稍微往高了调，提高响应速度；采样周期T要根据程序帧间运行时间、通信延迟、发弹延迟计算；目标加速度方差σ^2作为噪声协方差矩阵的控制量，较小可以平滑预测点，减小抖动，但是会出现一定程度滞后；较大会出现一定程度抖动，但是可以提高预测效果。

Date:2022-11-01 预测部分添加小陀螺横移模型

\(\begin{cases} x=x_0+r\cos(\omega t)+Vt\cos(\theta), &x方向\\ y=y_0+r\sin(\omega t)+Vt\sin(\theta), &y方向 \end{cases}\) 方程组中r为小陀螺旋转半径，w为旋转角速度，V为横移速度,θ为目标相对于自己的横移方位角。 此运动模型假设目标一边小陀螺转动，一边进行横向移动，用于移动目标的反陀螺。

2.Debug log

| Date | Description | | — | — | |2022-11-18 | 预测部分实现动态配置调试，代码略显冗余，但后续方便调试。| |2022-11-17| 预测部分加入基于Singer模型的扩展卡尔曼滤波，算法将持续优化，后续会添加CS模型。| |2022-11-05| Debug processor node.| |2022-11-01| 运动模型待调试。| |2022-10-26| Lapack library needs to be linked manually.|