Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

ahrs10filter

Altura y orientación a partir de lecturas MARG y altímetro.

Descripción

El objeto ahrs10filter fusiona datos del sensor MARG y del altímetro para estimar la altura y la orientación del dispositivo. Los datos MARG (magnéticos, de velocidad angular y de gravedad) generalmente se derivan de sensores de magnetómetro, giroscopio y acelerómetro. El filtro utiliza un vector de estado de 18 elementos para rastrear el cuaternión de orientación, la velocidad vertical, la posición vertical, los sesgos del sensor MARG y el vector geomagnético. El objeto ahrs10filter utiliza un filtro Kalman extendido para estimar estas cantidades.

Creación

Sintaxis

FUSE = ahrs10filter

FUSE = ahrs10filter('ReferenceFrame',RF)

FUSE = ahrs10filter(___,Name=Value)

Descripción

FUSE = ahrs10filter devuelve un objeto de filtro Kalman extendido, FUSE, para la fusión del sensor de lecturas MARG y del altímetro para estimar la altura y la orientación del dispositivo.

FUSE = ahrs10filter('ReferenceFrame',RF) devuelve un objeto de filtro Kalman extendido que estima la altura y la orientación del dispositivo en relación con el marco de referencia RF.

FUSE = ahrs10filter(___,Name=Value) establece una o más propiedades utilizando argumentos de nombre-valor además de cualquiera de los argumentos de entrada anteriores.

Argumentos de entrada

RF — Sistema de coordenadas de navegación local

'NED' (predeterminado) | 'ENU'

Sistema de coordenadas de navegación local, especificado como 'NED' (Noreste-Abajo) o 'ENU' (Este-Norte-Arriba).

Tipos de datos: char | string

Propiedades

IMUSampleRate — Frecuencia de muestreo de la IMU (Hz)

100 (predeterminado) | escalar positivo

Frecuencia de muestreo de la IMU en Hz, especificada como un escalar positivo.

Tipos de datos: single | double

GyroscopeNoise — Varianza del ruido del proceso multiplicativo del giroscopio ((rad/s)²)

[1e-9,1e-9,1e-9] (predeterminado) | escalar | vector fila de tres elementos

Variación del ruido del proceso multiplicativo del giroscopio en (rad/s)², especificada como números finitos reales positivos.

Tipos de datos: single | double

AccelerometerNoise — Varianza del ruido del proceso multiplicativo del acelerómetro ((m/s²)²)

[1e-4,1e-4,1e-4] (predeterminado) | escalar | vector fila de tres elementos

Varianza del ruido del proceso multiplicativo del acelerómetro en (m/s²)², especificada como números finitos reales positivos.

Tipos de datos: single | double

GyroscopeBiasNoise — Varianza del ruido del proceso multiplicativo a partir del sesgo del giroscopio ((rad/s²)²)

[1e-10,1e-10,1e-10] (predeterminado) | escalar | vector fila de tres elementos

Variación del ruido del proceso multiplicativo a partir del sesgo del giroscopio en (rad/s²)², especificado como números finitos reales positivos.

Tipos de datos: single | double

AccelerometerBiasNoise — Variación del ruido del proceso multiplicativo debido al sesgo del acelerómetro ((m/s²)²)

[1e-4,1e-4,1e-4] (predeterminado) | escalar | vector fila de tres elementos

Variación del ruido del proceso multiplicativo a partir del sesgo del acelerómetro en (m/s²)², especificado como números finitos reales positivos.

Tipos de datos: single | double

GeomagneticVectorNoise — Ruido de proceso aditivo para el vector geomagnético (μT²)

[1e-6,1e-6,1e-6] (predeterminado) | escalar | vector fila de tres elementos

Ruido de proceso aditivo para el vector geomagnético en μT², especificado como números finitos reales positivos.

Tipos de datos: single | double

MagnetometerBiasNoise — Ruido de proceso aditivo para polarización del magnetómetro (μT²)

[0.1,0.1,0.1] (predeterminado) | escalar | vector fila de tres elementos

Ruido de proceso aditivo para la polarización del magnetómetro en μT², especificado como números finitos reales positivos.

Tipos de datos: single | double

State — Vector de estado del filtro de Kalman extendido

vector columna de 18 elementos

Vector de estado del filtro de Kalman extendido. Los valores estatales representan:

Estado	Unidades	Índice
Orientación (partes del cuaternión)	N / A	1:4
Altitud (NED o ENU)	metro	5
Velocidad vertical (NED o ENU)	EM	6
Sesgo del ángulo delta (XYZ)	rad/s	7:9
Sesgo de velocidad delta (XYZ)	EM	10:12
Vector de campo geomagnético (NED o ENU)	µT	13:15
Polarización del magnetómetro (XYZ)	µT	16:18

