Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

tune

Ajustez les paramètres insfilterNonholonomic pour réduire l'erreur d'estimation

réduire tous les éléments de la page

Syntaxe

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

Description

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) ajuste les propriétés de l'objet filtre insfilterNonholonomic , filter et les bruits de mesure pour réduire la moyenne quadratique (RMS) erreur d'estimation d'état entre les données du capteur fusionnées et la vérité terrain. La fonction renvoie également le bruit de mesure réglé, tunedMeasureNoise. La fonction utilise les valeurs des propriétés du filtre et le bruit de mesure fourni dans la structure measureNoise comme estimation initiale pour l'algorithme d'optimisation.

tunedMeasureNoise = tune(___,config) spécifie la configuration de réglage basée sur un objet tunerconfig , config.

exemple

Exemples

réduire tout

Ouvrir le live script

Chargez les données de capteur enregistrées et les données de vérité terrain.

load('insfilterNonholonomicTuneData.mat');

Créez des tableaux pour les données des capteurs et les données de vérité.

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

Créez un objet filtre insfilterNonholonimic .

filter = insfilterNonholonomic('State', initialState, ...
        'StateCovariance', initialStateCovariance, ...
        'DecimationFactor', 1);

Créez un objet de configuration de tuner pour le filtre. Définissez le nombre maximum d'itérations sur 30. 

config = tunerconfig('insfilterNonholonomic','MaxIterations',30);

Utilisez la fonction tunernoise pour obtenir un ensemble de bruits de capteur initiaux utilisés dans le filtre.

measNoise = tunernoise('insfilterNonholonomic')
measNoise = struct with fields:
    GPSPositionNoise: 1
    GPSVelocityNoise: 1

Ajustez le filtre et obtenez le bruit de mesure réglé.

