Ajuste automático del filtro insfilterAsync
El objeto insfilterAsync es un filtro Kalman extendido complejo que estima la postura 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 del ruido del filtro.
Configuración de trayectoria y sensor
Para ilustrar el proceso de ajuste del filtro insfilterAsync, utilice una trayectoria de punto de referencia aleatorio 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 operan 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 la 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 se utiliza groundTruth para establecer el estado inicial para la 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);Procesar sensorData con un filtro no sintonizado
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);Ajuste el filtro y procese sensorData
Utilice la función tune para minimizar el error cuadrático medio (RMS) entre groundTruth y las estimaciones de estado.
cfg = tunerconfig(class(filtTuned),'MaxIterations',15,'StepForward',1.1); tunedmn = tune(filtTuned,mn,sensorData,groundTruth,cfg);
Iteration Parameter Metric
_________ _________ ______
1 AccelerometerNoise 3.9157
1 GyroscopeNoise 3.9157
1 MagnetometerNoise 3.8618
1 GPSPositionNoise 3.8162
1 GPSVelocityNoise 3.6551
1 QuaternionNoise 3.6522
1 AngularVelocityNoise 3.4632
1 PositionNoise 3.4632
1 VelocityNoise 3.4632
1 AccelerationNoise 3.4420
1 GyroscopeBiasNoise 3.4420
1 AccelerometerBiasNoise 3.4389
1 GeomagneticVectorNoise 3.4389
1 MagnetometerBiasNoise 3.4389
2 AccelerometerNoise 3.4389
2 GyroscopeNoise 3.4389
2 MagnetometerNoise 3.3792
2 GPSPositionNoise 3.3792
2 GPSVelocityNoise 3.2526
2 QuaternionNoise 3.2506
2 AngularVelocityNoise 2.6514
2 PositionNoise 2.6514
2 VelocityNoise 2.6514
2 AccelerationNoise 2.6514
2 GyroscopeBiasNoise 2.6514
2 AccelerometerBiasNoise 2.6496
2 GeomagneticVectorNoise 2.6496
2 MagnetometerBiasNoise 2.6386
3 AccelerometerNoise 2.6351
3 GyroscopeNoise 2.6351
3 MagnetometerNoise 2.6165
3 GPSPositionNoise 2.6165
3 GPSVelocityNoise 2.5219
3 QuaternionNoise 2.4994
3 AngularVelocityNoise 2.4183
3 PositionNoise 2.4183
3 VelocityNoise 2.4183
3 AccelerationNoise 2.4032
3 GyroscopeBiasNoise 2.4032
3 AccelerometerBiasNoise 2.3922
3 GeomagneticVectorNoise 2.3922
3 MagnetometerBiasNoise 2.3922
4 AccelerometerNoise 2.3888
4 GyroscopeNoise 2.3888
4 MagnetometerNoise 2.3888
4 GPSPositionNoise 2.3888
4 GPSVelocityNoise 2.2745
4 QuaternionNoise 2.2641
4 AngularVelocityNoise 2.1749
4 PositionNoise 2.1749
4 VelocityNoise 2.1749
4 AccelerationNoise 2.1628
4 GyroscopeBiasNoise 2.1628
4 AccelerometerBiasNoise 2.1624
4 GeomagneticVectorNoise 2.1624
4 MagnetometerBiasNoise 2.1589
5 AccelerometerNoise 2.1587
5 GyroscopeNoise 2.1587
5 MagnetometerNoise 2.1587
5 GPSPositionNoise 2.1573
5 GPSVelocityNoise 2.0520
5 QuaternionNoise 2.0518
5 AngularVelocityNoise 2.0044
5 PositionNoise 2.0044
5 VelocityNoise 2.0044
5 AccelerationNoise 2.0013
5 GyroscopeBiasNoise 2.0013
5 AccelerometerBiasNoise 1.9900
5 GeomagneticVectorNoise 1.9900
5 MagnetometerBiasNoise 1.9900
6 AccelerometerNoise 1.9893
6 GyroscopeNoise 1.9893
6 MagnetometerNoise 1.9893
6 GPSPositionNoise 1.9893
6 GPSVelocityNoise 1.8894
6 QuaternionNoise 1.8894
6 AngularVelocityNoise 1.8273
6 PositionNoise 1.8273
6 VelocityNoise 1.8273
6 AccelerationNoise 1.8189
6 GyroscopeBiasNoise 1.8189
6 AccelerometerBiasNoise 1.8189
6 GeomagneticVectorNoise 1.8188
6 MagnetometerBiasNoise 1.8188
7 AccelerometerNoise 1.8188
7 GyroscopeNoise 1.8188
7 MagnetometerNoise 1.8177
7 GPSPositionNoise 1.8157
7 GPSVelocityNoise 1.7122
7 QuaternionNoise 1.7122
7 AngularVelocityNoise 1.6420
7 PositionNoise 1.6420
7 VelocityNoise 1.6420
7 AccelerationNoise 1.6420
7 GyroscopeBiasNoise 1.6420
7 AccelerometerBiasNoise 1.6412
7 GeomagneticVectorNoise 1.6412
7 MagnetometerBiasNoise 1.6412
8 AccelerometerNoise 1.6412
8 GyroscopeNoise 1.6411
8 MagnetometerNoise 1.6411
8 GPSPositionNoise 1.6383
8 GPSVelocityNoise 1.5216
8 QuaternionNoise 1.5216
8 AngularVelocityNoise 1.5004
8 PositionNoise 1.5004
8 VelocityNoise 1.5004
8 AccelerationNoise 1.5000
8 GyroscopeBiasNoise 1.5000
8 AccelerometerBiasNoise 1.5000
8 GeomagneticVectorNoise 1.5000
8 MagnetometerBiasNoise 1.4998
9 AccelerometerNoise 1.4998
9 GyroscopeNoise 1.4998
9 MagnetometerNoise 1.4998
9 GPSPositionNoise 1.4998
9 GPSVelocityNoise 1.4106
9 QuaternionNoise 1.4106
9 AngularVelocityNoise 1.3916
9 PositionNoise 1.3916
9 VelocityNoise 1.3916
9 AccelerationNoise 1.3916
9 GyroscopeBiasNoise 1.3916
9 AccelerometerBiasNoise 1.3911
9 GeomagneticVectorNoise 1.3911
9 MagnetometerBiasNoise 1.3911
10 AccelerometerNoise 1.3911
10 GyroscopeNoise 1.3910
10 MagnetometerNoise 1.3910
10 GPSPositionNoise 1.3892
10 GPSVelocityNoise 1.2977
10 QuaternionNoise 1.2977
10 AngularVelocityNoise 1.2977
10 PositionNoise 1.2977
10 VelocityNoise 1.2977
10 AccelerationNoise 1.2975
10 GyroscopeBiasNoise 1.2975
10 AccelerometerBiasNoise 1.2975
10 GeomagneticVectorNoise 1.2975
10 MagnetometerBiasNoise 1.2974
11 AccelerometerNoise 1.2973
11 GyroscopeNoise 1.2973
11 MagnetometerNoise 1.2973
11 GPSPositionNoise 1.2973
11 GPSVelocityNoise 1.2031
11 QuaternionNoise 1.2030
11 AngularVelocityNoise 1.1998
11 PositionNoise 1.1998
11 VelocityNoise 1.1998
11 AccelerationNoise 1.1998
11 GyroscopeBiasNoise 1.1998
11 AccelerometerBiasNoise 1.1995
11 GeomagneticVectorNoise 1.1995
11 MagnetometerBiasNoise 1.1994
12 AccelerometerNoise 1.1993
12 GyroscopeNoise 1.1992
12 MagnetometerNoise 1.1992
12 GPSPositionNoise 1.1982
12 GPSVelocityNoise 1.1009
12 QuaternionNoise 1.1008
12 AngularVelocityNoise 1.0972
12 PositionNoise 1.0972
12 VelocityNoise 1.0972
12 AccelerationNoise 1.0972
12 GyroscopeBiasNoise 1.0972
12 AccelerometerBiasNoise 1.0968
12 GeomagneticVectorNoise 1.0968
12 MagnetometerBiasNoise 1.0967
13 AccelerometerNoise 1.0966
13 GyroscopeNoise 1.0965
13 MagnetometerNoise 1.0965
13 GPSPositionNoise 1.0955
13 GPSVelocityNoise 0.9830
13 QuaternionNoise 0.9829
13 AngularVelocityNoise 0.9829
13 PositionNoise 0.9829
13 VelocityNoise 0.9829
13 AccelerationNoise 0.9829
13 GyroscopeBiasNoise 0.9829
13 AccelerometerBiasNoise 0.9825
13 GeomagneticVectorNoise 0.9825
13 MagnetometerBiasNoise 0.9824
14 AccelerometerNoise 0.9822
14 GyroscopeNoise 0.9822
14 MagnetometerNoise 0.9822
14 GPSPositionNoise 0.9812
14 GPSVelocityNoise 0.8868
14 QuaternionNoise 0.8867
14 AngularVelocityNoise 0.8816
14 PositionNoise 0.8816
14 VelocityNoise 0.8816
14 AccelerationNoise 0.8815
14 GyroscopeBiasNoise 0.8815
14 AccelerometerBiasNoise 0.8812
14 GeomagneticVectorNoise 0.8812
14 MagnetometerBiasNoise 0.8811
15 AccelerometerNoise 0.8809
15 GyroscopeNoise 0.8809
15 MagnetometerNoise 0.8809
15 GPSPositionNoise 0.8799
15 GPSVelocityNoise 0.7916
15 QuaternionNoise 0.7916
15 AngularVelocityNoise 0.7916
15 PositionNoise 0.7916
15 VelocityNoise 0.7916
15 AccelerationNoise 0.7916
15 GyroscopeBiasNoise 0.7916
15 AccelerometerBiasNoise 0.7913
15 GeomagneticVectorNoise 0.7913
15 MagnetometerBiasNoise 0.7913
[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');
