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

Desde R2021a

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 insfilterErrorState, 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.

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

ejemplo

Ejemplos

contraer todo

Abrir script en vivo

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

load('insfilterErrorStateTuneData.mat');

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

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

Crea un objeto de filtro insfilterErrorState.

filter = insfilterErrorState('State',initialState, ...
    'StateCovariance',initialStateCovariance);

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

cfg = tunerconfig('insfilterErrorState','MaxIterations',40);
measNoise = tunernoise('insfilterErrorState')
measNoise = struct with fields:
    MVOOrientationNoise: 1
       MVOPositionNoise: 1
       GPSPositionNoise: 1
       GPSVelocityNoise: 1

Ajusteel filtro y obtenga los parámetros ajustados.

tunedmn = tune(filter,measNoise,sensorData, ...
    groundTruth,cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        4.1291
    1            GyroscopeNoise            4.1291
    1            AccelerometerBiasNoise    4.1290
    1            GyroscopeBiasNoise        4.1290
    1            GPSPositionNoise          4.0213
    1            GPSVelocityNoise          4.0051
    1            MVOPositionNoise          3.9949
    1            MVOOrientationNoise       3.9886
    2            AccelerometerNoise        3.9886
    2            GyroscopeNoise            3.9886
    2            AccelerometerBiasNoise    3.9886
    2            GyroscopeBiasNoise        3.9886
    2            GPSPositionNoise          3.8381
    2            GPSVelocityNoise          3.8268
    2            MVOPositionNoise          3.8219
    2            MVOOrientationNoise       3.8035
    3            AccelerometerNoise        3.8035
    3            GyroscopeNoise            3.8035
    3            AccelerometerBiasNoise    3.8035
    3            GyroscopeBiasNoise        3.8035
    3            GPSPositionNoise          3.6299
    3            GPSVelocityNoise          3.6276
    3            MVOPositionNoise          3.6241
    3            MVOOrientationNoise       3.5911
    4            AccelerometerNoise        3.5911
    4            GyroscopeNoise            3.5911
    4            AccelerometerBiasNoise    3.5911
    4            GyroscopeBiasNoise        3.5911
    4            GPSPositionNoise          3.1728
    4            GPSVelocityNoise          3.1401
    4            MVOPositionNoise          2.7686
    4            MVOOrientationNoise       2.6632
    5            AccelerometerNoise        2.6632
    5            GyroscopeNoise            2.6632
    5            AccelerometerBiasNoise    2.6632
    5            GyroscopeBiasNoise        2.6632
    5            GPSPositionNoise          2.3242
    5            GPSVelocityNoise          2.2291
    5            MVOPositionNoise          2.2291
    5            MVOOrientationNoise       2.0904
    6            AccelerometerNoise        2.0903
    6            GyroscopeNoise            2.0903
    6            AccelerometerBiasNoise    2.0903
    6            GyroscopeBiasNoise        2.0903
    6            GPSPositionNoise          2.0903
    6            GPSVelocityNoise          2.0141
    6            MVOPositionNoise          1.9952
    6            MVOOrientationNoise       1.8497
    7            AccelerometerNoise        1.8497
    7            GyroscopeNoise            1.8496
    7            AccelerometerBiasNoise    1.8496
    7            GyroscopeBiasNoise        1.8496
    7            GPSPositionNoise          1.8398
    7            GPSVelocityNoise          1.7528
    7            MVOPositionNoise          1.7362
    7            MVOOrientationNoise       1.5762
    8            AccelerometerNoise        1.5762
    8            GyroscopeNoise            1.5762
    8            AccelerometerBiasNoise    1.5762
    8            GyroscopeBiasNoise        1.5762
    8            GPSPositionNoise          1.5762
    8            GPSVelocityNoise          1.5107
    8            MVOPositionNoise          1.4786
    8            MVOOrientationNoise       1.3308
    9            AccelerometerNoise        1.3308
    9            GyroscopeNoise            1.3308
    9            AccelerometerBiasNoise    1.3308
    9            GyroscopeBiasNoise        1.3308
    9            GPSPositionNoise          1.3308
    9            GPSVelocityNoise          1.2934
    9            MVOPositionNoise          1.2525
    9            MVOOrientationNoise       1.1462
    10           AccelerometerNoise        1.1462
    10           GyroscopeNoise            1.1462
    10           AccelerometerBiasNoise    1.1462
    10           GyroscopeBiasNoise        1.1462
    10           GPSPositionNoise          1.1443
    10           GPSVelocityNoise          1.1332
    10           MVOPositionNoise          1.0964
    10           MVOOrientationNoise       1.0382
    11           AccelerometerNoise        1.0382
    11           GyroscopeNoise            1.0382
    11           AccelerometerBiasNoise    1.0382
    11           GyroscopeBiasNoise        1.0382
    11           GPSPositionNoise          1.0348
    11           GPSVelocityNoise          1.0348
    11           MVOPositionNoise          1.0081
    11           MVOOrientationNoise       0.9734
    12           AccelerometerNoise        0.9734
    12           GyroscopeNoise            0.9734
    12           AccelerometerBiasNoise    0.9734
    12           GyroscopeBiasNoise        0.9734
    12           GPSPositionNoise          0.9693
    12           GPSVelocityNoise          0.9682
    12           MVOPositionNoise          0.9488
    12           MVOOrientationNoise       0.9244
    13           AccelerometerNoise        0.9244
    13           GyroscopeNoise            0.9244
    13           AccelerometerBiasNoise    0.9244
    13           GyroscopeBiasNoise        0.9244
    13           GPSPositionNoise          0.9203
    13           GPSVelocityNoise          0.9199
    13           MVOPositionNoise          0.9045
    13           MVOOrientationNoise       0.8846
    14           AccelerometerNoise        0.8846
    14           GyroscopeNoise            0.8846
    14           AccelerometerBiasNoise    0.8845
    14           GyroscopeBiasNoise        0.8845
    14           GPSPositionNoise          0.8807
    14           GPSVelocityNoise          0.8807
    14           MVOPositionNoise          0.8659
    14           MVOOrientationNoise       0.8501
    15           AccelerometerNoise        0.8501
    15           GyroscopeNoise            0.8501
    15           AccelerometerBiasNoise    0.8500
    15           GyroscopeBiasNoise        0.8500
    15           GPSPositionNoise          0.8457
    15           GPSVelocityNoise          0.8453
    15           MVOPositionNoise          0.8299
    15           MVOOrientationNoise       0.8173
    16           AccelerometerNoise        0.8173
    16           GyroscopeNoise            0.8173
    16           AccelerometerBiasNoise    0.8172
    16           GyroscopeBiasNoise        0.8172
    16           GPSPositionNoise          0.8122
    16           GPSVelocityNoise          0.8116
    16           MVOPositionNoise          0.7961
    16           MVOOrientationNoise       0.7858
    17           AccelerometerNoise        0.7858
    17           GyroscopeNoise            0.7858
    17           AccelerometerBiasNoise    0.7857
    17           GyroscopeBiasNoise        0.7857
    17           GPSPositionNoise          0.7807
    17           GPSVelocityNoise          0.7800
    17           MVOPositionNoise          0.7655
    17           MVOOrientationNoise       0.7572
    18           AccelerometerNoise        0.7572
    18           GyroscopeNoise            0.7572
    18           AccelerometerBiasNoise    0.7570
    18           GyroscopeBiasNoise        0.7570
    18           GPSPositionNoise          0.7525
    18           GPSVelocityNoise          0.7520
    18           MVOPositionNoise          0.7401
    18           MVOOrientationNoise       0.7338
    19           AccelerometerNoise        0.7337
    19           GyroscopeNoise            0.7337
    19           AccelerometerBiasNoise    0.7335
    19           GyroscopeBiasNoise        0.7335
    19           GPSPositionNoise          0.7293
    19           GPSVelocityNoise          0.7290
    19           MVOPositionNoise          0.7185
    19           MVOOrientationNoise       0.7140
    20           AccelerometerNoise        0.7138
    20           GyroscopeNoise            0.7138
    20           AccelerometerBiasNoise    0.7134
    20           GyroscopeBiasNoise        0.7134
    20           GPSPositionNoise          0.7086
    20           GPSVelocityNoise          0.7068
    20           MVOPositionNoise          0.6956
    20           MVOOrientationNoise       0.6926
    21           AccelerometerNoise        0.6922
    21           GyroscopeNoise            0.6922
    21           AccelerometerBiasNoise    0.6916
    21           GyroscopeBiasNoise        0.6916
    21           GPSPositionNoise          0.6862
    21           GPSVelocityNoise          0.6822
    21           MVOPositionNoise          0.6682
    21           MVOOrientationNoise       0.6667
    22           AccelerometerNoise        0.6660
    22           GyroscopeNoise            0.6660
    22           AccelerometerBiasNoise    0.6650
    22           GyroscopeBiasNoise        0.6650
    22           GPSPositionNoise          0.6605
    22           GPSVelocityNoise          0.6541
    22           MVOPositionNoise          0.6372
    22           MVOOrientationNoise       0.6368
    23           AccelerometerNoise        0.6356
    23           GyroscopeNoise            0.6356
    23           AccelerometerBiasNoise    0.6344
    23           GyroscopeBiasNoise        0.6344
    23           GPSPositionNoise          0.6324
    23           GPSVelocityNoise          0.6252
    23           MVOPositionNoise          0.6087
    23           MVOOrientationNoise       0.6087
    24           AccelerometerNoise        0.6075
    24           GyroscopeNoise            0.6075
    24           AccelerometerBiasNoise    0.6068
    24           GyroscopeBiasNoise        0.6068
    24           GPSPositionNoise          0.6061
    24           GPSVelocityNoise          0.6032
    24           MVOPositionNoise          0.6032
    24           MVOOrientationNoise       0.6032
    25           AccelerometerNoise        0.6017
    25           GyroscopeNoise            0.6017
    25           AccelerometerBiasNoise    0.6012
    25           GyroscopeBiasNoise        0.6012
    25           GPSPositionNoise          0.6010
    25           GPSVelocityNoise          0.6005
    25           MVOPositionNoise          0.6005
    25           MVOOrientationNoise       0.6005
    26           AccelerometerNoise        0.5992
    26           GyroscopeNoise            0.5992
    26           AccelerometerBiasNoise    0.5987
    26           GyroscopeBiasNoise        0.5987
    26           GPSPositionNoise          0.5983
    26           GPSVelocityNoise          0.5983
    26           MVOPositionNoise          0.5983
    26           MVOOrientationNoise       0.5983
    27           AccelerometerNoise        0.5975
    27           GyroscopeNoise            0.5975
    27           AccelerometerBiasNoise    0.5974
    27           GyroscopeBiasNoise        0.5974
    27           GPSPositionNoise          0.5973
    27           GPSVelocityNoise          0.5972
    27           MVOPositionNoise          0.5971
    27           MVOOrientationNoise       0.5971
    28           AccelerometerNoise        0.5971
    28           GyroscopeNoise            0.5971
    28           AccelerometerBiasNoise    0.5970
    28           GyroscopeBiasNoise        0.5970
    28           GPSPositionNoise          0.5970
    28           GPSVelocityNoise          0.5970
    28           MVOPositionNoise          0.5970
    28           MVOOrientationNoise       0.5970
    29           AccelerometerNoise        0.5970
    29           GyroscopeNoise            0.5970
    29           AccelerometerBiasNoise    0.5970
    29           GyroscopeBiasNoise        0.5970
    29           GPSPositionNoise          0.5970
    29           GPSVelocityNoise          0.5970
    29           MVOPositionNoise          0.5970
    29           MVOOrientationNoise       0.5970
    30           AccelerometerNoise        0.5969
    30           GyroscopeNoise            0.5969
    30           AccelerometerBiasNoise    0.5969
    30           GyroscopeBiasNoise        0.5969
    30           GPSPositionNoise          0.5969
    30           GPSVelocityNoise          0.5969
    30           MVOPositionNoise          0.5968
    30           MVOOrientationNoise       0.5968
    31           AccelerometerNoise        0.5968
    31           GyroscopeNoise            0.5968
    31           AccelerometerBiasNoise    0.5968
    31           GyroscopeBiasNoise        0.5968
    31           GPSPositionNoise          0.5968
    31           GPSVelocityNoise          0.5968
    31           MVOPositionNoise          0.5967
    31           MVOOrientationNoise       0.5967
    32           AccelerometerNoise        0.5967
    32           GyroscopeNoise            0.5967
    32           AccelerometerBiasNoise    0.5967
    32           GyroscopeBiasNoise        0.5967
    32           GPSPositionNoise          0.5967
    32           GPSVelocityNoise          0.5967
    32           MVOPositionNoise          0.5966
    32           MVOOrientationNoise       0.5966
    33           AccelerometerNoise        0.5966
    33           GyroscopeNoise            0.5966
    33           AccelerometerBiasNoise    0.5966
    33           GyroscopeBiasNoise        0.5966
    33           GPSPositionNoise          0.5966
    33           GPSVelocityNoise          0.5966
    33           MVOPositionNoise          0.5965
    33           MVOOrientationNoise       0.5965
    34           AccelerometerNoise        0.5965
    34           GyroscopeNoise            0.5965
    34           AccelerometerBiasNoise    0.5965
    34           GyroscopeBiasNoise        0.5965
    34           GPSPositionNoise          0.5965
    34           GPSVelocityNoise          0.5964
    34           MVOPositionNoise          0.5964
    34           MVOOrientationNoise       0.5964
    35           AccelerometerNoise        0.5964
    35           GyroscopeNoise            0.5964
    35           AccelerometerBiasNoise    0.5963
    35           GyroscopeBiasNoise        0.5963
    35           GPSPositionNoise          0.5963
    35           GPSVelocityNoise          0.5963
    35           MVOPositionNoise          0.5963
    35           MVOOrientationNoise       0.5963
    36           AccelerometerNoise        0.5963
    36           GyroscopeNoise            0.5963
    36           AccelerometerBiasNoise    0.5963
    36           GyroscopeBiasNoise        0.5963
    36           GPSPositionNoise          0.5963
    36           GPSVelocityNoise          0.5963
    36           MVOPositionNoise          0.5963
    36           MVOOrientationNoise       0.5963
    37           AccelerometerNoise        0.5963
    37           GyroscopeNoise            0.5963
    37           AccelerometerBiasNoise    0.5963
    37           GyroscopeBiasNoise        0.5963
    37           GPSPositionNoise          0.5962
    37           GPSVelocityNoise          0.5962
    37           MVOPositionNoise          0.5962
    37           MVOOrientationNoise       0.5962
    38           AccelerometerNoise        0.5962
    38           GyroscopeNoise            0.5962
    38           AccelerometerBiasNoise    0.5962
    38           GyroscopeBiasNoise        0.5962
    38           GPSPositionNoise          0.5962
    38           GPSVelocityNoise          0.5961
    38           MVOPositionNoise          0.5961
    38           MVOOrientationNoise       0.5961
    39           AccelerometerNoise        0.5961
    39           GyroscopeNoise            0.5961
    39           AccelerometerBiasNoise    0.5961
    39           GyroscopeBiasNoise        0.5961
    39           GPSPositionNoise          0.5961
    39           GPSVelocityNoise          0.5960
    39           MVOPositionNoise          0.5960
    39           MVOOrientationNoise       0.5960
    40           AccelerometerNoise        0.5960
    40           GyroscopeNoise            0.5960
    40           AccelerometerBiasNoise    0.5960
    40           GyroscopeBiasNoise        0.5960
    40           GPSPositionNoise          0.5960
    40           GPSVelocityNoise          0.5959
    40           MVOPositionNoise          0.5959
    40           MVOOrientationNoise       0.5959

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(GPSPosition(ii,1)))
        fusegps(filter,GPSPosition(ii,:), ...
            tunedmn.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedmn.GPSVelocityNoise);
    end
    if all(~isnan(MVOPosition(ii,1)))
        fusemvo(filter,MVOPosition(ii,:),tunedmn.MVOPositionNoise, ...
            MVOOrientation{ii},tunedmn.MVOOrientationNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Calcule los errores RMS.

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

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

Visualizar los resultados

figure;
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t, positionErrorTuned,'b');
title("Tuned insfilterErrorState" + 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)');

