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, el giroscopio y el magnetómetro se simulan utilizando imuSensor. El altímetro se modela utilizando 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 del 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 la posición y la orientación estimadas junto con la ground-truth a medida que se ejecuta la simulación, configure 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 usar ahrs10filter para realizar la fusión de sensores de 10 ejes para altura y orientación.
