Пассивное расположение Используя один датчик маневрирования
Этот пример иллюстрирует, как отследить цели с помощью пассивных измерений только для угла от одного датчика. Пассивные измерения только для угла содержат азимут и вертикальное изменение цели относительно датчика. Отсутствие измерений области значений делает проблему, бросающую вызов, когда цели, которые будут прослежены, полностью заметны только при определенных обстоятельствах.
В этом примере вы узнаете о некоторых возможных решениях этой проблемы при помощи пассивного инфракрасного датчика, смонтированного на маневрирующей платформе.
Введение
Отсутствие измерений диапазона от цели подразумевает неполную наблюдаемость целевого состояния. Следующая фигура изображает это измерения, только для угла, полученные наблюдателем, перемещающимся при постоянной скорости, приводят к нескольким возможным траекториям (предполагаемый постоянной скоростью) цель.
Теоретические результаты подразумевают, что целевое состояние неразличимо, пока следующим условиям не отвечают [1].
Датчик должен перехитрить цель, т.е. движение датчика должно быть по крайней мере 1 порядком выше, чем цель. Например, если цель перемещается при постоянной скорости, у наблюдателя должно быть, по крайней мере, постоянное ускорение.
Компонент датчика маневрирует, перпендикуляр к углу обзора должен быть ненулевым.
Задайте сценарий
Функция помощника helperCreatePassiveRangingScenario используется, чтобы задать один инфракрасный датчик, смонтированный на платформе. Распознающаяся платформа, которую часто называют как ownship, перемещения при постоянной скорости в начале и затем, выполняет маневр, чтобы наблюдать область значений цели. Цель принята, чтобы не маневрировать и перемещения при постоянной скорости в сценарии.
% Setup exPath = fullfile(matlabroot,'examples','fusion','main'); addpath(exPath); [scene,ownship,theaterDisplay] = helperCreatePassiveRangingScenario; showScenario(theaterDisplay);
Дорожка Используя EKF в декартовых координатах
Проблема отслеживания цели с помощью измерений только для угла может быть сформулирована с помощью расширенного Фильтра Калмана с нелинейной моделью измерения в Декартовых координатах. В этом разделе, постоянная скорость 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);
Дорожка Используя EKF в модифицированных сферических координатах (MSC-EKF)
Модифицированные сферические координаты (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
Подход 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) || isa(tracks,'objectTrack') % 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.