tunedNoise = tune(filter,measNoise,sensorData,groundTruth,config);
    Iteration    Parameter                       Metric
    _________    _________                       ______
    1            GyroscopeNoise                  3.4877
    1            AccelerometerNoise              3.3961
    1            GyroscopeBiasNoise              3.3961
    1            GyroscopeBiasDecayFactor        3.3961
    1            AccelerometerBiasNoise          3.3961
    1            AccelerometerBiasDecayFactor    3.3961
    1            ZeroVelocityConstraintNoise     3.3935
    1            GPSPositionNoise                3.2848
    1            GPSVelocityNoise                3.2798
    2            GyroscopeNoise                  3.2641
    2            AccelerometerNoise              3.1715
    2            GyroscopeBiasNoise              3.1715
    2            GyroscopeBiasDecayFactor        2.9661
    2            AccelerometerBiasNoise          2.9661
    2            AccelerometerBiasDecayFactor    2.9661
    2            ZeroVelocityConstraintNoise     2.9617
    2            GPSPositionNoise                2.8438
    2            GPSVelocityNoise                2.8384
    3            GyroscopeNoise                  2.8373
    3            AccelerometerNoise              2.7382
    3            GyroscopeBiasNoise              2.7382
    3            GyroscopeBiasDecayFactor        2.7382
    3            AccelerometerBiasNoise          2.7382
    3            AccelerometerBiasDecayFactor    2.7382
    3            ZeroVelocityConstraintNoise     2.7335
    3            GPSPositionNoise                2.6105
    3            GPSVelocityNoise                2.6045
    4            GyroscopeNoise                  2.6023
    4            AccelerometerNoise              2.5001
    4            GyroscopeBiasNoise              2.5001
    4            GyroscopeBiasDecayFactor        2.5001
    4            AccelerometerBiasNoise          2.5001
    4            AccelerometerBiasDecayFactor    2.5001
    4            ZeroVelocityConstraintNoise     2.4953
    4            GPSPositionNoise                2.3692
    4            GPSVelocityNoise                2.3626
    5            GyroscopeNoise                  2.3595
    5            AccelerometerNoise              2.2561
    5            GyroscopeBiasNoise              2.2561
    5            GyroscopeBiasDecayFactor        2.2508
    5            AccelerometerBiasNoise          2.2508
    5            AccelerometerBiasDecayFactor    2.2508
    5            ZeroVelocityConstraintNoise     2.2469
    5            GPSPositionNoise                2.1265
    5            GPSVelocityNoise                2.1191
    6            GyroscopeNoise                  2.1148
    6            AccelerometerNoise              2.0150
    6            GyroscopeBiasNoise              2.0150
    6            GyroscopeBiasDecayFactor        2.0150
    6            AccelerometerBiasNoise          2.0150
    6            AccelerometerBiasDecayFactor    2.0150
    6            ZeroVelocityConstraintNoise     2.0116
    6            GPSPositionNoise                1.8970
    6            GPSVelocityNoise                1.8888
    7            GyroscopeNoise                  1.8847
    7            AccelerometerNoise              1.7921
    7            GyroscopeBiasNoise              1.7921
    7            GyroscopeBiasDecayFactor        1.7845
    7            AccelerometerBiasNoise          1.7845
    7            AccelerometerBiasDecayFactor    1.7845
    7            ZeroVelocityConstraintNoise     1.7815
    7            GPSPositionNoise                1.6794
    7            GPSVelocityNoise                1.6708
    8            GyroscopeNoise                  1.6679
    8            AccelerometerNoise              1.5886
    8            GyroscopeBiasNoise              1.5886
    8            GyroscopeBiasDecayFactor        1.5866
    8            AccelerometerBiasNoise          1.5866
    8            AccelerometerBiasDecayFactor    1.5866
    8            ZeroVelocityConstraintNoise     1.5850
    8            GPSPositionNoise                1.5057
    8            GPSVelocityNoise                1.4965
    9            GyroscopeNoise                  1.4950
    9            AccelerometerNoise              1.4364
    9            GyroscopeBiasNoise              1.4364
    9            GyroscopeBiasDecayFactor        1.4364
    9            AccelerometerBiasNoise          1.4364
    9            AccelerometerBiasDecayFactor    1.4364
    9            ZeroVelocityConstraintNoise     1.4355
    9            GPSPositionNoise                1.3894
    9            GPSVelocityNoise                1.3790
    10           GyroscopeNoise                  1.3773
    10           AccelerometerNoise              1.3422
    10           GyroscopeBiasNoise              1.3422
    10           GyroscopeBiasDecayFactor        1.3421
    10           AccelerometerBiasNoise          1.3421
    10           AccelerometerBiasDecayFactor    1.3421
    10           ZeroVelocityConstraintNoise     1.3399
    10           GPSPositionNoise                1.3319
    10           GPSVelocityNoise                1.3190
    11           GyroscopeNoise                  1.3159
    11           AccelerometerNoise              1.3102
    11           GyroscopeBiasNoise              1.3102
    11           GyroscopeBiasDecayFactor        1.3100
    11           AccelerometerBiasNoise          1.3100
    11           AccelerometerBiasDecayFactor    1.3100
    11           ZeroVelocityConstraintNoise     1.3069
    11           GPSPositionNoise                1.2964
    11           GPSVelocityNoise                1.2762
    12           GyroscopeNoise                  1.2740
    12           AccelerometerNoise              1.2655
    12           GyroscopeBiasNoise              1.2655
    12           GyroscopeBiasDecayFactor        1.2641
    12           AccelerometerBiasNoise          1.2641
    12           AccelerometerBiasDecayFactor    1.2641
    12           ZeroVelocityConstraintNoise     1.2631
    12           GPSPositionNoise                1.2511
    12           GPSVelocityNoise                1.2198
    13           GyroscopeNoise                  1.2184
    13           AccelerometerNoise              1.2058
    13           GyroscopeBiasNoise              1.2058
    13           GyroscopeBiasDecayFactor        1.2029
    13           AccelerometerBiasNoise          1.2029
    13           AccelerometerBiasDecayFactor    1.2029
    13           ZeroVelocityConstraintNoise     1.2029
    13           GPSPositionNoise                1.1874
    13           GPSVelocityNoise                1.1408
    14           GyroscopeNoise                  1.1403
    14           AccelerometerNoise              1.1236
    14           GyroscopeBiasNoise              1.1236
    14           GyroscopeBiasDecayFactor        1.1186
    14           AccelerometerBiasNoise          1.1186
    14           AccelerometerBiasDecayFactor    1.1186
    14           ZeroVelocityConstraintNoise     1.1183
    14           GPSPositionNoise                1.0975
    14           GPSVelocityNoise                1.0348
    15           GyroscopeNoise                  1.0347
    15           AccelerometerNoise              1.0155
    15           GyroscopeBiasNoise              1.0155
    15           GyroscopeBiasDecayFactor        1.0081
    15           AccelerometerBiasNoise          1.0081
    15           AccelerometerBiasDecayFactor    1.0081
    15           ZeroVelocityConstraintNoise     1.0076
    15           GPSPositionNoise                0.9813
    15           GPSVelocityNoise                0.9078
    16           GyroscopeNoise                  0.9074
    16           AccelerometerNoise              0.8926
    16           GyroscopeBiasNoise              0.8926
    16           GyroscopeBiasDecayFactor        0.8823
    16           AccelerometerBiasNoise          0.8823
    16           AccelerometerBiasDecayFactor    0.8823
    16           ZeroVelocityConstraintNoise     0.8815
    16           GPSPositionNoise                0.8526
    16           GPSVelocityNoise                0.7926
    17           GyroscopeNoise                  0.7920
    17           AccelerometerNoise              0.7870
    17           GyroscopeBiasNoise              0.7870
    17           GyroscopeBiasDecayFactor        0.7742
    17           AccelerometerBiasNoise          0.7742
    17           AccelerometerBiasDecayFactor    0.7742
    17           ZeroVelocityConstraintNoise     0.7730
    17           GPSPositionNoise                0.7665
    17           GPSVelocityNoise                0.7665
    18           GyroscopeNoise                  0.7662
    18           AccelerometerNoise              0.7638
    18           GyroscopeBiasNoise              0.7638
    18           GyroscopeBiasDecayFactor        0.7495
    18           AccelerometerBiasNoise          0.7495
    18           AccelerometerBiasDecayFactor    0.7495
    18           ZeroVelocityConstraintNoise     0.7482
    18           GPSPositionNoise                0.7482
    18           GPSVelocityNoise                0.7475
    19           GyroscopeNoise                  0.7474
    19           AccelerometerNoise              0.7474
    19           GyroscopeBiasNoise              0.7474
    19           GyroscopeBiasDecayFactor        0.7474
    19           AccelerometerBiasNoise          0.7474
    19           AccelerometerBiasDecayFactor    0.7474
    19           ZeroVelocityConstraintNoise     0.7453
    19           GPSPositionNoise                0.7416
    19           GPSVelocityNoise                0.7382
    20           GyroscopeNoise                  0.7378
    20           AccelerometerNoise              0.7370
    20           GyroscopeBiasNoise              0.7370
    20           GyroscopeBiasDecayFactor        0.7370
    20           AccelerometerBiasNoise          0.7370
    20           AccelerometerBiasDecayFactor    0.7370
    20           ZeroVelocityConstraintNoise     0.7345
    20           GPSPositionNoise                0.7345
    20           GPSVelocityNoise                0.7345
    21           GyroscopeNoise                  0.7334
    21           AccelerometerNoise              0.7334
    21           GyroscopeBiasNoise              0.7334
    21           GyroscopeBiasDecayFactor        0.7334
    21           AccelerometerBiasNoise          0.7334
    21           AccelerometerBiasDecayFactor    0.7334
    21           ZeroVelocityConstraintNoise     0.7306
    21           GPSPositionNoise                0.7279
    21           GPSVelocityNoise                0.7268
    22           GyroscopeNoise                  0.7248
    22           AccelerometerNoise              0.7247
    22           GyroscopeBiasNoise              0.7247
    22           GyroscopeBiasDecayFactor        0.7234
    22           AccelerometerBiasNoise          0.7234
    22           AccelerometerBiasDecayFactor    0.7234
    22           ZeroVelocityConstraintNoise     0.7207
    22           GPSPositionNoise                0.7206
    22           GPSVelocityNoise                0.7170
    23           GyroscopeNoise                  0.7138
    23           AccelerometerNoise              0.7134
    23           GyroscopeBiasNoise              0.7134
    23           GyroscopeBiasDecayFactor        0.7134
    23           AccelerometerBiasNoise          0.7134
    23           AccelerometerBiasDecayFactor    0.7134
    23           ZeroVelocityConstraintNoise     0.7122
    23           GPSPositionNoise                0.7122
    23           GPSVelocityNoise                0.7122
    24           GyroscopeNoise                  0.7081
    24           AccelerometerNoise              0.7080
    24           GyroscopeBiasNoise              0.7080
    24           GyroscopeBiasDecayFactor        0.7080
    24           AccelerometerBiasNoise          0.7080
    24           AccelerometerBiasDecayFactor    0.7080
    24           ZeroVelocityConstraintNoise     0.7080
    24           GPSPositionNoise                0.7080
    24           GPSVelocityNoise                0.7072
    25           GyroscopeNoise                  0.7009
    25           AccelerometerNoise              0.7009
    25           GyroscopeBiasNoise              0.7009
    25           GyroscopeBiasDecayFactor        0.7007
    25           AccelerometerBiasNoise          0.7007
    25           AccelerometerBiasDecayFactor    0.7007
    25           ZeroVelocityConstraintNoise     0.7005
    25           GPSPositionNoise                0.6997
    25           GPSVelocityNoise                0.6997
    26           GyroscopeNoise                  0.6912
    26           AccelerometerNoise              0.6906
    26           GyroscopeBiasNoise              0.6906
    26           GyroscopeBiasDecayFactor        0.6906
    26           AccelerometerBiasNoise          0.6906
    26           AccelerometerBiasDecayFactor    0.6906
    26           ZeroVelocityConstraintNoise     0.6896
    26           GPSPositionNoise                0.6896
    26           GPSVelocityNoise                0.6896
    27           GyroscopeNoise                  0.6840
    27           AccelerometerNoise              0.6831
    27           GyroscopeBiasNoise              0.6831
    27           GyroscopeBiasDecayFactor        0.6831
    27           AccelerometerBiasNoise          0.6831
    27           AccelerometerBiasDecayFactor    0.6831
    27           ZeroVelocityConstraintNoise     0.6818
    27           GPSPositionNoise                0.6816
    27           GPSVelocityNoise                0.6816
    28           GyroscopeNoise                  0.6816
    28           AccelerometerNoise              0.6809
    28           GyroscopeBiasNoise              0.6809
    28           GyroscopeBiasDecayFactor        0.6809
    28           AccelerometerBiasNoise          0.6809
    28           AccelerometerBiasDecayFactor    0.6809
    28           ZeroVelocityConstraintNoise     0.6804
    28           GPSPositionNoise                0.6802
    28           GPSVelocityNoise                0.6802
    29           GyroscopeNoise                  0.6793
    29           AccelerometerNoise              0.6785
    29           GyroscopeBiasNoise              0.6785
    29           GyroscopeBiasDecayFactor        0.6785
    29           AccelerometerBiasNoise          0.6785
    29           AccelerometerBiasDecayFactor    0.6785
    29           ZeroVelocityConstraintNoise     0.6778
    29           GPSPositionNoise                0.6773
    29           GPSVelocityNoise                0.6773
    30           GyroscopeNoise                  0.6773
    30           AccelerometerNoise              0.6769
    30           GyroscopeBiasNoise              0.6769
    30           GyroscopeBiasDecayFactor        0.6769
    30           AccelerometerBiasNoise          0.6769
    30           AccelerometerBiasDecayFactor    0.6769
    30           ZeroVelocityConstraintNoise     0.6769
    30           GPSPositionNoise                0.6769
    30           GPSVelocityNoise                0.6769

