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.