仅距离测量跟踪
这个例子说明了使用粒子滤波器和高斯和滤波器来跟踪使用范围测量的单个对象。
介绍
仅能观察距离信息的传感器无法通过单次检测提供对目标状态的完整了解。此外，仅距离测量的不确定性，当在笛卡尔坐标系中表示时，是非高斯的，并产生凹形状。对于窄视场的纯距离传感器，角度不确定性很小，可以用椭球近似，即高斯分布。然而，对于具有宽视场的距离传感器，当在笛卡尔坐标系中表示时，不确定性由凹环形状描述，不容易用椭球体近似。如下图所示。
这种现象也可以用远程雷达观察到，它可以提供方位角和距离信息。当每个方位分辨率单元的视场在空间上跨越较大区域时，状态的分布变为非高斯分布。像远程校正这样的技术经常被用来利用高斯滤波器，比如扩展卡尔曼滤波器(trackingEKF)和Unscented卡尔曼滤波器(trackingUKF)。中详细说明了这一点多平台雷达探测融合的例子。
在本例中，您将学习如何使用粒子滤波器和高斯和滤波器来表示由大型FOV传感器的距离测量引起的状态中的非高斯不确定性。
定义场景
该场景模拟一个物体在X-Y平面上以恒定速度运动。物体穿过三个等间距传感器的覆盖区域，视场角为60度。传感器视场重叠，增强了目标穿过重叠区域时的可观测性。每个传感器报告距离测量，测量精度为5厘米。
S = rng;rng (2018);exPath = fullfile(matlabroot，“例子”，“融合”，“主要”);目录exPath);[scene,theaterDisplay] = helperCreateRangeOnlySensorScenario;showScenario (theaterDisplay)
showGrabs (theaterDisplay []);
使用粒子过滤器跟踪
在本节中，一个粒子过滤器，trackingPF用于跟踪对象。跟踪由使用的单假设跟踪器执行trackerGNN．跟踪器负责维护跟踪，同时减少假警报的数量。
这个函数initRangeOnlyCVPF初始化一个等速粒子过滤器。函数类似于initcvpf函数，但限制角度从方位角[-180 180]和仰角[-90 90]initcvpf在这个问题中，已知的传感器视场。
%追踪跟踪器= trackernn“FilterInitializationFcn”@initRangeOnlyCVPF,“MaxNumTracks”5);更新显示以绘制粒子数据theaterDisplay。FilterType =“trackingPF”；%高级场景和跟踪对象而推进(场景)%当前时间time = scene.SimulationTime;%生成检测[detection, configs] = generateRangeDetections(场景);%将检测传递给跟踪器如果~isempty(detections) [confTracks，~，allTracks] = tracker(detections,time);elseifisLocked(跟踪器)confTracks = predictTracksToTime(跟踪器)“确认”、时间);结束%更新显示theaterDisplay(检测、配置、confTracks追踪);结束
请注意，在目标被下一个传感器检测到之前，粒子沿着仅距离测量的弧线在状态上具有非高斯不确定性。当目标穿过传感器覆盖区域的边界时，与其他粒子相比，边界处粒子的可能性增加。这种可能性的增加触发了粒子滤波器中的重新采样步骤，粒子坍缩到真正的目标位置。
showGrabs (theaterDisplay [1 - 2]);
使用粒子过滤器，跟踪器在场景的持续时间内保持跟踪。
showGrabs (theaterDisplay 3);
使用高斯和滤波器跟踪
在本节中，一个高斯和滤波器，trackingGSF，用于跟踪对象。对于仅限范围的测量，可以初始化trackingGSF使用initapekf．的initapekf函数初始化一个角度参数化的扩展卡尔曼滤波器。这个函数initRangeOnlyGSF在此示例中定义的initapekf功能跟踪缓慢移动的物体。
%重启场景重启(现场);释放(theaterDisplay);%追踪跟踪器= trackernn“FilterInitializationFcn”, @initRangeOnlyGSF);%更新显示以绘制高斯和分量theaterDisplay。FilterType =“trackingGSF”；而推进(场景)%当前时间time = scene.SimulationTime;%生成检测[detection, configs] = generateRangeDetections(场景);%将检测传递给跟踪器如果~isempty(detections) [confTracks，~，allTracks] = tracker(detections,time);elseifisLocked(跟踪器)confTracks = predictTracksToTime(跟踪器)“确认”、时间);结束%更新显示theaterDisplay(检测、配置、confTracks追踪);结束
你可以使用TrackingFilters的性质trackingGSF查看每个扩展卡尔曼滤波器的状态。由下图中的“单个过滤器”表示，注意过滤器是如何沿着仅距离测量产生的弧线对齐的，直到目标到达重叠区域。在越过边界后，边界处滤波器的可能性增加，轨迹收敛到那个单独的滤波器。重量,ModelProbabilities与最靠近边界的过滤器相比，其他单个过滤器的性能下降，并且它们对状态估计的贡献减少。
showGrabs (theaterDisplay [4 - 5]);
使用高斯和过滤器，跟踪器在场景持续期间维护跟踪。
showGrabs (theaterDisplay 6);rng (s) rmpath (exPath);
总结
在本例中，您学习了如何使用粒子过滤器和高斯和过滤器来跟踪仅使用范围测量的对象。粒子滤波器和高斯和滤波器都提供跟踪遵循非高斯状态分布的对象的功能。虽然高斯和滤波器通过高斯分量的加权和近似分布，但粒子滤波器通过一组样本来表示该分布。粒子滤波提供了一种更自然的方法来表示任意分布，只要样本可以从中生成。然而，为了完美地表示分布，可能需要大量的粒子，这增加了使用过滤器的计算量。由于状态是由样本描述的，粒子滤波结构允许使用任何过程噪声分布以及测量噪声。相反，高斯和滤波器对每个分量使用高斯过程和测量噪声。粒子的主要缺点之一是“样品贫化”问题。在重新采样后，粒子可能会崩溃到高可能性的区域，而不允许过滤器从“不正确”的关联中恢复。由于在高斯和滤波器中不进行重采样，并且每个状态都是高斯的，因此高斯和滤波器提供了一些从这种关联中恢复的能力。 However, this recovery is often dependent on the weights and covariances of each Gaussian-component.
金宝app支持功能
generateRangeDetections此函数生成仅范围检测
函数[detection,config] = generateRangeDetections(场景)detection = [];Config = [];time = scene.SimulationTime;为i = 1:numel(scene.Platforms) [thisDet, thisConfig] = detectOnlyRange(scene.Platforms{i}，time);config = [config;thisConfig];% #好< AGROW >detections = [detections;thisDet];% #好< AGROW >结束结束
detectOnlyRange这个函数从球面检测中移除az和el
函数[rangeDetections,config] = detectOnlyRange(观察者，时间)[fullDetections,numDetections,config] = detect(观察者，时间);为I = 1:numel(配置)正确填充配置以进行绘图配置(i)。FieldOfView = observer.Sensors{i}.FieldOfView;配置(i) .MeasurementParameters(1)。OriginPosition = observer.Sensors{i}.MountingLocation(:);配置(i) .MeasurementParameters(1)。方向= rotmat(四元数(observer.Sensors{i}.MountingAngles(:))'，“eulerd”，“ZYX股票”，“帧”)，“帧”);配置(i) .MeasurementParameters(1)。IsParentToChild = true;结束%删除除范围信息外的所有信息rangedetection = fullDetections;为i = 1:numDetections rangeDetections{i}。测量= rangedetection {i}.Measurement(end);rangeDetections{}。MeasurementNoise = rangedetection {i}.MeasurementNoise(end);rangeDetections{我}.MeasurementParameters(1)。HasAzimuth = false;rangeDetections{我}.MeasurementParameters(1)。HasElevation = false;结束结束
initRangeOnlyGSF这个函数初始化一个角度参数化的高斯和滤波器。它使用[-30 30]的角度限制来指定FOV，并将过滤器的数量设置为10。对二维慢速运动物体的状态协方差和过程噪声进行了修正。
函数filter = initRangeOnlyGSF(detection) filter = initapekf(detection,10，[-30 30]);为i = 1:num (filter.TrackingFilters) filterK = filter.TrackingFilters{i};filterK。ProcessNoise = 0.01*eye(3);filterK.ProcessNoise(3,3) = 0;对于缓慢移动的目标，协方差较小filterK.StateCovariance(2,2) = 0.1;filterK.StateCovariance(4,4) = 0.1;% v = 0时z的低标准差;filterK.StateCovariance(6,6) = 0.01;结束结束
initRangeOnlyCVPF该函数初始化一个等速粒子滤波器，其粒子视场限制为[-30 -30]。
函数filter = initRangeOnlyCVPF(detection) filter = initcvpf(detection);%视场均匀的azaz = -pi/6 + pi/3*rand(1,filter.NumParticles);%无标高;el = 0 (1,filter.NumParticles);%来自高斯范围测量的范围样本R =检测。测量+ sqrt(detection.MeasurementNoise)*randn(1,filter.NumParticles);% x,y,z在传感器框架中[x,y,z] = sph2cart(z, y, r);%从传感器旋转到场景框架。senToPlat = detection.MeasurementParameters(1).Orientation';senPosition = detection.MeasurementParameters(1).OriginPosition;platToScenario = detection.MeasurementParameters(2).Orientation';platPosition = detection.MeasurementParameters(2).OriginPosition;posPlat = senToPlat*[x;y;z] + senPosition;posScen = platToScenario*posPlat + platPosition;%位置粒子filter.Particles(1，:) = posScen(1，:);filter.Particles(3，:) = posScen(2，:);filter.Particles(5，:) = posScen(3，:);%速度粒子%均匀分布过滤器。Particles([2 4]，:) = -1 + 2*rand(2,filter.NumParticles);对于缓慢移动的目标，将%进程噪声设置为低数值。比% GSF增加更多的噪音，以防止粒子崩溃。过滤器。ProcessNoise = 0.1*eye(3);z-速度% 0filter.Particles(6，:) = 0;filter.ProcessNoise(3,3) = 0;结束