Fusionnez les données du capteur à l’aide du filtre optimisé. Obtenez une pose et une orientation estimées.

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,:), ...
            tunedNoise.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedNoise.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Calculez les erreurs RMS. 

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

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

Visualisez les résultats.

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

Arguments d'entrée

réduire tout

Objet filtre, spécifié comme objet insfilterNonholonomic .

Bruit de mesure, spécifié comme structure. La fonction utilise l'entrée de bruit de mesure comme estimation initiale pour régler le bruit de mesure. La structure doit contenir ces champs :

Nom de domaineDescription
GPSPositionNoiseVariation du bruit de position GPS, spécifiée sous forme de scalaire en m2
GPSVelocityNoiseVariation du bruit de vitesse GPS, spécifiée sous forme de scalaire en (m/s) 2

Types de données : struct

Données du capteur, spécifiées sous la forme table. Dans chaque ligne, les données du capteur sont spécifiées comme suit :

  • Accelerometer — Données de l'accéléromètre, spécifiées sous la forme d'un vecteur 1 sur 3 de scalaires en m2/s.

  • Gyroscope — Données du gyroscope, spécifiées sous la forme d'un vecteur 1 sur 3 de scalaires en rad/s.

  • GPSPosition — Données de position GPS, spécifiées sous la forme d'un vecteur 1 x 3 de latitude en degrés, de longitude en degrés et d'altitude en mètres.

  • GPSVelocity — Données de vitesse GPS, spécifiées sous la forme d'un vecteur 1 sur 3 de scalaires en m/s.

