Estimar la orientación y la altura utilizando IMU, magnetómetro y altímetro
Este ejemplo muestra cómo fusionar datos de un acelerómetro de 3 ejes, un giroscopio de 3 ejes, un magnetómetro de 3 ejes (comúnmente denominados sensores MARG para velocidad magnética, angular y gravedad) y un altímetro de 1 eje para estimar la orientación. y altura.
Configuración de simulación
Esta simulación procesa datos de sensores a múltiples velocidades. La IMU (acelerómetro y giroscopio) normalmente funciona a la velocidad más alta. El magnetómetro generalmente funciona a una velocidad más baja que la IMU y el altímetro funciona a la velocidad más baja. Cambiar las frecuencias de muestreo hace que partes del algoritmo de fusión se ejecuten con más frecuencia y pueden afectar el rendimiento.
% Set the sampling rate for IMU sensors, magnetometer, and altimeter. imuFs = 100; altFs = 10; magFs = 25; imuSamplesPerAlt = fix(imuFs/altFs); imuSamplesPerMag = fix(imuFs/magFs); % Set the number of samples to simulate. N = 1000; % Construct object for other helper functions. hfunc = Helper10AxisFusion;
Definir trayectoria
El cuerpo del sensor gira alrededor de los tres ejes mientras oscila en posición vertical. Las oscilaciones aumentan en magnitud a medida que continúa la simulación.
% Define the initial state of the sensor body initPos = [0, 0, 0]; % initial position (m) initVel = [0, 0, -1]; % initial linear velocity (m/s) initOrient = ones(1, 'quaternion'); % Define the constant angular velocity for rotating the sensor body % (rad/s). angVel = [0.34 0.2 0.045]; % Define the acceleration required for simple oscillating motion of the % sensor body. fc = 0.2; t = 0:1/imuFs:(N-1)/imuFs; a = 1; oscMotionAcc = sin(2*pi*fc*t); oscMotionAcc = hfunc.growAmplitude(oscMotionAcc); % Construct the trajectory object traj = kinematicTrajectory('SampleRate', imuFs, ... 'Velocity', initVel, ... 'Position', initPos, ... 'Orientation', initOrient);
Configuración de sensores
El acelerómetro, giroscopio y magnetómetro se simulan usando imuSensor
. El altímetro se modela utilizando el altimeterSensor
. Los valores utilizados en las configuraciones del sensor corresponden a valores reales del sensor MEMS.
imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs); % Accelerometer imu.Accelerometer.MeasurementRange = 19.6133; imu.Accelerometer.Resolution = 0.0023928; imu.Accelerometer.ConstantBias = 0.19; imu.Accelerometer.NoiseDensity = 0.0012356; % Gyroscope imu.Gyroscope.MeasurementRange = deg2rad(250); imu.Gyroscope.Resolution = deg2rad(0.0625); imu.Gyroscope.ConstantBias = deg2rad(3.125); imu.Gyroscope.AxesMisalignment = 1.5; imu.Gyroscope.NoiseDensity = deg2rad(0.025); % Magnetometer imu.Magnetometer.MeasurementRange = 1000; imu.Magnetometer.Resolution = 0.1; imu.Magnetometer.ConstantBias = 100; imu.Magnetometer.NoiseDensity = 0.3/sqrt(50); % altimeter altimeter = altimeterSensor('UpdateRate', altFs, 'NoiseDensity', 2*0.1549);
Filtro de fusión
Construya un ahrs10filter
y configúrelo.
fusionfilt = ahrs10filter; fusionfilt.IMUSampleRate = imuFs;
Establezca valores iniciales para el filtro de fusión.
initstate = zeros(18,1); initstate(1:4) = compact(initOrient); initstate(5) = initPos(3); initstate(6) = initVel(3); initstate(7:9) = imu.Gyroscope.ConstantBias/imuFs; initstate(10:12) = imu.Accelerometer.ConstantBias/imuFs; initstate(13:15) = imu.MagneticField; initstate(16:18) = imu.Magnetometer.ConstantBias; fusionfilt.State = initstate;
Inicialice la matriz de covarianza de estado del filtro de fusión. La ground-truth se utiliza para los estados iniciales, por lo que debería haber poco error en las estimaciones.
icv = diag([1e-8*[1 1 1 1 1 1 1], 1e-3*ones(1,11)]); fusionfilt.StateCovariance = icv;
Los ruidos de medición del magnetómetro y altímetro son los ruidos de observación asociados con los sensores utilizados por el filtro Kalman interno en el ahrs10filter
. Estos valores normalmente provendrían de una hoja de datos del sensor.
magNoise = 2*(imu.Magnetometer.NoiseDensity(1).^2)*imuFs; altimeterNoise = 2*(altimeter.NoiseDensity).^2 * altFs;
Los ruidos del proceso de filtrado se utilizan para ajustar el filtro al rendimiento deseado.
fusionfilt.AccelerometerNoise = [1e-1 1e-1 1e-4]; fusionfilt.AccelerometerBiasNoise = 1e-8; fusionfilt.GeomagneticVectorNoise = 1e-12; fusionfilt.MagnetometerBiasNoise = 1e-12; fusionfilt.GyroscopeNoise = 1e-12;
Opción de simulación adicional: Visor
De forma predeterminada, esta simulación traza los errores de estimación al final de la simulación. Para ver tanto la posición estimada como la orientación junto con ground-truth a medida que se ejecuta la simulación, establezca la variable usePoseViewer
en true
.
usePoseViewer = false;
Bucle de simulación
q = initOrient; firstTime = true; actQ = zeros(N,1, 'quaternion'); expQ = zeros(N,1, 'quaternion'); actP = zeros(N,1); expP = zeros(N,1); for ii = 1: N % Generate a new set of samples from the trajectory generator accBody = rotateframe(q, [0 0 +oscMotionAcc(ii)]); omgBody = rotateframe(q, angVel); [pos, q, vel, acc] = traj(accBody, omgBody); % Feed the current position and orientation to the imuSensor object [accel, gyro, mag] = imu(acc, omgBody, q); fusionfilt.predict(accel, gyro); % Fuse magnetometer samples at the magnetometer sample rate if ~mod(ii,imuSamplesPerMag) fusemag(fusionfilt, mag, magNoise); end % Sample and fuse the altimeter output at the altimeter sample rate if ~mod(ii,imuSamplesPerAlt) altHeight = altimeter(pos); % Use the |fusealtimeter| method to update the fusion filter with % the altimeter output. fusealtimeter(fusionfilt,altHeight,altimeterNoise); end % Log the actual orientation and position [actP(ii), actQ(ii)] = pose(fusionfilt); % Log the expected orientation and position expQ(ii) = q; expP(ii) = pos(3); if usePoseViewer hfunc.view(actP(ii), actQ(ii),expP(ii), expQ(ii)); %#ok<*UNRCH> end end
Rendimiento del filtro de trazado
Trazar el rendimiento del filtro. La pantalla muestra el error en la orientación utilizando la distancia del cuaternión y el error de altura.
hfunc.plotErrs(actP, actQ, expP, expQ);
Conclusión
Este ejemplo muestra cómo el ahrs10filter
realiza la fusión de sensores de 10 ejes para altura y orientación.