Main Content

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

Ajuste automático del filtro insfilterAsync

El objeto insfilterAsync es un filtro de Kalman extendido complejo que estima la pose del dispositivo. Sin embargo, ajustar manualmente el filtro o encontrar los valores óptimos para los parámetros de ruido puede ser una tarea desafiante. Este ejemplo ilustra cómo utilizar la función tune para optimizar los parámetros de ruido del filtro.

Configuración de trayectoria y sensor

Para ilustrar el proceso de ajuste del filtro insfilterAsync , utilice una trayectoria de punto de ruta aleatoria simple. Los objetos imuSensor y gpsSensor crean entradas para el filtro.

% The IMU runs at 100 Hz and the GPS runs at 1 Hz.
imurate = 100;
gpsrate = 1;
decim = imurate/gpsrate;

% Create a random waypoint trajectory.
rng(1)
Npts = 4; % number of waypoints
wpPer = 5; % time between waypoints
tstart = 0;
tend = wpPer*(Npts -1);
wp = waypointTrajectory('Waypoints',5*rand(Npts,3), ...
        'TimeOfArrival',tstart:wpPer:tend, ...
        'Orientation',[quaternion.ones; randrot(Npts-1,1)], ...
        'SampleRate', imurate);

[Position,Orientation,Velocity,Acceleration,AngularVelocity] = lookupPose(...
    wp, tstart:(1/imurate):tend);

% Set up an IMU and process the trajectory.
imu = imuSensor('SampleRate',imurate);
loadparams(imu,fullfile(matlabroot, ...
    "toolbox","shared","positioning","positioningdata","generic.json"), ...
    "GenericLowCost9Axis");
[Accelerometer, Gyroscope, Magnetometer] = imu(Acceleration, ...
    AngularVelocity, Orientation);
imuData = timetable(Accelerometer,Gyroscope,Magnetometer,'SampleRate',imurate);

% Set up a GPS sensor and process the trajectory.
gps = gpsSensor('SampleRate', gpsrate,'DecayFactor',0.5, ...
    'HorizontalPositionAccuracy',1.6,'VerticalPositionAccuracy',1.6, ...
    'VelocityAccuracy',0.1);
[GPSPosition,GPSVelocity] = gps(Position(1:decim:end,:), Velocity(1:decim:end,:));
gpsData = timetable(GPSPosition,GPSVelocity,'SampleRate',gpsrate);

% Create a timetable for the tune function.
sensorData = synchronize(imuData,gpsData);

% Create a timetable capturing the ground truth pose.
groundTruth = timetable(Position,Orientation,'SampleRate',imurate);

Construir el filtro

El filtro insfilterAsync fusiona datos de múltiples sensores que funcionan de forma asincrónica

filtUntuned = insfilterAsync;

Determinar las condiciones iniciales del filtro

Establezca los valores iniciales para las propiedades State y StateCovariance en función de ground-truth. Normalmente, para obtener los valores iniciales, se utilizarían las primeras muestras de sensorData junto con rutinas de calibración. Sin embargo, en este ejemplo , groundTruth se utiliza para establecer el estado inicial para una convergencia rápida del filtro.

idx = stateinfo(filtUntuned);
filtUntuned.State(idx.Orientation) = compact(Orientation(1));
filtUntuned.State(idx.AngularVelocity) = AngularVelocity(1,:);
filtUntuned.State(idx.Position) = Position(1,:);
filtUntuned.State(idx.Velocity) = Velocity(1,:);
filtUntuned.State(idx.Acceleration) = Acceleration(1,:);
filtUntuned.State(idx.AccelerometerBias) = imu.Accelerometer.ConstantBias;
filtUntuned.State(idx.GyroscopeBias) = imu.Gyroscope.ConstantBias;
filtUntuned.State(idx.GeomagneticFieldVector) = imu.MagneticField;
filtUntuned.State(idx.MagnetometerBias) = imu.Magnetometer.ConstantBias;
filtUntuned.StateCovariance = 1e-5*eye(numel(filtUntuned.State));

