使用单个机动传感器的被动测距

打开脚本

这个例子演示了如何使用来自单个传感器的无源角度测量来跟踪目标。被动仅角测量包含目标相对于传感器的方位角和仰角。距离测量的缺失使得这个问题很有挑战性，因为要跟踪的目标只有在某些条件下才能完全观察到。

在本例中，您将了解通过使用安装在机动平台上的被动红外传感器来解决此问题的一些可能的解决方案。金宝搏官方网站

介绍

缺少目标的距离测量意味着目标状态的不完全可观测性。下图描述了由以恒定速度移动的观测者获得的仅角度测量结果导致（假定恒定速度）目标的多个可能轨迹。

理论结果表明，在满足以下条件之前，目标状态是不可观测的[1]。

传感器必须超出目标，即传感器的运动必须至少比目标高1个数量级。例如，如果目标以恒定速度移动，则观察者必须至少具有恒定加速度。
垂直于视线垂直于视线的传感器操纵的组件必须是非零。

定义场景

一个helper函数HelperCreatePassideringScenario用于定义安装在平台上的单个红外传感器。传感平台通常称为自有平台，开始时以恒定速度移动，然后执行机动以观察目标的范围。假设目标为非机动目标，并在场景中以恒定速度移动。

％ 设置expath = fullfile（matlabroot，“例子”,“融合”,'主要的'）;addpath（expath）;[场景，自己，TheaterDisplay] = HelperCreatePassivingScenenario;showscenario（theaterdisplay）;

使用笛卡尔坐标系中的EKF跟踪

用纯角度测量跟踪目标的问题可以用一个扩展卡尔曼滤波器在笛卡尔坐标下的非线性测量模型来表述。在这一段，速度是恒定的trackingekf.，在全局笛卡尔坐标系中描述状态，用于跟踪目标。

％设置随机种子以进行可重复的结果RNG（50）;%创建| trackerGNN |以使用％filterinitializationfcn为@initcartesianekf。跟踪器(“FilterInitializationFcn”，@ initcartesianekf，...“AssignmentThreshold”，50，...'maxnumtracks',5); [tem，tam]=HelperPassiveringErrorMetrics（所有权，false）；theaterDisplay.ErrorMetrics=tem；轨道=[]；％前进方案，模拟检测和轨道尽管预演（现场）%从跟踪场景中获取时间信息。真相=平台姿态（场景）；时间=场景。模拟时间；%从自己的船生成检测检测=检测（自己，时间）;对跟踪器的％通过检测如果~isempty（检测）轨迹=跟踪器（检测，时间）；埃尔塞夫Islocked（跟踪器）曲目= predicttrackstotime（跟踪器，“确认”，时间）；终止%更新错误和分配度量tam（轨道、真相）；[trackid，truthIDs]=当前分配（tam）；tem（轨迹、轨迹ID、真理、真理）；%更新显示剧院显示器（轨迹、检测、跟踪器）；终止

航迹初始化使用范围内的高协方差来解释缺失的测量。注意在剧场图中初始化后轨道范围的协方差。由于角估计是相当精确的，目标不确定性用一个细协方差椭圆来描述。

展品展览（剧院展览，1）；

下图为场景中传感器开始机动后的滤波性能。范围和范围率估计图是使用错误栏并展示 $\ sigma$ （标准偏差）轨迹估计的界限。

展品（剧院展示，2）；

笛卡尔坐标系下EKF的稳定性

使用扩展卡尔曼滤波器跟踪笛卡尔坐标，由于易于解决方案制定而受到吸引人。状态动态由一组线性方程表示，并且使用两个（相对）简单的方程来嵌入非线性的非线性方程，用于方位角和高度。

也就是说，扩展卡尔曼滤波器的行为是不稳定的，并且经常是不稳定的。这是因为状态（位置和速度）和不可观测范围是高度耦合的。当距离不可观测时，即在传感平台运动的恒定速度阶段，滤波器根据测量历史和相关噪声值“错误”估计目标的距离。这可能导致协方差椭圆过早崩溃，这可能导致滤波器需要很长时间才能收敛（甚至发散）[2]，即使在满足足够的可观测性条件后也是如此。

使用不同的随机种子观察笛卡尔坐标系下扩展卡尔曼滤波器的早熟收敛。

随机种子rng (2015);％重置TheaterDisplay以捕获具有新方案的快照。发布（剧院展示）；theaterDisplay.GrabFigureFcn=@（fig，场景）helperGrabPassiveRangingDisplay（fig，场景，“CartEKF2”）;HelperrunpassiveSimingimulation（现场，TheaterDisplay，@ initcartesianekf）;

