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

tune

Ajuste los parámetros insfilterMARG para reducir el error de estimación

Desde R2021a

Sintaxis

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)

tunedMeasureNoise = tune(___,config)

Descripción

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) ajusta las propiedades del objeto de filtro insfilterMARG , filter y los ruidos de medición para reducir la raíz cuadrática media (RMS) error de estimación de estado entre los datos del sensor fusionado y ground-truth. La función también devuelve el ruido de medición ajustado, tunedMeasureNoise. La función utiliza los valores de propiedad en el filtro y el ruido de medición proporcionado en la estructura measureNoise como estimación inicial para el algoritmo de optimización.

tunedMeasureNoise = tune(___,config) especifica la configuración de ajuste basada en un objeto tunerconfig , config.

Ejemplos

Ajuste insfilterMARG para optimizar la estimación de pose

Cargue los datos registrados del sensor y los datos ground-truth.

load('insfilterMARGTuneData.mat');

Cree tablas para los datos del sensor y los datos de verdad.

sensorData = table(Accelerometer, Gyroscope, ...
    Magnetometer, GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);

Cree un objeto de filtro insfilterMARG que tenga algunas propiedades de ruido.

filter = insfilterMARG('State',initialState,...
        'StateCovariance',initialStateCovariance,...
        'AccelerometerBiasNoise',1e-7,...
        'GyroscopeBiasNoise',1e-7,...
        'MagnetometerBiasNoise',1e-7,...
        'GeomagneticVectorNoise',1e-7);

Cree un objeto de configuración de ajustador para el filtro. Establezca el máximo de iteraciones en ocho. Además, configure los parámetros ajustables.

cfg = tunerconfig('insfilterMARG', 'MaxIterations', 8);
cfg.TunableParameters = setdiff(cfg.TunableParameters, ...
    {'GeomagneticFieldVector', 'AccelerometerBiasNoise', ...
    'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});

Utilice la función de ruido del ajustador para obtener un conjunto de ruidos iniciales del sensor utilizados en el filtro.

measNoise = tunernoise('insfilterMARG')

measNoise = struct with fields:
    MagnetometerNoise: 1
     GPSPositionNoise: 1
     GPSVelocityNoise: 1

Ajusteel filtro y obtenga los parámetros ajustados.

tunedParams = tune(filter, measNoise, sensorData, ...
        groundTruth, cfg);

    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        2.5701
    1            GPSPositionNoise          2.5446
    1            GPSVelocityNoise          2.5279
    1            GeomagneticVectorNoise    2.5268
    1            GyroscopeNoise            2.5268
    1            MagnetometerNoise         2.5204
    2            AccelerometerNoise        2.5203
    2            GPSPositionNoise          2.4908
    2            GPSVelocityNoise          2.4695
    2            GeomagneticVectorNoise    2.4684
    2            GyroscopeNoise            2.4684
    2            MagnetometerNoise         2.4615
    3            AccelerometerNoise        2.4615
    3            GPSPositionNoise          2.4265
    3            GPSVelocityNoise          2.4000
    3            GeomagneticVectorNoise    2.3988
    3            GyroscopeNoise            2.3988
    3            MagnetometerNoise         2.3911
    4            AccelerometerNoise        2.3911
    4            GPSPositionNoise          2.3500
    4            GPSVelocityNoise          2.3164
    4            GeomagneticVectorNoise    2.3153
    4            GyroscopeNoise            2.3153
    4            MagnetometerNoise         2.3068
    5            AccelerometerNoise        2.3068
    5            GPSPositionNoise          2.2587
    5            GPSVelocityNoise          2.2166
    5            GeomagneticVectorNoise    2.2154
    5            GyroscopeNoise            2.2154
    5            MagnetometerNoise         2.2063
    6            AccelerometerNoise        2.2063
    6            GPSPositionNoise          2.1505
    6            GPSVelocityNoise          2.0981
    6            GeomagneticVectorNoise    2.0971
    6            GyroscopeNoise            2.0971
    6            MagnetometerNoise         2.0875
    7            AccelerometerNoise        2.0874
    7            GPSPositionNoise          2.0240
    7            GPSVelocityNoise          1.9601
    7            GeomagneticVectorNoise    1.9594
    7            GyroscopeNoise            1.9594
    7            MagnetometerNoise         1.9499
    8            AccelerometerNoise        1.9499
    8            GPSPositionNoise          1.8802
    8            GPSVelocityNoise          1.8035
    8            GeomagneticVectorNoise    1.8032
    8            GyroscopeNoise            1.8032
    8            MagnetometerNoise         1.7959

Fusione los datos del sensor utilizando el filtro ajustado.

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
posEstTuned = zeros(N,3);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(Magnetometer(ii,1)))
        fusemag(filter,Magnetometer(ii,:),...
            tunedParams.MagnetometerNoise);
    end
    if all(~isnan(GPSPosition(ii,1)))
        fusegps(filter,GPSPosition(ii,:),...
            tunedParams.GPSPositionNoise,GPSVelocity(ii,:),...
            tunedParams.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Calcule los errores RMS.

orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))