Si le capteur GPS ne produit pas de mesures complètes, spécifiez l'entrée correspondante pour GPSPosition et/ou GPSVelocity comme NaN. Si vous définissez la propriété Cost de l'entrée de configuration du tuner, config, sur Custom, vous pouvez utiliser d'autres types de données pour le sensorData entrée en fonction de votre choix.

Types de données : table

Données de vérité terrain, spécifiées sous forme de tableau. Dans chaque ligne, le tableau peut éventuellement contenir l'une de ces variables :

  • Orientation — Orientation du cadre de navigation vers le cadre de carrosserie, spécifiée sous la forme d'un quaternion ou d'une matrice de rotation 3 x 3.

  • Position — Position dans le cadre de navigation, spécifiée sous la forme d'un vecteur 1 sur 3 de scalaires en mètres.

  • Velocity — Vitesse dans le cadre de navigation, spécifiée comme un vecteur 1 sur 3 de scalaires en m/s.

  • GyroscopeBias — Biais d'angle delta du gyroscope dans le cadre du corps, spécifié comme un vecteur 1 sur 3 de scalaires en rad/s.

  • AccelerometerBias — Biais d'angle delta de l'accéléromètre dans le cadre du corps, spécifié comme un vecteur 1 sur 3 de scalaires en m2/s.