Figure contains 2 axes objects. Axes object 1 with title Tuned insfilterErrorState Euclidean Distance Position Error, xlabel Time (s), ylabel Position Error (meters) contains an object of type line. Axes object 2 with title Orientation Error, xlabel Time (s), ylabel Orientation Error (degrees) contains an object of type line.

Argumentos de entrada

contraer todo

Objeto de filtro, especificado como un objeto insfilterErrorState.

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
MVOOrientationNoiseCovarianza de la medición de la orientación de la odometría visual monocular, especificada como un escalar, un vector de 3 elementos o una matriz de 3 por 3 en rad2
MVOPositionNoiseCovarianza de medición de posición de MVO, especificada como un escalar, un vector de 3 elementos o una matriz de 3 por 3 en m2
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 una tabla. En cada fila, los datos del sensor se especifican como:

  • 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.

  • MVOOrienation — Orientación de la cámara con respecto al marco de navegación local, especificado como un cuaternión escalar o una matriz de rotación de 3 por 3. El cuaternión o matriz de rotación es una rotación de cuadro desde el cuadro de navegación local hasta el sistema de coordenadas de la cámara actual.

  • MVOPosition — Posición de la cámara en el marco de navegación local, especificada como un vector fila real de 3 elementos en metros.

  • 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 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 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 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 una 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 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.

  • 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.

  • VisualOdometryScale — Factor de escala de odometría visual, especificado como escalar.

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 el mismo número de filas.

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
MVOOrientationNoiseCovarianza de la medición de la orientación de la odometría visual monocular, especificada como un escalar, un vector de 3 elementos o una matriz de 3 por 3 en rad2
MVOPositionNoiseCovarianza de medición de posición de MVO, especificada como un escalar, un vector de 3 elementos o una matriz de 3 por 3 en m2
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 R2021a

Consulte también

|