AFSim仿真系统—03 跟踪的不确定性和协方差

本栏目将对《AFSim 2.9中文参考手册》进行持续更新，欢迎关注交流！

对本书全文和AFSIM其它资料感兴趣的伙伴，可四新作者获取~

全部内容索引请看 ⬇️⬇️⬇️

《AFSim 2.9中文参考手册》-CSDN博客https://blog.csdn.net/henggesim/article/details/145566384

---

目录

Tracking Uncertainties（跟踪的不确定性）

Covariance Matrices（协方差矩阵）

Transformation of Covariance Matrices（协方差矩阵的变换）

Covariance Matrix Types in AFSIM（AFSIM中的协方差矩阵类型）

Weighted Average Fusion of Covariances（协方差的加权平均方法）

Visualization of Covariance Matrices（协方差矩阵可视化）

---

Tracking Uncertainties（跟踪的不确定性）

在AFSIM中，跟踪不确定性通常源于传感器误差的处理。这些误差包括距离误差标准差range_error_sigma（），方位误差标准差azimuth_error_sigma（）和仰角误差标准差elevation_error_sigma（）。顾名思义，它们表示正态分布中的一个标准差（）。这些误差在传感器的自然坐标系中定义，即波束坐标系（BCS），该坐标系通常（但不总是）与关节部件的部件坐标系（PCS）对齐。在这个坐标系中，x轴沿距离向量方向，方位和仰角误差定义了切向坐标的不确定性。

沿给定方向x，对于零均值，测量的方差定义为：

           公式1

其中，表示函数的期望值。

如符号所示，传感器误差的方差由sigma值的平方给出（它们符合正态分布）。对于给定的距离R，BCS（x,y,z）坐标系中的线性方差为：

           公式2

Covariance Matrices（协方差矩阵）

协方差衡量一个独立变量与另一个变量的依赖性或相关性。在三维笛卡尔坐标系（x,y,z）中，有三个协方差：、和（理解为、、）。一个变量与自身的协方差就是方差（即等）。

对于两个方向x和y，以及零均值，测量的协方差定义为：

            公式3

三维中的完整协方差集可以方便地表示为协方差矩阵的形式：

           公式4

测量协方差矩阵在BCS坐标系中表达传感器测量误差，仅在对角线上有非零值（它是一个对角矩阵）:

   公式5

Transformation of Covariance Matrices（协方差矩阵的变换）

变换的目的

为了简化轨迹报告，通常将协方差表示为相对于一个公共坐标系；对于AFSIM，这个坐标系是世界坐标系（WCS）。因此，需要将测量协方差转换为与WCS对齐。

变换的流程

根据线性代数，使用以下关系可以将对角矩阵表示为非对角矩阵：

      公式6

其中，C是非对角协方差矩阵，T是用于将协方差转换为对角形式的矩阵。对于AFSIM，这个变换矩阵T是WCS到BCS的坐标变换，而C是我们想要的与WCS对齐的协方差矩阵。求解C：

      公式7

因此，对于每个测量矩阵M（如公式(5)所示），执行操作以生成存储在WsfTrack的测量协方差字段中的矩阵C。

这种转换确保了在不同坐标系之间的协方差矩阵的一致性和准确性，使得在AFSIM中进行多目标跟踪时能够有效地处理和报告轨迹数据。

Covariance Matrix Types in AFSIM（AFSIM中的协方差矩阵类型）

尽管可能存在无限多种协方差矩阵类型，AFSIM仅识别两种类型，这些类型在WsfTracks中作为测量协方差或状态协方差（卡尔曼滤波器的产物）传递。其中一种形式如公式(4)所示，仅包括位置不确定性。另一种形式同时提供x、y和z方向上的位置和速度不确定性，并由一个6x6矩阵表示：

       公式8

其中，、和表示被跟踪目标的感知速度向量的分量。

Weighted Average Fusion of Covariances（协方差的加权平均方法）

在AFSIM中，使用加权平均融合方法，当协方差来自两个独立的原始轨迹时，进行协方差矩阵的融合。这一过程遵循参考文献1中的方程8.3.3-15（协方差融合方程）：

         公式9

其中，和代表来自两个不同原始轨迹的协方差矩阵，而C是融合后的协方差，存储在本地轨迹的状态协方差字段中。

通过使用协方差矩阵的逆（称为信息矩阵），可以得到方程(9)的更直观形式：

          公式10

在这种形式下，协方差融合方程变为：

这种方法通过将协方差矩阵转换为信息矩阵形式，简化了融合过程，并提供了一种有效的方式来处理多个传感器数据的融合。

Visualization of Covariance Matrices（协方差矩阵可视化）

以（4）中的形式表示的三维协方差可以表现为一个椭球，该椭球以轨迹的估计位置为中心，并根据对协方差进行对角化的变换进行定向（6）。这种表示作为可视化辅助工具非常有用，并且在AFSIM脚本中可以通过Ellipsoid和WsfDraw脚本类实现支持。例如，一个为轨迹测量协方差提供这种可视化的脚本如下所示：

script void DrawCovariance(WsfTrack aTrack)
   WsfDraw draw = {};
   draw.SetId(aTrack.TrackId().Number());
   draw.Erase(aTrack.TrackId().Number());
   draw.BeginLines();
   draw.Vertex(aTrack.Originator());
   draw.Vertex(aTrack.Target());
   draw.End();
   WsfCovariance PWCS = aTrack.MeasurementCovariance(); // The WCS-oriented covariance from the track
   draw.SetColor(1.0, 0.0, 0.0, 0.6);
   WsfGeoPoint trackLocation = aTrack.CurrentLocation();

   if (PWCS.IsValid() && aTrack.LocationValid())
   {
       double S = 100; // Scale factor to make small ellipsoids easier to see
       Ellipsoid e = PWCS.EllipsoidNED();  // NED-referenced ellipsoid representation of the covariance
       draw.BeginEllipsoid(e.OrientationHeadingDeg(), // See documentation.
                           e.OrientationPitchDeg(),
                           e.OrientationRollDeg(),
                           e.SemiAxisForward() * S,
                           e.SemiAxisSide() * S,
                           e.SemiAxisUp() * S);
       draw.Vertex(trackLocation);
       draw.End();
   }
end_script