El estado inicial predeterminado corresponde a un objeto en reposo ubicado en [0 0 0] en coordenadas geodésicas LLA.

Tipos de datos: single | double

StateCovariance — Covarianza de error de estado para el filtro de Kalman extendido

eye(18)*1e-6 (predeterminado) | matriz de 18 por 18

Covarianza de error de estado para el filtro de Kalman, especificada como una matriz de números reales de 18 por 18 elementos.

Tipos de datos: single | double

Funciones del objeto

predict	Actualizar estados utilizando datos del acelerómetro y giroscopio para ahrs10filter
fusemag	Estados correctos utilizando datos del magnetómetro para ahrs10filter
fusealtimeter	Estados correctos utilizando datos del altímetro para ahrs10filter
correct	Estados correctos utilizando mediciones de estado directas para ahrs10filter
residual	Residuos y covarianzas residuales de mediciones de estado directas para ahrs10filter
residualmag	Residuos y covarianza residual de las mediciones del magnetómetro para ahrs10filter
residualaltimeter	Residuos y covarianza residual de las mediciones del altímetro para ahrs10filter
pose	Estimación de la orientación y posición actual para ahrs10filter
reset	Restablecer estados internos para ahrs10filter
stateinfo	Mostrar información del vector de estado para ahrs10filter
tune	Ajuste los parámetros ahrs10filter para reducir el error de estimación
copy	Crear copia de ahrs10filter

Ejemplos

Estimar la pose del VANT

Cargue datos de sensores registrados, pose ground-truth y estado inicial y covarianza del estado inicial. Calcule el número de muestras de IMU por muestra de altímetro y el número de muestras de IMU por muestra de magnetómetro.

load('fuse10exampledata.mat', ...
     'imuFs','accelData','gyroData', ...
     'magnetometerFs','magData', ...
     'altimeterFs','altData', ...
     'expectedHeight','expectedOrient', ...
     'initstate','initcov');

imuSamplesPerAlt = fix(imuFs/altimeterFs);
imuSamplesPerMag = fix(imuFs/magnetometerFs);

Cree un filtro AHRS que fusione lecturas de MARG y altímetro para estimar la altura y la orientación. Establezca la frecuencia de muestreo y los ruidos de medición de los sensores. Los valores se determinaron a partir de hojas de datos y experimentación.

filt = ahrs10filter('IMUSampleRate',imuFs, ...
                    'AccelerometerNoise',0.1, ...
                    'State',initstate, ...
                    'StateCovariance',initcov);

Ralt = 0.24;
Rmag = 0.9;

Preasigne variables a la altura y orientación del registro.

numIMUSamples = size(accelData,1);
estHeight = zeros(numIMUSamples,1);
estOrient = zeros(numIMUSamples,1,'quaternion');

Combinar datos de acelerómetro, giroscopio, magnetómetro y altímetro. El bucle externo predice el avance del filtro a la frecuencia de muestreo más rápida (la frecuencia de muestreo de la IMU).

for ii = 1:numIMUSamples
    
    % Use predict to estimate the filter state based on the accelometer and
    % gyroscope data.
    predict(filt,accelData(ii,:),gyroData(ii,:));
    
    % Magnetometer data is collected at a lower rate than IMU data. Fuse
    % magnetometer data at the lower rate.
    if ~mod(ii,imuSamplesPerMag)
        fusemag(filt,magData(ii,:),Rmag);
    end
    
    % Altimeter data is collected at a lower rate than IMU data. Fuse
    % altimeter data at the lower rate.
    if ~mod(ii, imuSamplesPerAlt)
        fusealtimeter(filt,altData(ii),Ralt);
    end
    
    % Log the current height and orientation estimate.
    [estHeight(ii),estOrient(ii)] = pose(filt);
end

Calcule los errores RMS entre la altura y orientación verdaderas conocidas y la salida del filtro AHRS.

pErr = expectedHeight - estHeight;
qErr = rad2deg(dist(expectedOrient,estOrient));

pRMS = sqrt(mean(pErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Altitude RMS Error\n');

Altitude RMS Error

fprintf('\t%.2f (meters)\n\n',pRMS);

	0.38 (meters)

Visualice la altura real y estimada a lo largo del tiempo.

t = (0:(numIMUSamples-1))/imuFs;
plot(t,expectedHeight);hold on
plot(t,estHeight);hold off
legend('Ground Truth','Estimated Height','location','best')
ylabel('Height (m)')
xlabel('Time (s)')
grid on

fprintf('Quaternion Distance RMS Error\n');

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n',qRMS);

	2.93 (degrees)

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2019a

Consulte también

ahrsfilter | insfilter