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

Localización de peatones basada en gráficos de factores con sensores IMU y GPS

Este ejemplo muestra cómo estimar la posición de un peatón utilizando datos de sensores registrados de una unidad de medición inercial (IMU) y un receptor del Sistema de posicionamiento global (GPS) y un gráfico de factores.

Un gráfico de factores es un gráfico bipartito o bigrafo. Un gráfico bipartito contiene vértices que se pueden dividir en dos conjuntos disjuntos e independientes. El primer conjunto de vértices son factores y contienen mediciones fijas, generalmente de un sensor, y una matriz de incertidumbre correspondiente para cada medición. El segundo conjunto de vértices son nodos de estado y se actualizan durante la optimización del gráfico de factores. El objetivo de la optimización es minimizar cada función de coste definida para cada conexión de un factor y un estado.

Este asistente de ejemplo incluye una función que traza un gráfico de factores básico que consta de cinco factores y tres nodos de estado, que se muestran como cuadrados azules y círculos rojos respectivamente:

exampleHelperPlotBasicFactorGraph

El primer factor, una medición GPS en el momento t0, está conectado al primer nodo de estado que representa una pose en el mismo momento t0. El segundo factor son todas las mediciones de IMU entre el tiempo t0 y t1, y se conecta al primer nodo de estado, una pose en t0, y el segundo nodo de estado, una pose en t1. Durante la optimización del gráfico de factores, los factores GPS e IMU restringen sus estados de pose conectados de modo que el nuevo valor de estado minimice el valor de la función de coste correspondiente a cada factor.

Cargar el archivo de registro del sensor IMU y GPS

Cargue un archivo MAT que contenga datos de sensores IMU y GPS, pedestrianSensorDataIMUGPS, y extraiga la frecuencia de muestreo y los valores de ruido para la IMU, la frecuencia de muestreo para la optimización del gráfico de factores y la posición estimada informada por el sistema integrado. Filtros de los sensores. Este archivo MAT se creó registrando datos de un sensor sostenido por un peatón que caminaba por un sendero alrededor del campus de MathWorks .

load("pedestrianSensorDataIMUGPS.mat","sensorData","imuFs","imuNoise", ...
     "numGPSSamplesPerOptim","posLLA","initOrient","initPos","numIMUSamplesPerGPS");

Crear gráfico de factores

Este diagrama muestra la estructura de un gráfico de factores para procesar los datos del sensor. Para cada iteración del bucle en la sección Estimate Pedestrian Position , el paso de tiempo se actualiza y luego avanza a la siguiente lectura de GPS. El proceso se repite hasta que se hayan procesado todos los datos del sensor. El diagrama muestra la relación entre los nodos de estado y los factores para un par de pasos de tiempo o una iteración del bucle for .

Cree un vector para realizar un seguimiento de los ID de los nodos actuales para cada par de pasos de tiempo.

nodesPerTimeStep = [exampleHelperFactorGraphNodes.Pose; ...
                    exampleHelperFactorGraphNodes.Velocity; ...
                    exampleHelperFactorGraphNodes.IMUBias];
numNodesPerTimeStep = numel(nodesPerTimeStep);
currNodeIDs = nodesPerTimeStep + [0 numNodesPerTimeStep];
currNodeIDs = currNodeIDs(:).';

Cree variables de índice de tiempo para acceder fácilmente al vector de ID de nodo actual. Los ID de nodo iniciales para los sesgos de pose, velocidad y IMU se definen en la función auxiliar exampleHelperFactorGraphNodes. Esto aclara el proceso de inicialización de este ejemplo.

t0IndexPose = exampleHelperFactorGraphNodes.Pose;
t1IndexPose = t0IndexPose + numNodesPerTimeStep;
t0IndexVel = exampleHelperFactorGraphNodes.Velocity;
t1IndexVel = t0IndexVel + numNodesPerTimeStep;
t0IndexBias = exampleHelperFactorGraphNodes.IMUBias;
t1IndexBias = t0IndexBias + numNodesPerTimeStep;

Cree el gráfico de factores como un objeto factorGraph y los tres factores anteriores como factorPoseSE3Prior, factorVelocity3Prior y factorIMUBiasPrior objeto. Inicialice la posición inicial, la orientación, la velocidad y los sesgos de IMU utilizando la posición inicial y la orientación del filtro integrado. La matriz de información Information utiliza valores grandes porque provienen de mediciones ground-truth. Agregue los factores anteriores al gráfico de factores usando la función addFactor .

G = factorGraph;
prevPose =  [initPos compact(initOrient)]; % x y z | qw qx qy qz
prevVel = [0 0 0]; % m/s
prevBias = [0 0 0 0 0 0]; % Gyroscope and accelerometer biases.

fPosePrior = factorPoseSE3Prior(currNodeIDs(t0IndexPose),Measurement=prevPose, ...
    Information=diag([4e4 4e4 4e4 1e4 1e4 1e4]));
fVelPrior = factorVelocity3Prior(currNodeIDs(t0IndexVel),Measurement=prevVel, ...
    Information=100*eye(3));
