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');