Introducción a la simulación de mediciones IMU
Este ejemplo muestra cómo simular mediciones de unidades de medida inercial (IMU) utilizando el System object imuSensor
. Una IMU puede incluir una combinación de sensores individuales, incluido un giroscopio, un acelerómetro y un magnetómetro. Puede especificar las propiedades de los sensores individuales usando gyroparams
, accelparams
y magparams
, respectivamente.
En los siguientes gráficos, a menos que se indique lo contrario, solo se muestran las medidas del eje x. Utilice los controles deslizantes para ajustar los parámetros de forma interactiva.
Parámetros predeterminados
Los parámetros predeterminados para el modelo de giroscopio simulan una señal ideal. Dada una entrada sinusoidal, la salida del giroscopio debería coincidir exactamente.
params = gyroparams
params = gyroparams with properties: MeasurementRange: Inf rad/s Resolution: 0 (rad/s)/LSB ConstantBias: [0 0 0] rad/s AxesMisalignment: [3⨯3 double] % NoiseDensity: [0 0 0] (rad/s)/√Hz BiasInstability: [0 0 0] rad/s RandomWalk: [0 0 0] (rad/s)*√Hz NoiseType: "double-sided" BiasInstabilityCoefficients: [1⨯1 struct] TemperatureBias: [0 0 0] (rad/s)/°C TemperatureScaleFactor: [0 0 0] %/°C AccelerationBias: [0 0 0] (rad/s)/(m/s²)
% Generate N samples at a sampling rate of Fs with a sinusoidal frequency % of Fc. N = 1000; Fs = 100; Fc = 0.25; t = (0:(1/Fs):((N-1)/Fs)).'; acc = zeros(N, 3); angvel = zeros(N, 3); angvel(:,1) = sin(2*pi*Fc*t); imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Ideal Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
Ajuste de parámetros de hardware
Los siguientes parámetros modelan las limitaciones o defectos del hardware. Algunos se pueden corregir mediante calibración.
MeasurementRange
determina el valor absoluto máximo informado por el giroscopio. Los valores absolutos más grandes están saturados. El efecto se muestra estableciendo el rango de medición en un valor menor que la amplitud de la velocidad angular sinusoidal de ground-truth.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.MeasurementRange = 0.5; % rad/s [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Saturated Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
Resolution
afecta el tamaño del paso de las mediciones digitales. Utilice este parámetro para modelar los efectos de cuantificación del convertidor analógico a digital (ADC). El efecto se muestra aumentando el parámetro a un valor mucho mayor de lo habitual.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.Resolution = 0.5; % (rad/s)/LSB [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Quantized Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
AxesMisalignment
es la cantidad de desviación en los ejes del sensor. Esta desviación normalmente ocurre cuando el sensor está montado en la PCB y se puede corregir mediante calibración. El efecto se muestra al inclinar ligeramente el eje x y trazar tanto el eje x como el eje y.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); xMisalignment = 2; % percent imu.Gyroscope.AxesMisalignment = [xMisalignment, 0, 0]; % percent [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1:2), '--', t, gyroData(:,1:2)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Misaligned Gyroscope Data') legend('x (ground truth)', 'y (ground truth)', ... 'x (gyroscope)', 'y (gyroscope)')
ConstantBias
ocurre en las mediciones del sensor debido a defectos de hardware. Dado que este sesgo no es causado por factores ambientales, como la temperatura, se puede corregir mediante la calibración.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); xBias = 0.4; % rad/s imu.Gyroscope.ConstantBias = [xBias, 0, 0]; % rad/s [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Biased Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
Ajuste aleatorio de parámetros de ruido
Los siguientes parámetros modelan el ruido aleatorio en las mediciones del sensor. Puede encontrar más información sobre estos parámetros en el ejemplo Inertial Sensor Noise Analysis Using Allan Variance (Sensor Fusion and Tracking Toolbox) .
NoiseDensity
es la cantidad de ruido blanco en la medición del sensor. A veces se le llama caminata aleatoria de ángulo para giroscopios o caminata aleatoria de velocidad para acelerómetros.
rng('default') imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.NoiseDensity = 0.0125; % (rad/s)/sqrt(Hz) [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('White Noise Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
BiasInstability
es la cantidad de ruido rosa o parpadeo en la medición del sensor.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.BiasInstability = 0.02; % rad/s [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Bias Instability Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
RandomWalk
es la cantidad de ruido browniano en la medición del sensor. A veces se le llama caminata aleatoria de velocidad para giroscopios o caminata aleatoria de aceleración para acelerómetros.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.RandomWalk = 0.091; % (rad/s)*sqrt(Hz) [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Random Walk Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
Ajuste de parámetros ambientales
Los siguientes parámetros modelan el ruido que surge de cambios en el entorno del sensor.
TemperatureBias
es el sesgo agregado a las mediciones del sensor debido a la diferencia de temperatura con respecto a la temperatura de funcionamiento predeterminada. La mayoría de las hojas de datos de los sensores enumeran la temperatura de funcionamiento predeterminada como 25 grados Celsius. Este sesgo se muestra configurando el parámetro en un valor distinto de cero y configurando la temperatura de funcionamiento en un valor superior a 25 grados Celsius.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.TemperatureBias = 0.06; % (rad/s)/(degrees C) imu.Temperature = 35; [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Temperature-Biased Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
TemperatureScaleFactor
es el error en el factor de escala del sensor debido a cambios en la temperatura de funcionamiento. Esto provoca errores en el escalado de la medición; en otras palabras, los valores ideales más pequeños tienen menos error que los valores más grandes. Este error se muestra aumentando linealmente la temperatura.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.TemperatureScaleFactor = 3.2; % %/(degrees C) standardTemperature = 25; % degrees C temperatureSlope = 2; % (degrees C)/s temperature = temperatureSlope*t + standardTemperature; gyroData = zeros(N, 3); for i = 1:N imu.Temperature = temperature(i); [~, gyroData(i,:)] = imu(acc(i,:), angvel(i,:)); end figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Temperature-Scaled Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')
AccelerationBias
es el sesgo agregado a la medición del giroscopio debido a las aceleraciones lineales. Este parámetro es específico del giroscopio. Este sesgo se muestra estableciendo el parámetro en un valor distinto de cero y utilizando una aceleración de entrada distinta de cero.
imu = imuSensor('SampleRate', Fs, 'Gyroscope', params); imu.Gyroscope.AccelerationBias = 0.3; % (rad/s)/(m/s^2) acc(:,1) = 1; [~, gyroData] = imu(acc, angvel); figure plot(t, angvel(:,1), '--', t, gyroData(:,1)) xlabel('Time (s)') ylabel('Angular Velocity (rad/s)') title('Acceleration-Biased Gyroscope Data') legend('x (ground truth)', 'x (gyroscope)')