Estimación de pose a partir de sensores asíncronos
Este ejemplo muestra cómo se pueden fusionar sensores a diferentes velocidades para estimar la pose. Se utilizan acelerómetro, giroscopio, magnetómetro y GPS para determinar la orientación y posición de un vehículo que se mueve a lo largo de una trayectoria circular. Puede usar los controles en la ventana de la figura para variar las velocidades del sensor y experimentar con la caída del sensor mientras observa el efecto en la pose estimada.
Configuración de simulación
Cargue datos de sensores pregrabados. Los datos del sensor se basan en una trayectoria circular creada utilizando la clase waypointTrajectory
. Los valores del sensor se crearon utilizando las clases gpsSensor
y imuSensor
. El archivo CircularTrajectorySensorData.mat
utilizado aquí se puede generar con la función generateCircularTrajSensorData
.
ld = load('CircularTrajectorySensorData.mat'); Fs = ld.Fs; % maximum MARG rate gpsFs = ld.gpsFs; % maximum GPS rate ratio = Fs./gpsFs; refloc = ld.refloc; trajOrient = ld.trajData.Orientation; trajVel = ld.trajData.Velocity; trajPos = ld.trajData.Position; trajAcc = ld.trajData.Acceleration; trajAngVel = ld.trajData.AngularVelocity; accel = ld.accel; gyro = ld.gyro; mag = ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;
Filtro de fusión
Cree un insfilterAsync
para fusionar las mediciones de IMU + GPS. Este filtro de fusión utiliza un filtro de Kalman extendido (EKF) continuo y discreto para rastrear la orientación (como un cuaternión), la velocidad angular, la posición, la velocidad, la aceleración, los sesgos del sensor y el vector geomagnético.
Este insfilterAsync
tiene varios métodos para procesar los datos del sensor: fuseaccel
, fusegyro
, fusemag
y fusegps
. Debido a que insfilterAsync
utiliza un EKF discreto continuo, el método predict
puede hacer avanzar el filtro una cantidad de tiempo arbitraria.
fusionfilt = insfilterAsync('ReferenceLocation', refloc);
Inicialice el vector de estado del insfilterAsync
El insfilterAsync
rastrea los estados de pose en un vector de 28 elementos. Los estados son:
States Units Index Orientation (quaternion parts) 1:4 Angular Velocity (XYZ) rad/s 5:7 Position (NED) m 8:10 Velocity (NED) m/s 11:13 Acceleration (NED) m/s^2 14:16 Accelerometer Bias (XYZ) m/s^2 17:19 Gyroscope Bias (XYZ) rad/s 20:22 Geomagnetic Field Vector (NED) uT 23:25 Magnetometer Bias (XYZ) uT 26:28
La ground-truth se utiliza para ayudar a inicializar los estados del filtro, de modo que el filtro converja rápidamente hacia buenas respuestas.
Nav = 100; initstate = zeros(28,1); initstate(1:4) = compact( meanrot(trajOrient(1:Nav))); initstate(5:7) = mean( trajAngVel(10:Nav,:), 1); initstate(8:10) = mean( trajPos(1:Nav,:), 1); initstate(11:13) = mean( trajVel(1:Nav,:), 1); initstate(14:16) = mean( trajAcc(1:Nav,:), 1); initstate(23:25) = ld.magField; % The gyroscope bias initial value estimate is low for the Z-axis. This is % done to illustrate the effects of fusing the magnetometer in the % simulation. initstate(20:22) = deg2rad([3.125 3.125 3.125]); fusionfilt.State = initstate;
Establezca los valores de ruido del proceso del insfilterAsync
La variación del ruido del proceso describe la incertidumbre del modelo de movimiento que utiliza el filtro.
fusionfilt.QuaternionNoise = 1e-2; fusionfilt.AngularVelocityNoise = 100; fusionfilt.AccelerationNoise = 100; fusionfilt.MagnetometerBiasNoise = 1e-7; fusionfilt.AccelerometerBiasNoise = 1e-7; fusionfilt.GyroscopeBiasNoise = 1e-7;
Definir los valores de ruido de medición utilizados para fusionar datos del sensor
Cada sensor tiene algo de ruido en las mediciones. Estos valores normalmente se pueden encontrar en la hoja de datos de un sensor.
Rmag = 0.4; Rvel = 0.01; Racc = 610; Rgyro = 0.76e-5; Rpos = 3.4; fusionfilt.StateCovariance = diag(1e-3*ones(28,1));
Inicializar ámbitos
El alcance HelperScrollingPlotter
permite trazar variables a lo largo del tiempo. Se utiliza aquí para rastrear errores en la pose. El alcance PoseViewerWithSwitches
permite la visualización 3D de la estimación del filtro y la pose de ground-truth. Los alcances pueden ralentizar la simulación. Para deshabilitar un alcance, establezca la variable lógica correspondiente en falso.
useErrScope = true; % Turn on the streaming error plot. usePoseView = true; % Turn on the 3D pose viewer. if usePoseView posescope = PoseViewerWithSwitches(... 'XPositionLimits', [-30 30], ... 'YPositionLimits', [-30, 30], ... 'ZPositionLimits', [-10 10]); end f = gcf; if useErrScope errscope = HelperScrollingPlotter(... 'NumInputs', 4, ... 'TimeSpan', 10, ... 'SampleRate', Fs, ... 'YLabel', {'degrees', ... 'meters', ... 'meters', ... 'meters'}, ... 'Title', {'Quaternion Distance', ... 'Position X Error', ... 'Position Y Error', ... 'Position Z Error'}, ... 'YLimits', ... [ -1, 30 -2, 2 -2 2 -2 2]); end
Bucle de simulación
La simulación del algoritmo de fusión le permite inspeccionar los efectos de las diferentes frecuencias de muestreo del sensor. Además, se puede evitar la fusión de sensores individuales desmarcando la casilla de verificación correspondiente. Esto se puede utilizar para simular la caída del sensor.
Algunas configuraciones producen resultados espectaculares. Por ejemplo, apagar el sensor GPS hace que la estimación de posición se desvíe rápidamente. Apagar el sensor del magnetómetro hará que la estimación de orientación se desvíe lentamente de ground-truth ya que la estimación gira demasiado rápido. Por el contrario, si el giroscopio está apagado y el magnetómetro está encendido, la orientación estimada muestra una oscilación y carece de la suavidad presente si se utilizan ambos sensores.
Al encender todos los sensores pero configurarlos para que funcionen a la velocidad más baja se produce una estimación que se desvía visiblemente de ground-truth y luego vuelve a un resultado más correcto cuando se fusionan los sensores. Esto se ve más fácilmente en el HelperScrollingPlotter
de los errores de estimación corriente.
La simulación principal se ejecuta a 100 Hz. Cada iteración inspecciona las casillas de verificación en la ventana de la figura y, si el sensor está habilitado, fusiona los datos para ese sensor a la velocidad adecuada.
for ii=1:size(accel,1) fusionfilt.predict(1./Fs); % Fuse Accelerometer if (f.UserData.Accelerometer) && ... mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0 fusionfilt.fuseaccel(accel(ii,:), Racc); end % Fuse Gyroscope if (f.UserData.Gyroscope) && ... mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0 fusionfilt.fusegyro(gyro(ii,:), Rgyro); end % Fuse Magnetometer if (f.UserData.Magnetometer) && ... mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0 fusionfilt.fusemag(mag(ii,:), Rmag); end % Fuse GPS if (f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0 fusionfilt.fusegps(lla(ii,:), Rpos, gpsvel(ii,:), Rvel); end % Plot the pose error [p,q] = pose(fusionfilt); posescope(p, q, trajPos(ii,:), trajOrient(ii)); orientErr = rad2deg(dist(q, trajOrient(ii) )); posErr = p - trajPos(ii,:); errscope(orientErr, posErr(1), posErr(2), posErr(3)); end
Conclusión
El insfilterAsync
permite varias y variables frecuencias de muestreo. La calidad de los resultados estimados depende en gran medida de las tasas de fusión de sensores individuales. Cualquier caída del sensor tendrá un efecto profundo en la salida.