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');
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)