Этот пример иллюстрирует, как отследить цели с помощью пассивных измерений только для угла от одного датчика. Пассивные измерения только для угла содержат азимут и повышение цели относительно датчика. Отсутствие измерений области значений делает проблему, бросающую вызов, когда цели, которые будут прослежены, полностью заметны только при определенных обстоятельствах.
В этом примере вы узнаете о некоторых возможных решениях этой проблемы при помощи пассивного инфракрасного датчика, смонтированного на маневрирующей платформе.
Отсутствие измерений диапазона от цели подразумевает неполную наблюдаемость целевого состояния. Следующая фигура изображает это измерения, только для угла, полученные наблюдателем, перемещающимся в постоянной скорости, приводят к нескольким возможным траекториям (предполагаемый постоянной скоростью) цель.
Теоретические результаты подразумевают, что целевое состояние неразличимо, пока следующим условиям не отвечают [1].
Датчик должен перехитрить цель, т.е. движение датчика должно быть по крайней мере 1 порядком выше, чем цель. Например, если цель перемещается в постоянной скорости, у наблюдателя должно быть, по крайней мере, постоянное ускорение.
Компонент датчика маневрирует, перпендикуляр к углу обзора должен быть ненулевым.
Функция помощника helperCreatePassiveRangingScenario используется, чтобы задать один инфракрасный датчик, смонтированный на платформе. Распознающаяся платформа, которую часто называют как ownship, перемещения в постоянной скорости в начале и затем, выполняет маневр, чтобы наблюдать область значений цели. Цель принята, чтобы не маневрировать и перемещения в постоянной скорости в сценарии.
% Setup exPath = fullfile(matlabroot,'examples','fusion','main'); addpath(exPath); [scene,ownship,theaterDisplay] = helperCreatePassiveRangingScenario; showScenario(theaterDisplay);
Проблема отслеживания цели с помощью измерений только для угла может быть сформулирована с помощью расширенного Фильтра Калмана с нелинейной моделью измерения в Декартовых координатах. В этом разделе постоянная скорость trackingEKF, описывая состояние в глобальных Декартовых координатах используется, чтобы отследить цель.
% Set random seed for reproducible results rng(50); % Create a |trackerGNN| to track the targets using the % FilterInitializationFcn as @initCartesianEKF. tracker = trackerGNN('FilterInitializationFcn',@initCartesianEKF,... 'AssignmentThreshold',50,... 'MaxNumTracks',5); [tem, tam] = helperPassiveRangingErrorMetrics(ownship,false); theaterDisplay.ErrorMetrics = tem; tracks = []; % Advance scenario, simulate detections and track while advance(scene) % Get time information from tracking scenario. truths = platformPoses(scene); time = scene.SimulationTime; % Generate detections from the ownship detections = detect(ownship,time); % Pass detections to tracker if ~isempty(detections) tracks = tracker(detections,time); elseif isLocked(tracker) tracks = predictTracksToTime(tracker,'confirmed',time); end % Update error and assignment metrics tam(tracks,truths); [trackIDs, truthIDs] = currentAssignment(tam); tem(tracks,trackIDs,truths,truthIDs); % Update display theaterDisplay(tracks,detections,tracker); end
Дорожка инициализируется с помощью высокой ковариации в области значений, чтобы составлять недостающее измерение. Заметьте ковариацию в области значений дорожки сразу после инициализации в театральном графике. Когда угловые оценки довольно точны, целевая неуверенность описана тонким эллипсом ковариации.
showGrabs(theaterDisplay,1);
Следующие данные показывают производительность фильтра после того, как датчик начнет маневрировать в сценарии. Область значений и оценочные графики уровня области значений создаются с помощью errorbar и показывают
(стандартное отклонение) границы оценки дорожки.
showGrabs(theaterDisplay,2);
Устойчивость EKF в декартовых координатах
Отслеживание в Декартовых координатах с помощью расширенного Фильтра Калмана обращается из-за простоты проблемной формулировки. Движущие силы состояния представлены набором линейных уравнений, и нелинейность встраивается с помощью двух (относительно) простых уравнений для азимута и повышения.
Однако поведение расширенного Фильтра Калмана, как показывают, ошибочно и может часто быть нестабильным. Это вызвано тем, что состояния (положение и скорость) и неразличимая область значений высоко связываются. Когда область значений неразличима, т.е. во время постоянной скоростной фазы распознающегося движения платформы, фильтр "ложно" оценивает область значений цели в зависимости от истории измерения и сопоставил шумовые значения. Это может привести к преждевременному коллапсу эллипса ковариации, который может заставить фильтр занимать очень долгое время, чтобы сходиться (или даже отличаться) [2] даже после того, как достаточные условия наблюдаемости соблюдают.
Используйте различный случайный seed, чтобы наблюдать преждевременную сходимость расширенного Фильтра Калмана в Декартовых координатах.
% set random seed rng(2015); % Reset the theaterDisplay to capture snapshots with new scenario. release(theaterDisplay); theaterDisplay.GrabFigureFcn = @(fig,scene)helperGrabPassiveRangingDisplay(fig,scene,'CartEKF2'); helperRunPassiveRangingSimulation(scene,theaterDisplay,@initCartesianEKF);
После 2 секунд ковариация в области значений уже вышла из строя, делая фильтр ложно уверенным в его оценке области значений. Заметьте, что цель находится вне эллипса ковариации состояния дорожки и также - строка нулевой ошибки в графике оценки области значений.
showGrabs(theaterDisplay,3);
Поскольку фильтр остается уверенным в этом оценка области значений, это берет намного дольше, чтобы сходиться ближе к фактическим значениям области значений.
showGrabs(theaterDisplay,4);
Измененные сферические координаты (MSC), существующие стабильная система координат для отслеживания использующих измерений только для угла. Путем разъединения состояния в заметные и неразличимые части фильтр преодолевает ограничения, наложенные EKF в Декартовых координатах. Состояние в измененных сферических координатах задано относительным способом т.е. [цель - наблюдатель], и следовательно вход от наблюдателя требуется, чтобы предсказывать состояние в будущем. Это видно в следующих уравнениях, где термины высшего порядка относятся к движению наблюдателя, не полученному постоянной скоростной моделью.
% Set the same random seed to compare with the same detections rng(2015); % restart the scene restart(scene); release(theaterDisplay); theaterDisplay.GrabFigureFcn = @(fig,scene)helperGrabPassiveRangingDisplay(fig,scene,'MSCEKF'); [tem, tam] = helperPassiveRangingErrorMetrics(ownship,true); theaterDisplay.ErrorMetrics = tem; % Create a tracker using |trackerGNN| and MSC-EKF filter initialization. tracker = trackerGNN('FilterInitializationFcn',@initMSCEKF,... 'AssignmentThreshold',50); % The tracks carry a state which is non-linearly dependent on position. % Inform the theaterDisplay that tracks have non-linear state and position % can be extracted using a function_handle. theaterDisplay.HasNonLinearState = true; theaterDisplay.NonLinearTrackPositionFcn = @getTrackPositionsMSC; % Initialization for MSC-EKF. prevPose = pose(ownship,'true'); lastCorrectionTime = 0; allTracks = []; % Advance scenario, simulate detections and track while advance(scene) time = scene.SimulationTime; truths = platformPoses(scene); % Generate detections from ownship detections = detect(ownship,time); % Update the input from the ownship i.e. it's maneuver since last % correction time. currentPose = pose(ownship,'true'); dT = time - lastCorrectionTime; observerManeuver = calculateManeuver(currentPose,prevPose,dT); for i = 1:numel(allTracks) % Set the ObserverInput property using |setTrackFilterProperties| % function of the tracker setTrackFilterProperties(tracker,allTracks(i).TrackID,'ObserverInput',observerManeuver); end % Pass detections to tracker if ~isempty(detections) lastCorrectionTime = time; % Store the previous pose to calculate maneuver prevPose = currentPose; [tracks,~,allTracks] = tracker(detections,time); elseif isLocked(tracker) tracks = predictTracksToTime(tracker,'confirmed',time); end % Update error and assignment metrics tam(tracks,truths); [trackIDs, truthIDs] = currentAssignment(tam); tem(tracks,trackIDs,truths,truthIDs); % Update display theaterDisplay.NonLinearStateInput = currentPose.Position(:); theaterDisplay(detections,tracks,tracker); end
MSC-EKF пытается поддержать ковариацию в области значений, пока датчик не сделал маневр. Это происходит чрезвычайно из-за разъединенных заметных и неразличимых частей в состоянии.
showGrabs(theaterDisplay,5);
Фильтр сходится ближе к истинным значениям быстрее, чем EKF в Декартовых координатах и границах обеспечивают истинную, объективную оценку ошибки.
showGrabs(theaterDisplay,6);
Подход MSC-EKF использует методы линеаризации, чтобы спроектировать ковариации на шаге прогноза. Ковариация в области значений при инициализации обычно высока, и динамика изменения состояния очень нелинейна, который может вызвать проблемы со сходимостью фильтра.
Этот раздел демонстрирует использование фильтра Гауссовой суммы, trackingGSF, чтобы описать состояние с банком фильтров, каждый инициализированный в различных предположениях области значений. Этот метод обычно называют как параметризация области значений [3].
% Set random seed rng(2015); % restart the scene restart(scene); release(theaterDisplay); theaterDisplay.GrabFigureFcn = @(fig,scene)helperGrabPassiveRangingDisplay(fig,scene,'MSCRPEKF'); % Create error and assignment metrics. [tem, tam] = helperPassiveRangingErrorMetrics(ownship,true); theaterDisplay.ErrorMetrics = tem; % Create a tracker using |trackerGNN| and range-parameterized MSC-EKF % filter initialization. tracker = trackerGNN('FilterInitializationFcn',@initMSCRPEKF,... 'AssignmentThreshold',50); theaterDisplay.HasNonLinearState = true; theaterDisplay.NonLinearTrackPositionFcn = @getTrackPositionsMSC; % Initialization for MSC-RPEKF prevPose = pose(ownship,'true'); lastCorrectionTime = 0; allTracks = []; % Advance scenario, simulate detections and track while advance(scene) time = scene.SimulationTime; truths = platformPoses(scene); % Generate detections from ownship detections = detect(ownship,time); % Update the input from the ownship i.e. it's maneuver since last % correction time. currentPose = pose(ownship,'true'); dT = time - lastCorrectionTime; observerManeuver = calculateManeuver(currentPose,prevPose,dT); % Get each filter from the trackingGSF property TrackingFilters using % the |getTrackFilterProperties| function of the tracker. for i = 1:numel(allTracks) trackingFilters = getTrackFilterProperties(tracker,allTracks(i).TrackID,'TrackingFilters'); % Set the ObserverInput for each tracking filter for m = 1:numel(trackingFilters{1}) trackingFilters{1}{m}.ObserverInput = observerManeuver; end end % Pass detections to tracker if ~isempty(detections) lastCorrectionTime = time; % Store the previous pose to calculate maneuver prevPose = currentPose; [tracks,~,allTracks] = tracker(detections,time); elseif isLocked(tracker) tracks = predictTracksToTime(tracker,'confirmed',time); end % Update error and assignment metrics tam(tracks,truths); [trackIDs, truthIDs] = currentAssignment(tam); tem(tracks,trackIDs,truths,truthIDs); % Update display theaterDisplay.NonLinearStateInput = currentPose.Position(:); theaterDisplay(detections,tracks,tracker); end
Следующие данные показывают параметризованный областью значений фильтр после того, как дорожка будет инициализирована и производительность отслеживания фильтра. Процесс параметризации области значений позволяет каждому фильтру нести относительно маленькую ковариацию в области значений и следовательно менее восприимчив к проблемам линеаризации. Time-converge для параметризованного областью значений просачиваются, этот сценарий подобен MSC-EKF. Однако фильтр продемонстрировал меньше переходного поведения, когда датчик перемещается во вторую главную фазу маневрирования траектории, т.е., 35 - 40 секунд в симуляцию. Заметьте, график оценки области значений MSC-EKF выше, который произвел ошибку 10 + км в области значений. Ошибка оценки составляла приблизительно 5 км для параметризованного областью значений фильтра.
showGrabs(theaterDisplay,[7 8]);
Закройте отображения
showGrabs(theaterDisplay,[]);
Продемонстрированный подход с помощью MSC-EKF и параметризованного областью значений MSC-EKF применим больше чем для 1 цели. Чтобы наблюдать каждую цель, датчик должен перехитрить каждого из них. Когда фильтры как MSC-EKF могут поддержать ковариацию области значений во время неманеврирования этапов, оценка каждой дорожки должна сходиться ближе к целям, когда датчик делает маневры относительно них.
MSC-EKF
% Set random seed rng(2015); % Create a two-target scenario and theater display. [sceneTwo,~,theaterDisplayTwo] = helperCreatePassiveRangingScenario(2); % Use the helper function to run the simulation using @initMSCEKF. helperRunPassiveRangingSimulation(sceneTwo,theaterDisplayTwo,@initMSCEKF);
% Show results
showGrabs(theaterDisplayTwo,1);
Параметризованный областью значений MSC-EKF
% Set random seed rng(2015); release(theaterDisplayTwo); % Run the simulation using range-parameterized MSC-EKF helperRunPassiveRangingSimulation(sceneTwo,theaterDisplayTwo,@initMSCRPEKF);
% Show results
showGrabs(theaterDisplayTwo,2);
rmpath(exPath);
Этот пример иллюстрирует проблемы, сопоставленные с одно датчиком проблема отслеживания только для угла, и демонстрирует, как использовать различные алгоритмы отслеживания, чтобы оценить область значений и уровень области значений цели. Вы изучили, как использовать initcvekf, initcvmscekf и trackingGSF, чтобы задать настроенного Последователя-Декарта-EKF, MSC-EKF и параметризованный областью значений MSC-EKF для пассивного расположения. Вы также изучили, как использовать заданные фильтры со средством отслеживания GNN, trackerGNN.
Отфильтруйте Функции инициализации
Следующий раздел перечисляет весь FilterInitializationFcn, используемый для объекта trackerGNN в этом Примере.
initCartesianEKF Инициализирует trackingEKF от обнаружения только для угла
function filter = initCartesianEKF(detection) % Create a full detection with high covariance in range estimate. rangeEstimate = 5e4; rangeCov = 16e8; fullDetection = detection; fullDetection.Measurement = [fullDetection.Measurement;rangeEstimate]; fullDetection.MeasurementNoise = blkdiag(fullDetection.MeasurementNoise,rangeCov); % Update the MeasurementParameters to include range. fullDetection.MeasurementParameters(1).HasRange = true; % Use the initcvekf function to initialize a trackingEKF using the % fullDetection. fullFilter = initcvekf(fullDetection); % |initcvekf| defines the StateCovariance in velocity with 100. This % defines a standard deviation uncertainty in velocity as 10 m/s. Scale % the velocity covariance with 400 i.e. an equivalent velocity standard % deviation of 200 m/s velCov = fullFilter.StateCovariance(2:2:end,2:2:end); fullFilter.StateCovariance(2:2:end,2:2:end) = 400*velCov; % fullFilter can only be corrected with [az el r] measurements. % Create a |trackingEKF| using the State and StateCovariance from % fullFilter. filter = trackingEKF(@constvel,@cvmeas,fullFilter.State,... 'StateCovariance',fullFilter.StateCovariance,... 'StateTransitionJacobianFcn',@constveljac,... 'MeasurementJacobianFcn',@cvmeasjac,... 'HasAdditiveProcessNoise',false); % Unit standard deviation acceleration noise filter.ProcessNoise = eye(3); end
initMSCEKF Инициализирует MSC-EKF от обнаружения только для угла
function filter = initMSCEKF(detection) % Use the second input of the |initcvmscekf| function to provide an % estimate of range and standard deviation in range. rangeEstimate = 5e4; rangeSigma = 4e4; filter = initcvmscekf(detection,[rangeEstimate,rangeSigma]); % The initcvmscekf assumes a velocity standard deviation of 10 m/s, which % is linearly transformed into azimuth rate, elevation rate and vr/r. % Scale the velocity covariance by 400 to specify that target can move 20 % times faster. filter.StateCovariance(2:2:end,2:2:end) = 400*filter.StateCovariance(2:2:end,2:2:end); filter.ProcessNoise = eye(3); end
initMSCRPEKF Инициализирует параметризованный областью значений MSC-EKF от обнаружения только для угла
function filter = initMSCRPEKF(detection) % A range-parameterized MSC-EKF can be defined using a Gaussian-sum filter % (trackingGSF) containing multiple |trackingMSCEKF| as TrackingFilters. % Range-parametrization constants rMin = 3e4; rMax = 8e4; numFilters = 10; rho = (rMax/rMin)^(1/numFilters); Cr = 2*(rho - 1)/(rho + 1)/sqrt(12); indFilters = cell(numFilters,1); for i = 1:numFilters range = rMin/2*(rho^i + rho^(i-1)); rangeSigma = Cr*range; % Use initcvmscekf function to create a trackingMSCEKF with provided % range and rangeSigma. indFilters{i} = initcvmscekf(detection,[range rangeSigma]); % Update the velocity covariance of each filter. indFilters{i}.StateCovariance(2:2:end,2:2:end) = 400*indFilters{i}.StateCovariance(2:2:end,2:2:end); end filter = trackingGSF(indFilters); end
Служебные функции calculateManeuver
function maneuver = calculateManeuver(currentPose,prevPose,dT) % Calculate maneuver i.e. 1st order+ motion of the observer. This is % typically obtained using sensors operating at much higher rate. v = prevPose.Velocity; prevPos = prevPose.Position; prevVel = prevPose.Velocity; currentPos = currentPose.Position; currentVel = currentPose.Velocity; % position change apart from constant velocity motion deltaP = currentPos - prevPos - v*dT; % Velocity change deltaV = currentVel - prevVel; maneuver = zeros(6,1); maneuver(1:2:end) = deltaP; maneuver(2:2:end) = deltaV; end
getTrackPositionsMSC
function [pos,cov] = getTrackPositionsMSC(tracks,observerPosition) if isstruct(tracks) % Track struct state = [tracks.State]; stateCov = cat(3,tracks.StateCovariance); elseif isa(tracks,'trackingMSCEKF') % Tracking Filter state = tracks.State; stateCov = tracks.StateCovariance; end % Get relative position using measurement function. relPos = cvmeasmsc(state,'rectangular'); % Add observer position pos = relPos + observerPosition; pos = pos'; if nargout > 1 cov = zeros(3,3,numel(tracks)); for i = 1:numel(tracks) % Jacobian of position measurement jac = cvmeasmscjac(state(:,i),'rectangular'); cov(:,:,i) = jac*stateCov(:,:,i)*jac'; end end end
[1] Fogel, Ила и Мотти Гавис. "Движущие силы n-го порядка предназначаются для наблюдаемости от угловых измерений". Транзакции IEEE на Космических и Электронных системах 24.3 (1988): 305-308.
[2] Aidala, Винсент и Шерри Хаммель. "Использование измененных полярных координат для отслеживания только для подшипников". Транзакции IEEE на Автоматическом управлении 28.3 (1983): 283-294.
[3] Персик, N. "Отслеживание только для подшипников с помощью набора параметризованных областью значений расширенных Фильтров Калмана". Теория IEE Proceedings-Control и Приложения 142.1 (1995): 73-80.