Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Detecte errores de lectura de GPS de rutas múltiples mediante filtrado residual en la fusión de sensores inerciales

Este ejemplo muestra cómo utilizar la función de objeto residualgps y el filtrado residual para detectar cuando las nuevas mediciones del sensor pueden no ser consistentes con el estado actual del filtro.

Cargar datos de trayectoria y sensores

Cargue el archivo MAT loggedDataWithMultipath.mat. Este archivo contiene datos IMU y GPS simulados, así como la posición ground-truth y la orientación de una trayectoria circular. Los datos del GPS contienen errores debido a errores de trayectos múltiples en una sección de la trayectoria. Estos errores se modelaron añadiendo ruido blanco a los datos del GPS para simular los efectos de un cañón urbano.

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];

Filtro de fusión

Cree dos filtros de estimación de pose utilizando el objeto insfilterNonholonomic . Utilice un filtro para procesar todas las lecturas del sensor. Utilice el otro filtro para procesar únicamente las lecturas del sensor que no se consideran valores atípicos.

% 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);

Inicializar ámbitos

El alcance HelperPoseViewer permite una visualización tridimensional que compara la estimación del filtro y ground-truth. El uso de múltiples ámbitos puede ralentizar la simulación. Para deshabilitar los ámbitos, establezca la variable lógica correspondiente en false.

usePoseView = true;  % Turn on the 3D pose viewer

if usePoseView
    [viewerWithDetection, viewerNoDetection, annoHandle] ...
        = helperCreatePoseViewers(initialState, multipathAngles);
end

Bucle de simulación

El bucle de simulación principal es un bucle for con un bucle for anidado. El primer bucle se ejecuta en gpsFs, que es la velocidad de medición del GPS. El bucle anidado se ejecuta a imuFs, que es la frecuencia de muestreo de IMU. Cada alcance se actualiza a la frecuencia de muestreo de 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

Cálculo de métricas de error

Calcule el error de posición para ambas estimaciones de filtro. Hay un aumento en el error de posición en el filtro que no busca valores atípicos en las mediciones del 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'))

Conclusión

La función de objeto residualgps se puede utilizar para detectar posibles valores atípicos en las mediciones del sensor antes de usarlos para actualizar los estados de filtro del objeto insfilterNonholonomic . Los otros objetos de filtro de estimación de pose, como insfilterMARG, insfilterAsync y insfilterErrorState también tienen funciones de objeto similares para calcular los residuos de medición del sensor.