% Create a copy of the filtUntuned object for tuning later.
filtTuned = copy(filtUntuned);

Procese sensorData con un filtro no ajustado

Utilice la función tunernoise para crear ruidos de medición que también deben ajustarse. Para ilustrar la necesidad de realizar ajustes, primero utilice el filtro con sus parámetros predeterminados.

mn = tunernoise('insfilterAsync');
[posUntunedEst, orientUntunedEst] = fuse(filtUntuned, sensorData, mn);

Ajusteel filtro y procese sensorData

Utilice la función tune para minimizar el error cuadrático medio (RMS) entre groundTruth y las estimaciones estatales.

cfg = tunerconfig(class(filtTuned),'MaxIterations',15,'StepForward',1.1);
tunedmn = tune(filtTuned,mn,sensorData,groundTruth,cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        4.5898
    1            GyroscopeNoise            4.5694
    1            MagnetometerNoise         4.5481
    1            GPSPositionNoise          4.4737
    1            GPSVelocityNoise          4.2984
    1            QuaternionNoise           4.2984
    1            AngularVelocityNoise      3.9668
    1            PositionNoise             3.9668
    1            VelocityNoise             3.9668
    1            AccelerationNoise         3.9556
    1            GyroscopeBiasNoise        3.9556
    1            AccelerometerBiasNoise    3.9511
    1            GeomagneticVectorNoise    3.9511
    1            MagnetometerBiasNoise     3.9360
    2            AccelerometerNoise        3.9360
    2            GyroscopeNoise            3.9360
    2            MagnetometerNoise         3.9064
    2            GPSPositionNoise          3.9064
    2            GPSVelocityNoise          3.7129
    2            QuaternionNoise           3.7122
    2            AngularVelocityNoise      3.0116
    2            PositionNoise             3.0116
    2            VelocityNoise             3.0116
    2            AccelerationNoise         2.9850
    2            GyroscopeBiasNoise        2.9850
    2            AccelerometerBiasNoise    2.9824
    2            GeomagneticVectorNoise    2.9824
    2            MagnetometerBiasNoise     2.9821
    3            AccelerometerNoise        2.9821
    3            GyroscopeNoise            2.9613
    3            MagnetometerNoise         2.9432
    3            GPSPositionNoise          2.9432
    3            GPSVelocityNoise          2.8373
    3            QuaternionNoise           2.8369
    3            AngularVelocityNoise      2.6993
    3            PositionNoise             2.6993
    3            VelocityNoise             2.6993
    3            AccelerationNoise         2.6993
    3            GyroscopeBiasNoise        2.6993
    3            AccelerometerBiasNoise    2.6971
    3            GeomagneticVectorNoise    2.6971
    3            MagnetometerBiasNoise     2.6955
    4            AccelerometerNoise        2.6941
    4            GyroscopeNoise            2.6797
    4            MagnetometerNoise         2.6676
    4            GPSPositionNoise          2.6626
    4            GPSVelocityNoise          2.5530
    4            QuaternionNoise           2.5530
    4            AngularVelocityNoise      2.4285
    4            PositionNoise             2.4285
    4            VelocityNoise             2.4285
    4            AccelerationNoise         2.4232
    4            GyroscopeBiasNoise        2.4232
    4            AccelerometerBiasNoise    2.4217
    4            GeomagneticVectorNoise    2.4217
    4            MagnetometerBiasNoise     2.4023
    5            AccelerometerNoise        2.4019
    5            GyroscopeNoise            2.3900
    5            MagnetometerNoise         2.3900
    5            GPSPositionNoise          2.3887
    5            GPSVelocityNoise          2.2986
    5            QuaternionNoise           2.2986
    5            AngularVelocityNoise      2.1945
    5            PositionNoise             2.1945
    5            VelocityNoise             2.1945
    5            AccelerationNoise         2.1863
    5            GyroscopeBiasNoise        2.1863
    5            AccelerometerBiasNoise    2.1856
    5            GeomagneticVectorNoise    2.1856
    5            MagnetometerBiasNoise     2.1615
    6            AccelerometerNoise        2.1613
    6            GyroscopeNoise            2.1393
    6            MagnetometerNoise         2.1199
    6            GPSPositionNoise          2.1112
    6            GPSVelocityNoise          2.0140
    6            QuaternionNoise           2.0140
    6            AngularVelocityNoise      1.9288
    6            PositionNoise             1.9288
    6            VelocityNoise             1.9288
    6            AccelerationNoise         1.9242
    6            GyroscopeBiasNoise        1.9242
    6            AccelerometerBiasNoise    1.9216
    6            GeomagneticVectorNoise    1.9216
    6            MagnetometerBiasNoise     1.9054
    7            AccelerometerNoise        1.9047
    7            GyroscopeNoise            1.8887
    7            MagnetometerNoise         1.8820
    7            GPSPositionNoise          1.8803
    7            GPSVelocityNoise          1.7713
    7            QuaternionNoise           1.7712
    7            AngularVelocityNoise      1.7145
    7            PositionNoise             1.7145
    7            VelocityNoise             1.7145
    7            AccelerationNoise         1.7122
    7            GyroscopeBiasNoise        1.7122
    7            AccelerometerBiasNoise    1.7112
    7            GeomagneticVectorNoise    1.7112
    7            MagnetometerBiasNoise     1.6873
    8            AccelerometerNoise        1.6853
    8            GyroscopeNoise            1.6790
    8            MagnetometerNoise         1.6694
    8            GPSPositionNoise          1.6565
    8            GPSVelocityNoise          1.5700
    8            QuaternionNoise           1.5676
    8            AngularVelocityNoise      1.5366
    8            PositionNoise             1.5366
    8            VelocityNoise             1.5366
    8            AccelerationNoise         1.5366
    8            GyroscopeBiasNoise        1.5366
    8            AccelerometerBiasNoise    1.5353
    8            GeomagneticVectorNoise    1.5337
    8            MagnetometerBiasNoise     1.5166
    9            AccelerometerNoise        1.5129
    9            GyroscopeNoise            1.5117
    9            MagnetometerNoise         1.5114
    9            GPSPositionNoise          1.4953
    9            GPSVelocityNoise          1.4106
    9            QuaternionNoise           1.4106
    9            AngularVelocityNoise      1.3742
    9            PositionNoise             1.3742
    9            VelocityNoise             1.3742
    9            AccelerationNoise         1.3731
    9            GyroscopeBiasNoise        1.3731
    9            AccelerometerBiasNoise    1.3715
    9            GeomagneticVectorNoise    1.3715
    9            MagnetometerBiasNoise     1.3576
    10           AccelerometerNoise        1.3528
    10           GyroscopeNoise            1.3528
    10           MagnetometerNoise         1.3510
    10           GPSPositionNoise          1.3322
    10           GPSVelocityNoise          1.2512
    10           QuaternionNoise           1.2512
    10           AngularVelocityNoise      1.2507
    10           PositionNoise             1.2507
    10           VelocityNoise             1.2507
    10           AccelerationNoise         1.2497
    10           GyroscopeBiasNoise        1.2497
    10           AccelerometerBiasNoise    1.2480
    10           GeomagneticVectorNoise    1.2480
    10           MagnetometerBiasNoise     1.2301
    11           AccelerometerNoise        1.2233
    11           GyroscopeNoise            1.2222
    11           MagnetometerNoise         1.2220
    11           GPSPositionNoise          1.2008
    11           GPSVelocityNoise          1.1043
    11           QuaternionNoise           1.1043
    11           AngularVelocityNoise      1.1038
    11           PositionNoise             1.1038
    11           VelocityNoise             1.1038
    11           AccelerationNoise         1.1028
    11           GyroscopeBiasNoise        1.1028
    11           AccelerometerBiasNoise    1.1013
    11           GeomagneticVectorNoise    1.1013
    11           MagnetometerBiasNoise     1.0867
    12           AccelerometerNoise        1.0782
    12           GyroscopeNoise            1.0767
    12           MagnetometerNoise         1.0733
    12           GPSPositionNoise          1.0505
    12           GPSVelocityNoise          0.9564
    12           QuaternionNoise           0.9563
    12           AngularVelocityNoise      0.9563
    12           PositionNoise             0.9563
    12           VelocityNoise             0.9563
    12           AccelerationNoise         0.9550
    12           GyroscopeBiasNoise        0.9550
    12           AccelerometerBiasNoise    0.9534
    12           GeomagneticVectorNoise    0.9534
    12           MagnetometerBiasNoise     0.9402
    13           AccelerometerNoise        0.9303
    13           GyroscopeNoise            0.9291
    13           MagnetometerNoise         0.9269
    13           GPSPositionNoise          0.9036
    13           GPSVelocityNoise          0.8072
    13           QuaternionNoise           0.8071
    13           AngularVelocityNoise      0.8071
    13           PositionNoise             0.8071
    13           VelocityNoise             0.8071
    13           AccelerationNoise         0.8065
    13           GyroscopeBiasNoise        0.8065
    13           AccelerometerBiasNoise    0.8052
    13           GeomagneticVectorNoise    0.8052
    13           MagnetometerBiasNoise     0.7901
    14           AccelerometerNoise        0.7806
    14           GyroscopeNoise            0.7806
    14           MagnetometerNoise         0.7768
    14           GPSPositionNoise          0.7547
    14           GPSVelocityNoise          0.6932
    14           QuaternionNoise           0.6930
    14           AngularVelocityNoise      0.6923
    14           PositionNoise             0.6923
    14           VelocityNoise             0.6923
    14           AccelerationNoise         0.6906
    14           GyroscopeBiasNoise        0.6906
    14           AccelerometerBiasNoise    0.6896
    14           GeomagneticVectorNoise    0.6896
    14           MagnetometerBiasNoise     0.6801
    15           AccelerometerNoise        0.6704
    15           GyroscopeNoise            0.6676
    15           MagnetometerNoise         0.6653
    15           GPSPositionNoise          0.6469
    15           GPSVelocityNoise          0.6047
    15           QuaternionNoise           0.6047
    15           AngularVelocityNoise      0.6037
    15           PositionNoise             0.6037
    15           VelocityNoise             0.6037
    15           AccelerationNoise         0.6019
    15           GyroscopeBiasNoise        0.6019
    15           AccelerometerBiasNoise    0.6015
    15           GeomagneticVectorNoise    0.6015
    15           MagnetometerBiasNoise     0.5913
[posTunedEst, orientTunedEst] = fuse(filtTuned,sensorData,tunedmn);

Comparar filtro ajustado y no ajustado

Trazar las estimaciones de posición de los filtros ajustados y no ajustados junto con las posiciones ground-truth. Luego, trace el error de orientación (distancia del cuaternión) en grados para los filtros ajustados y no ajustados. El filtro ajustado estima la posición y la orientación mejor que el filtro no ajustado.

% Position error
figure;
t = sensorData.Time;
subplot(3,1,1);
plot(t, [posTunedEst(:,1) posUntunedEst(:,1) Position(:,1)]);
title('Position');
ylabel('X-axis');
legend('Tuned', 'Untuned', 'Truth');
subplot(3,1,2);
plot(t, [posTunedEst(:,2) posUntunedEst(:,2) Position(:,2)]);
ylabel('Y-axis');
subplot(3,1,3);
plot(t, [posTunedEst(:,3) posUntunedEst(:,3) Position(:,3)]);
ylabel('Z-axis');

% Orientation Error
figure;
plot(t, rad2deg(dist(Orientation, orientTunedEst)), ...
    t, rad2deg(dist(Orientation, orientUntunedEst)));
title('Orientation Error');
ylabel('Degrees');
legend('Tuned', 'Untuned');