La fonction traite chaque ligne des tables sensorData et groundTruth séquentiellement pour calculer l'estimation de l'état et l'erreur RMS à partir de la vérité terrain. Les variables d'état non présentes dans l'entrée groundTruth sont ignorées pour la comparaison. Les tables sensorData et groundTruth doivent avoir le même nombre de lignes.

Si vous définissez la propriété Cost de l'entrée de configuration du tuner, config, sur Custom, vous pouvez utiliser d'autres types de données pour le groundTruth entrée en fonction de votre choix.

Types de données : table

Configuration du tuner, spécifiée comme objet tunerconfig .

Arguments de sortie

réduire tout

Bruit de mesure réglé, renvoyé sous forme de structure. La structure contient ces champs.

Nom de domaineDescription
GPSPositionNoiseVariation du bruit de position GPS, spécifiée sous forme de scalaire en m2
GPSVelocityNoiseVariation du bruit de vitesse GPS, spécifiée sous forme de scalaire en (m/s) 2

Types de données : struct

Références

[1] Abbeel, Pieter, et al. “Discriminative Training of Kalman Filters.” Robotics: Science and Systems I, Robotics: Science and Systems Foundation, 2005. DOI.org (Crossref), doi:10.15607/RSS.2005.I.038.

Historique des versions

Introduit dans R2020b

Voir aussi

| |