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 insfilterAsync para reducir el error de estimación

contraer todo en la página

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 insfilterAsync, filter y los ruidos de medición para reducir el error de estimación de estado de raíz cuadrada media (RMS) entre los datos del sensor fusionado y la ground-truth. La función también devuelve el ruido de medición sintonizado, 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.

ejemplo

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

Ejemplos

contraer todo

Abrir script en vivo

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

load('insfilterAsyncTuneData.mat');

Cree cronogramas para los datos de los sensores y los datos de verdad.

sensorData = timetable(Accelerometer, Gyroscope, ...
    Magnetometer, GPSPosition, GPSVelocity, 'SampleRate', 100);
groundTruth = timetable(Orientation, Position, ...
    'SampleRate', 100);

Crea un objeto de filtro insfilterAsync que tenga algunas propiedades de ruido.

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

Cree un objeto de configuración de sintonizador para el filtro. Establezca el máximo de iteraciones en dos. Además, establezca los parámetros ajustables como propiedades no especificadas.

config = tunerconfig('insfilterAsync','MaxIterations',8);
config.TunableParameters = setdiff(config.TunableParameters, ...
    {'GeomagneticVectorNoise', 'AccelerometerBiasNoise', ...
    'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});
config.TunableParameters
ans = 1×10 string
    "AccelerationNoise"    "AccelerometerNoise"    "AngularVelocityNoise"    "GPSPositionNoise"    "GPSVelocityNoise"    "GyroscopeNoise"    "MagnetometerNoise"    "PositionNoise"    "QuaternionNoise"    "VelocityNoise"

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

measNoise = tunernoise('insfilterAsync')
measNoise = struct with fields:
    AccelerometerNoise: 1
        GyroscopeNoise: 1
     MagnetometerNoise: 1
      GPSPositionNoise: 1
      GPSVelocityNoise: 1

Ajusteel filtro y obtenga los parámetros ajustados.

tunedParams = tune(filter,measNoise,sensorData,groundTruth,config);
    Iteration    Parameter               Metric
    _________    _________               ______
    1            AccelerationNoise       2.1345
    1            AccelerometerNoise      2.1264
    1            AngularVelocityNoise    1.9659
    1            GPSPositionNoise        1.9341
    1            GPSVelocityNoise        1.8420
    1            GyroscopeNoise          1.7589
    1            MagnetometerNoise       1.7362
    1            PositionNoise           1.7362
    1            QuaternionNoise         1.7218
    1            VelocityNoise           1.7218
    2            AccelerationNoise       1.7190
    2            AccelerometerNoise      1.7170
    2            AngularVelocityNoise    1.6045
    2            GPSPositionNoise        1.5948
    2            GPSVelocityNoise        1.5323
    2            GyroscopeNoise          1.4803
    2            MagnetometerNoise       1.4703
    2            PositionNoise           1.4703
    2            QuaternionNoise         1.4632
    2            VelocityNoise           1.4632
    3            AccelerationNoise       1.4596
    3            AccelerometerNoise      1.4548
    3            AngularVelocityNoise    1.3923
    3            GPSPositionNoise        1.3810
    3            GPSVelocityNoise        1.3322
    3            GyroscopeNoise          1.2998
    3            MagnetometerNoise       1.2976
    3            PositionNoise           1.2976
    3            QuaternionNoise         1.2943
    3            VelocityNoise           1.2943
    4            AccelerationNoise       1.2906
    4            AccelerometerNoise      1.2836
    4            AngularVelocityNoise    1.2491
    4            GPSPositionNoise        1.2258
    4            GPSVelocityNoise        1.1880
    4            GyroscopeNoise          1.1701
    4            MagnetometerNoise       1.1698
    4            PositionNoise           1.1698
    4            QuaternionNoise         1.1688
    4            VelocityNoise           1.1688
    5            AccelerationNoise       1.1650
    5            AccelerometerNoise      1.1569
    5            AngularVelocityNoise    1.1454
    5            GPSPositionNoise        1.1100
    5            GPSVelocityNoise        1.0778
    5            GyroscopeNoise          1.0709
    5            MagnetometerNoise       1.0675
    5            PositionNoise           1.0675
    5            QuaternionNoise         1.0669
    5            VelocityNoise           1.0669
    6            AccelerationNoise       1.0634
    6            AccelerometerNoise      1.0549
    6            AngularVelocityNoise    1.0549
    6            GPSPositionNoise        1.0180
    6            GPSVelocityNoise        0.9866
    6            GyroscopeNoise          0.9810
    6            MagnetometerNoise       0.9775
    6            PositionNoise           0.9775
    6            QuaternionNoise         0.9768
    6            VelocityNoise           0.9768
    7            AccelerationNoise       0.9735
    7            AccelerometerNoise      0.9652
    7            AngularVelocityNoise    0.9652
    7            GPSPositionNoise        0.9283
    7            GPSVelocityNoise        0.8997
    7            GyroscopeNoise          0.8947
    7            MagnetometerNoise       0.8920
    7            PositionNoise           0.8920
    7            QuaternionNoise         0.8912
    7            VelocityNoise           0.8912
    8            AccelerationNoise       0.8885
    8            AccelerometerNoise      0.8811
    8            AngularVelocityNoise    0.8807
    8            GPSPositionNoise        0.8479
    8            GPSVelocityNoise        0.8238
    8            GyroscopeNoise          0.8165
    8            MagnetometerNoise       0.8165
    8            PositionNoise           0.8165
    8            QuaternionNoise         0.8159
    8            VelocityNoise           0.8159

Fusione los datos del sensor utilizando el filtro ajustado.

dt = seconds(diff(groundTruth.Time));
N = size(sensorData,1);
qEst = quaternion.zeros(N,1);
posEst = zeros(N,3);
% Iterate the filter for prediction and correction using sensor data.
for ii=1:N
    if ii ~= 1
        predict(filter, dt(ii-1));
    end
    if all(~isnan(Accelerometer(ii,:)))
        fuseaccel(filter,Accelerometer(ii,:), ...
            tunedParams.AccelerometerNoise);
    end
    if all(~isnan(Gyroscope(ii,:)))
        fusegyro(filter, Gyroscope(ii,:), ...
            tunedParams.GyroscopeNoise);
    end
    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
    [posEst(ii,:), qEst(ii,:)] = pose(filter);
end

Calcule los errores RMS. 

orientationError = rad2deg(dist(qEst, Orientation));
rmsorientationError = sqrt(mean(orientationError.^2))
rmsorientationError = 2.7801

positionError = sqrt(sum((posEst - Position).^2, 2));
rmspositionError = sqrt(mean( positionError.^2))
rmspositionError = 0.5966

Visualizar los resultados

figure();
t = (0:N-1)./ groundTruth.Properties.SampleRate;
subplot(2,1,1)
plot(t, positionError, 'b');
title("Tuned insfilterAsync" + newline + "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t, orientationError, 'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

Argumentos de entrada

contraer todo

Objeto de filtro, especificado como un objeto insfilterAsync.

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 campoDescripción
AccelerometerNoiseVarianza del ruido del acelerómetro, especificada como un escalar en (m/s2)2
GyroscopeNoiseVarianza del ruido del giroscopio, especificada como un escalar en (rad/s)2
MagnetometerNoiseVarianza del ruido del magnetómetro, especificada como un escalar en (μT)2
GPSPositionNoiseVarianza del ruido de posición GPS, especificada como un escalar en m2
GPSVelocityNoiseVarianza del ruido de velocidad del GPS, especificada como un escalar en (m/s)2

Datos del sensor, especificados como timetable. En cada fila, la hora y los datos del sensor se especifican como:

  • Time — Hora en que se obtienen los datos, especificada como un escalar en segundos.

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

  • Gyroscope — Datos del giroscopio, especificados como un vector de 1 por 3 de escalares 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 1 por 3 de latitud en grados, longitud en grados y altitud en metros.

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

Si un sensor no produce mediciones, especifique la entrada correspondiente como NaN. Si establece la propiedad Cost de la entrada de configuración del sintonizador, config, en Custom, podrá usar otros tipos de datos para la entrada sensorData según su elección.

Datos de verdad fundamental, especificados como timetable. 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 una quaternion o una matriz de rotación de 3 por 3.

  • AngularVelocity — Velocidad angular en el marco del cuerpo, especificada como un vector de 1 por 3 de escalares en rad/s.

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

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

  • Acceleration — Aceleración en el marco de navegación, especificada como un vector de 1 por 3 de escalares en m2/s.

  • AccelerometerBias — Sesgo del ángulo delta del acelerómetro en el marco del cuerpo, especificado como un vector de 1 por 3 de escalares en m2/s.

  • GyroscopeBias — Sesgo del ángulo delta del giroscopio en el marco del cuerpo, especificado como un vector de 1 por 3 de escalares en rad/s.

  • GeomagneticFieldVector — Vector del 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 el marco del cuerpo, especificado como un vector de 1 por 3 de escalares 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 la 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 los mismos pasos de tiempo.

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

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

Argumentos de salida

contraer todo

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

Nombre del campoDescripción
AccelerometerNoiseVarianza del ruido del acelerómetro, especificada como un escalar en (m2/s)2
GyroscopeNoiseVarianza del ruido del giroscopio, especificada como un escalar en (rad/s)2
MagnetometerNoiseVarianza del ruido del magnetómetro, especificada como un escalar en (μT)2
GPSPositionNoiseVarianza del ruido de posición GPS, especificada como un escalar en m2
GPSVelocityNoiseVarianza del ruido de 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 R2020b

Consulte también

|