fBiasPrior = factorIMUBiasPrior(currNodeIDs(t0IndexBias),Measurement=prevBias, ...
    Information=1e6*eye(6));

addFactor(G,fPosePrior);
addFactor(G,fVelPrior);
addFactor(G,fBiasPrior);

Establezca los nodos de estado conectados a los factores anteriores a los valores iniciales.

nodeState(G,currNodeIDs(t0IndexPose),prevPose);
nodeState(G,currNodeIDs(t0IndexVel),prevVel);
nodeState(G,currNodeIDs(t0IndexBias),prevBias);

Obtenga el origen geodésico local para el marco de navegación a partir de la lectura GPS inicial.

lla0 = sensorData{1}.GPSReading;

Cree opciones de solver como un objeto factorGraphSolverOptions , para usar al optimizar el gráfico.

opts = factorGraphSolverOptions(MaxIterations=100);

Cree un vector para almacenar todos los números de identificación de los nodos de estado que corresponden a las poses.

numGPSSamples = numel(sensorData);
poseIDs = zeros(1,numGPSSamples);

Estimar la posición del peatón

Procese los datos del sensor registrados y extraiga la estimación de la posición de los peatones del gráfico de factor optimizado. Cree un gráfico para visualizar la posición estimada.

visHelper = exampleHelperVisualizeFactorGraphAndFilteredPosition;

Realice estas operaciones para cada muestra de GPS en el registro de datos del sensor:

1. Almacene la ID de pose actual.

2. Cree un factor IMU.

3. Crea un factor GPS.

4. Suma los factores al gráfico de factores.

5. Establezca los estados iniciales de los nodos utilizando estimaciones de estado anteriores y una predicción de estado a partir de las mediciones de IMU.

6. Optimice el gráfico de factores.

7. Avance un paso de tiempo actualizando las estimaciones del estado anterior al estado actual e incrementando los ID de los nodos actuales.

for ii = 1:numGPSSamples
    % 1. Store the current pose ID. 
    poseIDs(ii) = currNodeIDs(t0IndexPose);
    
    % 2. Create an IMU factor. 
    fIMU = factorIMU(currNodeIDs,imuFs, ...
        imuNoise.GyroscopeBiasNoise, ...
        imuNoise.AccelerometerBiasNoise, ...
        imuNoise.GyroscopeNoise, ...
        imuNoise.AccelerometerNoise, ...
        sensorData{ii}.GyroReadings,sensorData{ii}.AccelReadings);

    % 3. Create a GPS factor. 
    fGPS = factorGPS(currNodeIDs(t0IndexPose),HDOP=sensorData{ii}.HDoP, ...
        VDOP=sensorData{ii}.VDoP, ...
        ReferenceLocation=lla0, ...
        Location=sensorData{ii}.GPSReading);

    % 4. Add the factors to the graph. 
    addFactor(G,fIMU);
    addFactor(G,fGPS);

    % 5. Set initial node states using previous state estimates and a state prediction from the IMU measurements.
    [predictedPose,predictedVel] = predict(fIMU,prevPose,prevVel,prevBias);
    nodeState(G,currNodeIDs(t1IndexPose),predictedPose);
    nodeState(G,currNodeIDs(t1IndexVel),predictedVel);
    nodeState(G,currNodeIDs(t1IndexBias),prevBias);
    
    % 6. Optimize the factor graph.
    if (mod(ii,numGPSSamplesPerOptim) == 0) || (ii == numGPSSamples)
        solnInfo = optimize(G,opts);

        % Visualize the current position estimate.
        updatePlot(visHelper,ii,G,poseIDs,posLLA,lla0,numIMUSamplesPerGPS);
        drawnow
    end

    % 7. Advance one time step by updating the previous state estimates to the current ones and incrementing the current node IDs.
    prevPose = nodeState(G,currNodeIDs(t1IndexPose));
    prevVel = nodeState(G,currNodeIDs(t1IndexVel));
    prevBias = nodeState(G,currNodeIDs(t1IndexBias));

    currNodeIDs = currNodeIDs + numNodesPerTimeStep;
end

El gráfico de factores estima con éxito la posición del peatón en comparación con la posición estimada por el filtro a bordo. Para verificar esto, calcule el error RMS de la posición tridimensional trazada:

onBoardPos = lla2enu(posLLA,lla0,"ellipsoid");
onBoardPos = onBoardPos(cumsum(numIMUSamplesPerGPS-1),:); % Convert to sampling rate of factor graph.
estPos = zeros(numel(poseIDs),3);
for ii = 1:numel(poseIDs)
    currPose = nodeState(G, poseIDs(ii));
    estPos(ii,:) = currPose(1:3);
end
posDiff = estPos - onBoardPos;
posDiff(isnan(posDiff(:,1)),:) = []; % Remove any missing position readings from the onboard filter.
posRMS = sqrt(mean(posDiff.^2));
fprintf("3D Position RMS: X: %.2f, Y: %.2f, Z: %.2f (m)\n",posRMS(1),posRMS(2),posRMS(3));

3D Position RMS: X: 1.02, Y: 0.71, Z: 3.64 (m)