2秒后，该范围内的协方差已经崩溃，使过滤器对其对范围的估计虚假充满信心。请注意，目标位于轨道状态的协方差椭圆之外，范围估计绘图中的零错误线也是如此。

展览（剧院展览，3）；

由于过滤器仍然对其估计的范围保持着自信，因此收敛到实际范围值更长的时间更长。

Showgrabs（TheaterDisplay，4）;

使用修正球坐标系中的EKF跟踪（MSC-EKF）

改进的球面坐标(MSC)提供了一种稳定的坐标系统，可用于仅角度测量的跟踪。通过将状态解耦为可观察部分和不可观察部分，该滤波器克服了EKF在笛卡儿坐标系下的限制。修正球坐标下的状态是相对定义的，即[目标-观测器]，因此需要观测器的输入来预测未来的状态。这可以在以下方程中看到，其中高阶项指的是不被恒定速度模型捕获的观察者运动。

$x=x{t}-x{o}$

$\dot{x}=\dot{x{t}-\dot{x{o}$

$\dot{x}=Ax{t}-（Ax{0}+\mathcal{O}（t^{2}）+\mathcal{O}（t^{3}）+…）$

${x} \点=(间{t} -间的{0})+ \ mbox{高}\ mbox{秩序}\ mbox{条款}$

％设置相同的随机种子以比较相同的检测rng (2015);%重新启动场景重启（场景）；发布（剧院展示）；theaterDisplay.GrabFigureFcn=@（fig，场景）helperGrabPassiveRangingDisplay（fig，场景，“MSCEKF”); [tem，tam]=HelperPassiveringErrorMetrics（所有权，true）；theaterDisplay.ErrorMetrics=tem；%使用| trackerGNN |和MSC-EKF过滤器初始化创建跟踪器。跟踪器(“FilterInitializationFcn”，@initMSCEKF，...“AssignmentThreshold”，50）;轨道携带一种与位置非线性相关的状态。％通知TheaterDisplay，轨道具有非线性状态和位置%可以使用函数句柄提取。theaterdisplay.hasnonlinearstate = true;theaterdisplay.nonlineartrackpositionfcn = @gettrackpositionsmsc;%MSC-EKF的初始化。prevPose =姿势(认同感,'真的'）;lastCorrectionTime = 0;allTracks = [];％前进方案，模拟检测和轨道尽管前进（场景）时间=场景.simulationtime;真相=平台（场景）;%从所有权生成检测检测=检测（自己，时间）;%更新来自船主的输入，即自去年以来的机动%修正时间。视图经=姿势（自己，'真的'）;dT = time - lastCorrectionTime;observerManeuver = calculateManeuver (currentPose prevPose, dT);对于i=1:numel（所有轨道）%使用| setTrackFilterProperties设置ObserverInput属性|%跟踪器的功能setTrackFilterProperties（跟踪器，所有轨迹（i）.TrackID，“ObserverInput”观察者动作）；终止对跟踪器的％通过检测如果〜isempty（检测）lastcorrectiontime =时间;%存储上一个姿势以计算机动普遍=函件;[曲目，〜，AllTracks] =跟踪器（检测，时间）;埃尔塞夫Islocked（跟踪器）曲目= predicttrackstotime（跟踪器，“确认”，时间）；终止%更新错误和分配度量tam（轨道、真相）；[trackid，truthIDs]=当前分配（tam）；tem（轨迹、轨迹ID、真理、真理）；%更新显示theaterDisplay.NonLinearStateInput=currentPose.Position（：）；剧院显示器（探测、跟踪、跟踪器）；终止

MSC-EKF尝试在范围内保持协方差，直到传感器未进行机动。这主要是由于状态中可观测和不可观测部分的解耦。

ShowGrabs（TheaterDisplay，5）;

在笛卡尔坐标系中，滤波器比EKF更快地收敛到更接近真值，并且 $\ sigma$ 界限提供了对错误的真实无偏见的估计。

ShowGrabs（TheaterDisplay，6）;

使用范围参数化MSC-EKF跟踪

MSC-EKF方法采用线性化技术在预测步骤中投影协方差。初始化范围内的协方差通常很高，状态转换动态高度非线性，这可能导致滤波器收敛的问题。

本节演示高斯和滤波器的使用，trackinggsf.，描述具有滤波器银行的状态，每个初始化在不同的范围假设。该技术通常称为范围参数化[3]。

%设定随机种子rng (2015);%重新启动场景重启（场景）；发布（剧院展示）；theaterDisplay.GrabFigureFcn=@（fig，场景）helperGrabPassiveRangingDisplay（fig，场景，'mscrpekf'）;％创建错误和分配度量标准。[tem, tam] = helperPassiveRangingErrorMetrics(owner,true);theaterDisplay。ErrorMetrics = tem;％创建跟踪器使用| trackergnn |和范围参数化MSC-EKF%过滤器初始化。跟踪器(“FilterInitializationFcn”，@initMSCRPEKF，...“AssignmentThreshold”,50); theaterDisplay.hasnollinearState=true；theaterDisplay.NonLinearTrackPositionFcn=@getTrackPositionsMSC；% MSC-RPEKF初始化prevPose =姿势(认同感,'真的'）;lastCorrectionTime = 0;allTracks = [];％前进方案，模拟检测和轨道尽管前进（场景）时间=场景.simulationtime;真相=平台（场景）;%从所有权生成检测检测=检测（自己，时间）;%更新来自船主的输入，即自去年以来的机动%修正时间。视图经=姿势（自己，'真的'）;dT = time - lastCorrectionTime;observerManeuver = calculateManeuver (currentPose prevPose, dT);％从TrackingGSF属性TrackingFilters获取每个过滤器%跟踪器的|getTrackFilterProperties|函数。对于i = 1：Numel（AllTracks）TrackingFilters = GetTrackFilterProperties（跟踪器，AllTracks（i）.trackid，'trackingfilters'）;%为每个跟踪过滤器设置ObserverInput对于m=1:numel（trackingFilters{1}）trackingFilters{1}{m}.ObserverInput=observerManeuver；终止终止对跟踪器的％通过检测如果〜isempty（检测）lastcorrectiontime =时间;%存储上一个姿势以计算机动普遍=函件;[曲目，〜，AllTracks] =跟踪器（检测，时间）;埃尔塞夫Islocked（跟踪器）曲目= predicttrackstotime（跟踪器，“确认”，时间）；终止%更新错误和分配度量tam（轨道、真相）；[trackid，truthIDs]=当前分配（tam）；tem（轨迹、轨迹ID、真理、真理）；%更新显示theaterDisplay.NonLinearStateInput=currentPose.Position（：）；剧院显示器（探测、跟踪、跟踪器）；终止

下图显示了跟踪初始化后的范围参数化过滤器以及过滤器的跟踪性能。范围参数化过程允许每个滤波器在范围内携带相对较小的协方差，因此不易受到线性化问题的影响。在这种情况下，距离参数化滤波器的收敛时间类似于MSC-EKF。然而，当传感器进入轨道的第二个主要机动阶段（即模拟开始35到40秒）时，滤波器表现出较少的瞬态行为。请注意，上面的MSC-EKF距离估计图产生了10+km的距离误差。距离参数化滤波器的估计误差约为5km。

展品（剧院陈列，[7 8]）；

关闭显示

展品（剧院展示，[]）；

多目标场景

演示的MSC-EKF和距离参数化MSC-EKF方法适用于1个以上的目标。为了观察每一个目标，传感器必须以机动取胜。由于MSC-EKF等滤波器可以在非机动阶段保持距离协方差，因此当传感器对目标进行机动时，每个航迹的估计值应该更接近目标。

MSC-EKF

%设定随机种子rng (2015);%创建一个双目标场景和剧场显示。[sceneTwo, ~, theaterDisplayTwo] = helperCreatePassiveRangingScenario (2);%使用helper函数使用@initMSCEKF运行模拟。HelperRunPassiveringSimulation（sceneTwo，theaterDisplayTwo，@initMSCEKF）；

%显示结果showGrabs (theaterDisplayTwo, 1);

距离参数化MSC-EKF

%设定随机种子rng (2015);释放(theaterDisplayTwo);％使用范围参数化MSC-EKF运行模拟HelperRunPassiveringSimulation（sceneTwo，theaterDisplayTwo，@initMSCRPEKF）；

%显示结果Showgrabs（TheaterDisplaytwo，2）;rmpath（expath）;

总结

这个例子说明了与单传感器纯角度跟踪问题相关的挑战，并演示了如何使用不同的跟踪算法来估计目标的距离和距离速率。你学会了如何使用initcvekf,initcvmscekf和trackinggsf.定义定制的笛卡尔ekf、MSC-EKF和距离参数化MSC-EKF用于被动测距。您还学习了如何使用GNN跟踪器定义的过滤器，trackerGNN.

金宝app支持功能

过滤器初始化函数

以下部分列出了所有过滤器初始化FCN用于这一点trackerGNN对象在此示例中。

initCartesianEKF从一个角度检测初始化trackingekf

功能filter=initCartesianEKF（检测）％在范围估计中创造了具有高协方差的完整检测。rangeEstimate = 5 e4;rangeCov = 16 e8;fullDetection =检测;fullDetection。测量= [fullDetection.Measurement; rangeEstimate];fullDetection。MeasurementNoise = blkdiag (fullDetection.MeasurementNoise rangeCov);%更新MeasurementParameters以包含范围。fullDetection.MeasurementParameters(1)。HasRange = true;使用initcvekf函数初始化trackingEKF% fullDetection。fullfilter = initcvekf（Fulldetection）;% |initcvekf|定义速度为100的StateCovariance。这%将速度的标准偏差不确定度定义为10 m/s%速度协方差为400，即等效速度标准%偏差200米/秒velCov=fullFilter.状态协方差（2:2:end，2:2:end）；状态协方差（2:2:end，2:2:end）=400*velCov；%fullFilter只能通过[az el r]测量值进行校正。创建|trackingEKF|使用State和StateCovariance from％fullfilter。过滤器= trackingEKF (@constvel @cvmeas fullFilter。状态,...'statecovariance'，fullfilter.statecovariance，...“状态转换JacobianFCN”，@constveljac，...“测量JacobianFCN”，@cvmeasjac，...'hasadditiveprocessnoise'，错误的）;%单位标准差加速度噪声过滤器。过程噪声=眼睛（3）；终止

initMSCEKF从仅角度检测初始化MSC-EKF

功能filter = initmscekf（检测）%使用|initcvmscekf|函数的第二个输入来提供百分比估计范围和标准偏差范围。范围估计=5e4；范围σ=4e4；filter=initcvmscekf（检测，[范围估计，范围西格玛]）；%initcvmscekf假设速度标准偏差为10 m/s，即%线性转换为方位角速率、仰角速率和vr/r。％缩放到400的速度协方差，以指定目标可以移动20%时间快了。filter.stateConvariance（2:2:end，2:2:end）=400*filter.stateConvariance（2:2:end，2:2:end）；filter.ProcessNoise=eye（3）；终止

initMSCRPEKF从仅角度检测初始化范围参数化MSC-EKF

功能filter = initmscrpekf（检测）％可以使用高斯 - 和滤波器定义范围参数化MSC-EKF%（trackingGSF）包含多个| trackingMSCEKF | as TrackingFilters。% Range-parametrization常量rmin = 3e4;rmax = 8e4;numfilters = 10;rho =（rmax / rmin）^（1 / numfilters）;Cr = 2 *（rho  -  1）/（rho + 1）/ sqrt（12）;Indfilters = Cell（NumFilters，1）;对于i = 1:numFilters range = rMin/2*(rho^i + rho^(i-1));rangeSigma = Cr *范围;%使用initcvmscekf函数创建具有提供的跟踪mscekf% range和rangessigma。indFilters{i}=initcvmscekf（检测，[范围范围西格玛]）；％更新每个过滤器的速度协方差。indFilters{i}.状态协方差（2:2:end，2:2:end）=400*indFilters{i}.状态协方差（2:2:end，2:2:end）；终止filter = trackinggsf（Indfilters）;终止

实用功能calculateManeuver

功能MANEUVER = CAMPULATEMANEUVER（湿润，常牙，DT）计算机动，即观察者的一阶+运动。这是%通常使用以更高速率运行的传感器获得。v=前置速度；prevPos=prevPose.位置；PrevLevel=prevPose.速度；currentPos=当前姿势位置；currentVel=currentPose.速度；除匀速运动外，位置变化百分比deltaP = currentPos - prevPos - v*dT;%速度变化deltaV=当前电平-前置电平；机动=零（6,1）；机动（1:2:end）=deltaP；机动（2:2:end）=deltaV；终止

getTrachtPositionSmsc.

功能[pos，cov] = getTrachtpositionSmsc（曲目，观察者）如果isstruct(跟踪)| | isa(跟踪,'ObjectTrack')％跟踪结构状态= [tracks.State];tracks.StateCovariance stateCov =猫(3日);埃尔塞夫isa（轨道，“跟踪mscekf”)%跟踪滤波器state=tracks.state；stateCov=tracks.StateCovariance；终止%使用测量功能获取相对位置。relPos=cvmeasmsc（状态，“矩形”）;%添加观察者位置pos=relPos+observer位置；pos=pos'；如果nargout>1 cov=零（3,3，numel（轨迹））；对于i = 1：numel（轨道）位置测量的％雅孚jac=CVMEASMSCCJAC（状态（：，i），“矩形”）;浸(:,:,i) =江淮* stateCov(:,:我)*江淮';终止终止终止