В этом примере показано, как использовать residualgps возразите функции и фильтрации невязки, чтобы обнаружить, когда новые измерения датчика не смогут быть сопоставимы с текущим состоянием фильтра.
Загрузите MAT-файл loggedDataWithMultipath.mat. Этот файл содержит симулированный IMU и данные о GPS, а также положение основной истины и ориентацию круговой траектории. Данные о GPS содержат ошибки из-за многопутевых ошибок в одном разделе траектории. Эти ошибки были смоделированы путем добавления белого шума в данные о GPS, чтобы симулировать эффекты городского каньона.
load('loggedDataWithMultipath.mat', ... 'imuFs', 'accel', 'gyro', ... % IMU readings 'gpsFs', 'lla', 'gpsVel', ... % GPS readings 'truePosition', 'trueOrientation', ... % Ground truth pose 'localOrigin', 'initialState', 'multipathAngles') % Number of IMU samples per GPS sample. imuSamplesPerGPS = (imuFs/gpsFs); % First and last indices corresponding to multipath errors. multipathIndices = [1850 2020];
Создайте два фильтра оценки положения с помощью insfilterNonholonomic объект. Используйте один фильтр, чтобы обработать все показания датчика. Используйте другой фильтр для, только обрабатывают показания датчика, которые не рассматриваются выбросами.
% Create filters. % Use this filter to only process sensor readings that are not detected as % outliers. gndFusionWithDetection = insfilterNonholonomic('ReferenceFrame', 'ENU', ... 'IMUSampleRate', imuFs, ... 'ReferenceLocation', localOrigin, ... 'DecimationFactor', 2); % Use this filter to process all sensor readings, regardless of whether or % not they are outliers. gndFusionNoDetection = insfilterNonholonomic('ReferenceFrame', 'ENU', ... 'IMUSampleRate', imuFs, ... 'ReferenceLocation', localOrigin, ... 'DecimationFactor', 2); % GPS measurement noises. Rvel = 0.01; Rpos = 1; % The dynamic model of the ground vehicle for this filter assumes there is % no side slip or skid during movement. This means that the velocity is % constrained to only the forward body axis. The other two velocity axis % readings are corrected with a zero measurement weighted by the % |ZeroVelocityConstraintNoise| parameter. gndFusionWithDetection.ZeroVelocityConstraintNoise = 1e-2; gndFusionNoDetection.ZeroVelocityConstraintNoise = 1e-2; % Process noises. gndFusionWithDetection.GyroscopeNoise = 4e-6; gndFusionWithDetection.GyroscopeBiasNoise = 4e-14; gndFusionWithDetection.AccelerometerNoise = 4.8e-2; gndFusionWithDetection.AccelerometerBiasNoise = 4e-14; gndFusionNoDetection.GyroscopeNoise = 4e-6; gndFusionNoDetection.GyroscopeBiasNoise = 4e-14; gndFusionNoDetection.AccelerometerNoise = 4.8e-2; gndFusionNoDetection.AccelerometerBiasNoise = 4e-14; % Initial filter states. gndFusionWithDetection.State = initialState; gndFusionNoDetection.State = initialState; % Initial error covariance. gndFusionWithDetection.StateCovariance = 1e-9*ones(16); gndFusionNoDetection.StateCovariance = 1e-9*ones(16);
HelperPoseViewer осциллограф позволяет 3-D визуализацию, сравнивающую оценку фильтра и основную истину. Используя несколько осциллографов может замедлить симуляцию. Чтобы отключить осциллографы, установите соответствующую логическую переменную на false.
usePoseView = true; % Turn on the 3D pose viewer if usePoseView [viewerWithDetection, viewerNoDetection, annoHandle] ... = helperCreatePoseViewers(initialState, multipathAngles); end
Основным циклом симуляции является for цикл с вложенным for цикл. Первый цикл выполняется в gpsFs, который является уровнем измерения GPS. Вложенный цикл выполняется в imuFs, который является частотой дискретизации IMU. Каждый осциллограф обновляется на уровне частоты дискретизации IMU.
numsamples = numel(trueOrientation); numGPSSamples = numsamples/imuSamplesPerGPS; % Log data for final metric computation. estPositionNoCheck = zeros(numsamples, 3); estOrientationNoCheck = quaternion.zeros(numsamples, 1); estPosition = zeros(numsamples, 3); estOrientation = quaternion.zeros(numsamples, 1); % Threshold for outlier residuals. residualThreshold = 6; idx = 0; for sampleIdx = 1:numGPSSamples % Predict loop at IMU update frequency. for i = 1:imuSamplesPerGPS idx = idx + 1; % Use the predict method to estimate the filter state based % on the accelData and gyroData arrays. predict(gndFusionWithDetection, accel(idx,:), gyro(idx,:)); predict(gndFusionNoDetection, accel(idx,:), gyro(idx,:)); % Log the estimated orientation and position. [estPositionNoCheck(idx,:), estOrientationNoCheck(idx,:)] ... = pose(gndFusionWithDetection); [estPosition(idx,:), estOrientation(idx,:)] ... = pose(gndFusionNoDetection); % Update the pose viewer. if usePoseView viewerWithDetection(estPositionNoCheck(idx,:), ... estOrientationNoCheck(idx,:), ... truePosition(idx,:), trueOrientation(idx,:)); viewerNoDetection(estPosition(idx,:), ... estOrientation(idx,:), truePosition(idx,:), ... trueOrientation(idx,:)); end end % This next section of code runs at the GPS sample rate. % Update the filter states based on the GPS data. fusegps(gndFusionWithDetection, lla(sampleIdx,:), Rpos, ... gpsVel(sampleIdx,:), Rvel); % Check the normalized residual of the current GPS reading. If the % value is too large, it is considered an outlier and disregarded. [res, resCov] = residualgps(gndFusionNoDetection, lla(sampleIdx,:), ... Rpos, gpsVel(sampleIdx,:), Rvel); normalizedRes = res(1:3) ./ sqrt( diag(resCov(1:3,1:3)).' ); if (all(abs(normalizedRes) <= residualThreshold)) % Update the filter states based on the GPS data. fusegps(gndFusionNoDetection, lla(sampleIdx,:), Rpos, ... gpsVel(sampleIdx,:), Rvel); if usePoseView set(annoHandle, 'String', 'Outlier status: none', ... 'EdgeColor', 'k'); end else if usePoseView set(annoHandle, 'String', 'Outlier status: detected', ... 'EdgeColor', 'r'); end end end
Вычислите ошибку положения для обеих оценок фильтра. Существует увеличение ошибки положения в фильтре, который не проверяет ни на какие выбросы в измерениях GPS.
% Calculate position errors. posdNoCheck = estPositionNoCheck - truePosition; posd = estPosition - truePosition; % Plot results. t = (0:size(posd,1)-1).' ./ imuFs; figure('Units', 'normalized', 'Position', [0.2615 0.2833 0.4552 0.3700]) subplot(1, 2, 1) plot(t, posdNoCheck) ax = gca; yLims = get(ax, 'YLim'); hold on mi = multipathIndices; fill([t(mi(1)), t(mi(1)), t(mi(2)), t(mi(2))], [7 -5 -5 7], ... [1 0 0],'FaceAlpha', 0.2); set(ax, 'YLim', yLims); title('Position Error (No outlier removal)') xlabel('time (s)') ylabel('error (m)') legend('x', 'y', 'z', sprintf('outlier\nregion')) subplot(1, 2, 2) plot(t, posd) ax = gca; yLims = get(ax, 'YLim'); hold on mi = multipathIndices; fill([t(mi(1)), t(mi(1)), t(mi(2)), t(mi(2))], [7 -5 -5 7], ... [1 0 0],'FaceAlpha', 0.2); set(ax, 'YLim', yLims); title('Position Error (Outlier removal)') xlabel('time (s)') ylabel('error (m)') legend('x', 'y', 'z', sprintf('outlier\nregion'))
residualgps объектная функция может использоваться, чтобы обнаружить потенциальные выбросы в измерениях датчика перед использованием их, чтобы обновить состояния фильтра insfilterNonholonomic объект. Другие объекты фильтра оценки положения такой как, insfilterMARG, insfilterAsync, и insfilterErrorState также имейте функции подобного объекта, чтобы вычислить остаточные значения измерения датчика.