rmsOrientationErrorTuned = 0.8580

positionErrorTuned = sqrt(sum((posEstTuned - Position).^2,2));
rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))

rmsPositionErrorTuned = 1.7946

Visualiza los resultados.

figure();
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t,positionErrorTuned,'b');
title("Tuned insfilterMARG" + newline + ...
    "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t, orientationErrorTuned,'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

Argumentos de entrada

filter — Objeto de filtro

infilterMARG objeto

Objeto de filtro, especificado como objeto insfilterMARG .

measureNoise — Ruido de medición

estructura

Ruido de medición, especificado como estructura. La función utiliza la entrada de ruido de medición como estimación inicial para ajustar el ruido de medición. La estructura debe contener estos campos:

Nombre del campo	Descripción
MagnetometerNoise	Varianza del ruido del magnetómetro, especificada como escalar en (μT) 2
GPSPositionNoise	Variación del ruido de la posición GPS, especificada como un escalar en m2
GPSVelocityNoise	Variación del ruido de la velocidad del GPS, especificada como un escalar en (m/s) 2

sensorData — Datos del sensor

table

Datos del sensor, especificados como table. En cada fila, los datos del sensor se especifican como:

• Accelerometer — Datos del acelerómetro, especificados como un vector de escalares de 1 por 3 en m²/s.

• Gyroscope — Datos del giroscopio, especificados como un vector de escalares de 1 por 3 en rad/s.

• Magnetometer — Datos del magnetómetro, especificados como un vector de escalares de 1 por 3 en μT.

• GPSPosition — Datos de posición GPS, especificados como un vector de escalares de 1 por 3 en [grados, grados, metros].

• GPSVelocity — Datos de velocidad GPS, especificados como un vector de escalares de 1 por 3 en m/s.

Si el sensor GPS no produce mediciones completas, especifique la entrada correspondiente para GPSPosition y/o GPSVelocity como NaN. Si establece la propiedad Cost de la entrada de configuración del ajustador, config, en Custom, entonces puede usar otros tipos de datos para la sensorData entrada según su elección.

groundTruth — Datos ground-truth

table

Datos reales, especificados como table. En cada fila, la tabla puede contener opcionalmente cualquiera de estas variables:

• Orientation — Orientación desde el marco de navegación hasta el marco del cuerpo, especificada como quaternion o una matriz de rotación de 3 por 3.

• Position — Posición en el marco de navegación, especificada como un vector de escalares de 1 por 3 en metros.

• Velocity — Velocidad en el marco de navegación, especificada como un vector de escalares de 1 por 3 en m/s.

• DeltaAngleBias — Sesgo del ángulo delta, especificado como un vector de escalares de 1 por 3 en radianes.

• DeltaVelocityBias — Sesgo de velocidad delta, especificado como un vector de escalares de 1 por 3 en m/s.

• GeomagneticFieldVector — Vector de campo geomagnético en el marco de navegación, especificado como un vector de escalares de 1 por 3.

• MagnetometerBias — Sesgo del magnetómetro en la estructura del cuerpo, especificado como un vector de escalares de 1 por 3 en μT.

La función procesa cada fila de las tablas sensorData y groundTruth secuencialmente para calcular la estimación del estado y el error RMS a partir de ground-truth. Las variables de estado que no están presentes en la entrada groundTruth se ignoran para la comparación. Las tablas sensorData y groundTruth deben tener el mismo número de filas.

Si establece la propiedad Cost de la entrada de configuración del ajustador, config, en Custom, entonces puede usar otros tipos de datos para la groundTruth entrada según su elección.

config — Configuración del ajustador

tunerconfig objeto

Configuración del ajustador, especificada como un objeto tunerconfig .

Argumentos de salida

tunedMeasureNoise — Ruido de medición ajustado

estructura

Ruido de medición ajustado, devuelto como una estructura. La estructura contiene estos campos.

Nombre del campo	Descripción
MagnetometerNoise	Variación del ruido del magnetómetro, especificada como escalar en (μT) 2
GPSPositionNoise	Variación del ruido de la posición GPS, especificada como un escalar en m2
GPSVelocityNoise	Variación del ruido de la velocidad del GPS, especificada como un escalar en (m/s) 2

Referencias

[1] Abbeel, P., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Kalman Filters. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.

Historial de versiones

Introducido en R2021a

Consulte también

tunerconfig | tunernoise