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

ahrsfilter

Orientación a partir de lecturas de acelerómetro, giroscopio y magnetómetro

Descripción

ahrsfilter System object™ fusiona datos del sensor acelerómetro, magnetómetro y giroscopio para estimar la orientación del dispositivo.

Para estimar la orientación del dispositivo:

1. Cree el objeto ahrsfilter y configure sus propiedades.

2. Llame al objeto con argumentos, como si fuera una función.

Para más información sobre el funcionamiento de los System objects, consulte ¿Qué son los System Objects?

Creación

Sintaxis

FUSE = ahrsfilter

FUSE = ahrsfilter('ReferenceFrame',RF)

FUSE = ahrsfilter(___,Name=Value)

Descripción

FUSE = ahrsfilter devuelve un filtro Kalman indirecto System object, FUSE, para la fusión de datos del sensor del acelerómetro, giroscopio y magnetómetro para estimar la orientación y la velocidad angular del dispositivo. El filtro utiliza un vector de estado de 12 elementos para rastrear el error de estimación de la orientación, el sesgo del giroscopio, la aceleración lineal y la perturbación magnética.

FUSE = ahrsfilter('ReferenceFrame',RF) devuelve un ahrsfilter System object que fusiona datos del acelerómetro, giroscopio y magnetómetro para estimar la orientación del dispositivo en relación con el marco de referencia RF.

FUSE = ahrsfilter(___,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

A menos que se indique lo contrario, las propiedades son no ajustables, lo que significa que no puede modificar sus valores después de llamar al objeto. Los objetos se bloquean cuando llama a ellos, y la función release los desbloquea.

Si una propiedad es ajustable, puede modificar su valor en cualquier momento.

Para obtener más información sobre cómo modificar los valores de las propiedades, consulte Diseñar sistemas en MATLAB utilizando System objects.

SampleRate — Frecuencia de muestreo de entrada de datos del sensor (Hz)

100 (predeterminado) | escalar positivo

Frecuencia de muestreo de entrada de los datos del sensor en Hz, especificada como un escalar positivo.

Ajustable: No

Tipos de datos: single | double

DecimationFactor — Factor de diezmado

1 (predeterminado) | número entero positivo

Factor de diezmado mediante el cual reducir la velocidad de datos del sensor de entrada como parte del algoritmo de fusión, especificado como un número entero positivo.

El número de filas de las entradas –– accelReadings, gyroReadings y magReadings –– debe ser un múltiplo del factor de diezmado.

Tipos de datos: single | double

AccelerometerNoise — Varianza del ruido de la señal del acelerómetro ((m/s²)²)

0.00019247 (predeterminado) | escalar real positivo

Varianza del ruido de la señal del acelerómetro en (m/s²)², especificada como un escalar real positivo.

Ajustable: Yes

Tipos de datos: single | double

MagnetometerNoise — Varianza del ruido de la señal del magnetómetro (μT²)

0.1 (predeterminado) | escalar real positivo

Varianza del ruido de la señal del magnetómetro en μT², especificada como un escalar real positivo.

Ajustable: Yes

Tipos de datos: single | double

GyroscopeNoise — Varianza del ruido de la señal del giroscopio ((rad/s)²)

9.1385e-5 (predeterminado) | escalar real positivo

Varianza del ruido de la señal del giroscopio en (rad/s)², especificada como un escalar real positivo.

Ajustable: Yes

Tipos de datos: single | double

GyroscopeDriftNoise — Varianza de la deriva del giroscopio ((rad/s)²)

3.0462e-13 (predeterminado) | escalar real positivo

Varianza de la deriva del giroscopio en (rad/s)², especificada como un escalar real positivo.

Ajustable: Yes

Tipos de datos: single | double

LinearAccelerationNoise — Varianza del ruido de aceleración lineal (m/s²)²

0.0096236 (predeterminado) | escalar real positivo

Varianza del ruido de aceleración lineal en (m/s²)², especificada como un escalar real positivo. La aceleración lineal se modela como un proceso de ruido blanco filtrado por paso bajo.

Ajustable: Yes

Tipos de datos: single | double

LinearAccelerationDecayFactor — Factor de caída para la deriva de aceleración lineal

0.5 (predeterminado) | escalar en el rango [0,1)

Factor de caída para la deriva de aceleración lineal, especificado como un escalar en el rango [0,1). Si la aceleración lineal cambia rápidamente, configure LinearAcclerationDecayFactor en un valor más bajo. Si la aceleración lineal cambia lentamente, configure LinearAcclerationDecayFactor en un valor más alto. La deriva de la aceleración lineal se modela como un proceso de ruido blanco filtrado por paso bajo.

Ajustable: Yes

Tipos de datos: single | double

MagneticDisturbanceNoise — Varianza del ruido de perturbación magnética (μT²)

0.5 (predeterminado) | escalar positivo finito real

Varianza del ruido de perturbación magnética en μT², especificado como un escalar positivo finito real.

Ajustable: Yes

Tipos de datos: single | double

MagneticDisturbanceDecayFactor — Factor de desintegración por perturbación magnética.

0.5 (predeterminado) | escalar positivo en el rango [0,1]

Factor de decaimiento para perturbaciones magnéticas, especificado como un escalar positivo en el rango [0,1]. La perturbación magnética se modela como un proceso de Markov de primer orden.

Ajustable: Yes

Tipos de datos: single | double

InitialProcessNoise — Matriz de covarianza para el ruido del proceso.

matriz de 12 por 12

Matriz de covarianza para el ruido del proceso, especificada como una matriz de 12 por 12. El valor predeterminado es:

  Columns 1 through 6

   0.000006092348396                   0                   0                   0                   0                   0
                   0   0.000006092348396                   0                   0                   0                   0
                   0                   0   0.000006092348396                   0                   0                   0
                   0                   0                   0   0.000076154354947                   0                   0
                   0                   0                   0                   0   0.000076154354947                   0
                   0                   0                   0                   0                   0   0.000076154354947
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0

  Columns 7 through 12

                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
   0.009623610000000                   0                   0                   0                   0                   0
                   0   0.009623610000000                   0                   0                   0                   0
                   0                   0   0.009623610000000                   0                   0                   0
                   0                   0                   0   0.600000000000000                   0                   0
                   0                   0                   0                   0   0.600000000000000                   0
                   0                   0                   0                   0                   0   0.600000000000000

La matriz de covarianza del proceso inicial explica el error en el modelo de proceso.

Tipos de datos: single | double

ExpectedMagneticFieldStrength — Estimación esperada de la intensidad del campo magnético (μT)

50 (predeterminado) | escalar positivo real

Estimación esperada de la intensidad del campo magnético en μT, especificada como un escalar positivo real. La intensidad del campo magnético esperada es una estimación de la intensidad del campo magnético de la Tierra en la ubicación actual.

Ajustable: Yes

Tipos de datos: single | double

OrientationFormat — Formato de orientación de salida

'quaternion' (predeterminado) | 'Rotation matrix'

Formato de orientación de salida, especificado como 'quaternion' o 'Rotation matrix'. El tamaño de la salida depende del tamaño de entrada, N, y del formato de orientación de salida:

• 'quaternion' –– La salida es un N por 1 quaternion.

• 'Rotation matrix' –– La salida es una matriz de rotación de 3 por 3 por N.

Tipos de datos: char | string

Uso

Sintaxis

[orientation,angularVelocity,interData] = FUSE(accelReadings,gyroReadings,magReadings)

Descripción

[orientation,angularVelocity,interData] = FUSE(accelReadings,gyroReadings,magReadings) fusiona datos del acelerómetro, giroscopio y magnetómetro para calcular mediciones de orientación y velocidad angular. El algoritmo supone que el dispositivo está parado antes de la primera llamada.

Argumentos de entrada

accelReadings — Lecturas del acelerómetro en el sistema de coordenadas del cuerpo del sensor (m/s²)

matriz de N por 3

Lecturas del acelerómetro en el sistema de coordenadas del cuerpo del sensor en m/s², especificadas como una matriz N por 3. N es el número de muestras y las tres columnas de accelReadings representan las [x y z] mediciones. Se supone que las lecturas del acelerómetro corresponden a la frecuencia de muestreo especificada por la propiedad SampleRate.

Tipos de datos: single | double

gyroReadings — Lecturas del giroscopio en el sistema de coordenadas del cuerpo del sensor (rad/s)

matriz de N por 3

Lecturas del giroscopio en el sistema de coordenadas del cuerpo del sensor en rad/s, especificadas como una matriz de N por 3. N es el número de muestras y las tres columnas de gyroReadings representan las [x y z] mediciones. Se supone que las lecturas del giroscopio corresponden a la frecuencia de muestreo especificada por la propiedad SampleRate.

Tipos de datos: single | double

magReadings — Lecturas del magnetómetro en el sistema de coordenadas del cuerpo del sensor (μT)

matriz de N por 3

Lecturas del magnetómetro en el sistema de coordenadas del cuerpo del sensor en µT, especificadas como una matriz de N por 3. N es el número de muestras y las tres columnas de magReadings representan las [x y z] mediciones. Se supone que las lecturas del magnetómetro corresponden a la frecuencia de muestreo especificada por la propiedad SampleRate.

Tipos de datos: single | double

Argumentos de salida

orientation — Orientación que rota cantidades desde el sistema de coordenadas de navegación local al sistema de coordenadas del cuerpo del sensor.

Arreglo de cuaterniones M por 1 (predeterminado) | Arreglo de 3 por 3 por M

Orientación que puede rotar cantidades del sistema de coordenadas de navegación local a un sistema de coordenadas corporales, devuelta como cuaterniones o un arreglo. El tamaño y el tipo de orientation dependen de si la propiedad OrientationFormat está establecida en 'quaternion' o 'Rotation matrix':

• 'quaternion' –– la salida es un vector de cuaterniones M por 1, con el mismo tipo de datos subyacente que las entradas

• 'Rotation matrix' –– la salida es un arreglo de matrices de rotación de 3 por 3 por M del mismo tipo de datos que las entradas

El número de muestras de entrada, N, y la propiedad DecimationFactor determinan M.

Puede utilizar orientation en una función rotateframe para rotar cantidades desde un sistema de navegación local a un sistema de coordenadas del cuerpo del sensor.

Tipos de datos: quaternion | single | double

angularVelocity — Velocidad angular en el sistema de coordenadas del cuerpo del sensor (rad/s)

Arreglo de M por 3 (predeterminado)

Velocidad angular con el sesgo del giroscopio eliminado en el sistema de coordenadas del cuerpo del sensor en rad/s, devuelta como un arreglo M por 3. El número de muestras de entrada, N, y la propiedad DecimationFactor determinan M.

Tipos de datos: single | double

interData — Datos intermedios

estructura

Desde R2024a

Datos intermedios, devueltos como una estructura que contiene residuos y covarianza residual.

Tipos de datos: struct

Funciones del objeto

Para usar una función de objeto, especifique el System object como el primer argumento de entrada. Por ejemplo, para liberar recursos de sistema de un System object llamado obj, utilice la siguiente sintaxis:

release(obj)

Específico de ahrsfilter

tune	Ajuste los parámetros ahrsfilter para reducir el error de estimación
residual	Calcular la covarianza residual y residual para ahrsfilter

Comunes a todos los System objects

step	Ejecutar el algoritmo System object
release	Release resources and allow changes to System object property values and input characteristics
reset	Reset internal states of System object

Ejemplos

Estimar la orientación usando ahrsfilter

Cargue el archivo rpy_9axis, que contiene datos registrados de los sensores acelerómetro, giroscopio y magnetómetro de un dispositivo que oscila en cabeceo (alrededor del eje y), luego en guiñada (alrededor del eje z) y luego en balanceo (alrededor del eje x). El archivo también contiene la frecuencia de muestreo de la grabación.

load 'rpy_9axis' sensorData Fs
accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;
magnetometerReadings = sensorData.MagneticField;

Cree un ahrsfilter System object ™ con SampleRate establecido en la frecuencia de muestreo de los datos del sensor. Especifique un factor de diezmado de dos para reducir el coste computacional del algoritmo.

decim = 2;
fuse = ahrsfilter('SampleRate',Fs,'DecimationFactor',decim);

Pase las lecturas del acelerómetro, del giroscopio y del magnetómetro al objeto ahrsfilter, fuse, para generar una estimación de la orientación del cuerpo del sensor a lo largo del tiempo. De forma predeterminada, la orientación se genera como un vector de cuaterniones.

q = fuse(accelerometerReadings,gyroscopeReadings,magnetometerReadings);

La orientación se define por el desplazamiento angular requerido para rotar un sistema de coordenadas principal a un sistema de coordenadas secundario. Trazar la orientación en ángulos de Euler en grados a lo largo del tiempo.

ahrsfilter estima correctamente el cambio de orientación a lo largo del tiempo, incluida la orientación inicial orientada al sur.

time = (0:decim:size(accelerometerReadings,1)-1)/Fs;

plot(time,eulerd(q,'ZYX','frame'))
title('Orientation Estimate')
legend('z-axis', 'y-axis', 'x-axis')
ylabel('Rotation (degrees)')

Simular interferencia magnética en ahrsFilter

Este ejemplo muestra cómo el rendimiento del ahrsfilter System object™ se ve afectado por el bloqueo magnético.

Cargue StationaryIMUReadings, que contiene lecturas del acelerómetro, magnetómetro y giroscopio de una IMU estacionaria.

load 'StationaryIMUReadings.mat' accelReadings magReadings gyroReadings SampleRate

numSamples = size(accelReadings,1);

El ahrsfilter utiliza la intensidad del campo magnético para estabilizar su orientación frente al supuesto campo magnético constante de la Tierra. Sin embargo, hay muchos objetos naturales y artificiales que generan campos magnéticos y pueden confundir el algoritmo. Para tener en cuenta la presencia de campos magnéticos transitorios, puede establecer la propiedad MagneticDisturbanceNoise en el objeto ahrsfilter.

Cree un objeto ahrsfilter con el factor de diezmado establecido en 2 y anote la intensidad del campo magnético esperada predeterminada.

decim = 2;
FUSE = ahrsfilter('SampleRate',SampleRate,'DecimationFactor',decim);

Fusione las lecturas de la IMU utilizando el filtro del sistema de referencia de actitud y rumbo (AHRS) y luego visualice la orientación del cuerpo del sensor a lo largo del tiempo. La orientación fluctúa al principio y se estabiliza después de aproximadamente 60 segundos.

orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');
time = (0:decim:(numSamples-1))'/SampleRate;

figure(1)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data')

Imite la interferencia magnética añadiendo un campo magnético transitorio y fuerte al campo magnético registrado en magReadings. Visualice el campo magnético interfiriéndose.

jamStrength = [10,5,2];
startStop = (50*SampleRate):(150*SampleRate);
jam = zeros(size(magReadings));
jam(startStop,:) = jamStrength.*ones(numel(startStop),3);

magReadings = magReadings + jam;

figure(2)
plot(time,magReadings(1:decim:end,:))
xlabel('Time (s)')
ylabel('Magnetic Field Strength (\mu T)')
title('Simulated Magnetic Field with Jamming')
legend('z-axis','y-axis','x-axis')

Ejecute la simulación nuevamente utilizando magReadings con interferencia magnética. Trace los resultados y observe la disminución del rendimiento en la estimación de la orientación.

reset(FUSE)
orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(3)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data with Magnetic Disturbance and Default Properties')

El filtro AHRS malinterpretó la interferencia magnética y la orientación del cuerpo del sensor se estimó incorrectamente. Puedes compensar el atasco aumentando la propiedad MagneticDisturbanceNoise. Al aumentar la propiedad MagneticDisturbanceNoise aumenta el rango de ruido asumido para la perturbación magnética, y toda la señal del magnetómetro tiene menos ponderación en el algoritmo de fusión subyacente de ahrsfilter.

Establezca MagneticDisturbanceNoise en 200 y ejecute la simulación nuevamente.

La salida de estimación de orientación de ahrsfilter es más precisa y se ve menos afectada por el transitorio magnético. Sin embargo, debido a que la señal del magnetómetro tiene menos peso en el algoritmo de fusión subyacente, el algoritmo puede tardar más en reestabilizarse.

reset(FUSE)
FUSE.MagneticDisturbanceNoise = 20;

orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(4)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data with Magnetic Disturbance and Modified Properties')

IMU de 9 ejes con vibración de seguimiento

Este ejemplo utiliza ahrsfilter System object™ para fusionar datos IMU de 9 ejes de un cuerpo de sensor que se agita. Trace la distancia del cuaternión entre el objeto y su posición de reposo final para visualizar el rendimiento y la rapidez con la que el filtro converge a la posición de reposo correcta. A continuación, ajuste los parámetros de ahrsfilter para que el filtro converja más rápidamente a la posición de reposo de ground-truth .

Cargue IMUReadingsShaken en su espacio de trabajo actual. Estos datos se registraron desde una IMU que se agitó y luego se colocó en posición de reposo. Visualice la aceleración, el campo magnético y la velocidad angular registrados por los sensores.

load 'IMUReadingsShaken' accelReadings gyroReadings magReadings SampleRate
numSamples = size(accelReadings,1);
time = (0:(numSamples-1))'/SampleRate;

figure(1)
subplot(3,1,1)
plot(time,accelReadings)
title('Accelerometer Reading')
ylabel('Acceleration (m/s^2)')

subplot(3,1,2)
plot(time,magReadings)
title('Magnetometer Reading')
ylabel('Magnetic Field (\muT)')

subplot(3,1,3)
plot(time,gyroReadings)
title('Gyroscope Reading')
ylabel('Angular Velocity (rad/s)')
xlabel('Time (s)')

Cree un ahrsfilter y luego fusione los datos de la IMU para determinar la orientación. La orientación se devuelve como un vector de cuaterniones; convierta los cuaterniones a ángulos de Euler en grados. Visualice la orientación del cuerpo del sensor a lo largo del tiempo trazando los ángulos de Euler necesarios, en cada paso de tiempo, para rotar el sistema de coordenadas global al sistema de coordenadas del cuerpo del sensor.

fuse = ahrsfilter('SampleRate',SampleRate);
orientation = fuse(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(2)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation over Time')
legend('Rotation around z-axis', ...
       'Rotation around y-axis', ...
       'Rotation around x-axis')

En la grabación IMU, el temblor se detiene después de aproximadamente seis segundos. Determina la orientación en reposo para que puedas caracterizar qué tan rápido converge ahrsfilter.

Para determinar la orientación en reposo, calcule los promedios del campo magnético y la aceleración durante los últimos cuatro segundos y luego utilice la función ecompass para fusionar los datos.

Visualice la distancia del cuaternión desde la posición de reposo a lo largo del tiempo.

restingOrientation = ecompass(mean(accelReadings(6*SampleRate:end,:)), ...
                              mean(magReadings(6*SampleRate:end,:)));

figure(3)
plot(time,rad2deg(dist(restingOrientation,orientation)))
hold on
xlabel('Time (s)')
ylabel('Quaternion Distance (degrees)')

Modifique las propiedades predeterminadas ahrsfilter para que el filtro converja a la gravedad más rápidamente. Aumente GyroscopeDriftNoise a 1e-2 y disminuya LinearAccelerationNoise a 1e-4. Esto le indica al algoritmo ahrsfilter que pondere menos los datos del giroscopio y más los datos del acelerómetro. Debido a que los datos accelerometer proporcionan el vector de gravedad estabilizador y consistente, la orientación resultante converge más rápidamente.

Restablezca el filtro, fusione los datos y trace los resultados.

fuse.LinearAccelerationNoise = 1e-4;
fuse.GyroscopeDriftNoise     = 1e-2;
reset(fuse)

orientation = fuse(accelReadings,gyroReadings,magReadings);

figure(3)
plot(time,rad2deg(dist(restingOrientation,orientation)))
legend('Default AHRS Filter','Tuned AHRS Filter')

Algoritmos

Nota: El siguiente algoritmo sólo se aplica a un marco de referencia NED.

El ahrsfilter utiliza la estructura de filtro Kalman de nueve ejes descrita en [1]. El algoritmo intenta rastrear los errores de orientación, desplazamiento del giroscopio, aceleración lineal y perturbación magnética para generar la orientación final y la velocidad angular. En lugar de rastrear la orientación directamente, el filtro Kalman indirecto modela el proceso de error, x, con una actualización recursiva:

$$x_k=\left[\begin{array}{c}{\theta }_k\\ b_k\\ a_k\\ d_k\end{array}\right]=F_k\left[\begin{array}{c}{\theta }_{k-1}\\ b_{k-1}\\ a_{k-1}\\ d_{k-1}\end{array}\right]+w_k$$

donde x_k es un vector de 12 por 1 que consta de:

• θ_k –– Vector de error de orientación de 3 por 1, en grados, en el tiempo k

• b_k –– Vector de sesgo de velocidad angular cero del giroscopio 3 por 1, en grados/s, en el momento k

• a_k –– Vector de error de aceleración de 3 por 1 medido en el marco del sensor, en g, en el tiempo k

• d_k –– Vector de error de perturbación magnética de 3 por 1 medido en el marco del sensor, en µT, en el tiempo k

y donde w_k es un vector de ruido aditivo de 12 por 1, y F_k es el modelo de transición de estado.

Dado que x_k se define como el proceso de error, la estimación a priori siempre es cero y, por lo tanto, el modelo de transición de estado, F_k, es cero. Esta idea da como resultado la siguiente reducción de las ecuaciones estándar de Kalman:

Ecuaciones de Kalman estándar:

$$\begin{array}{l}{x}_k^{-}=F_k{x}_{k-1}^{+}\\ P_k^{-}=F_k{P}_{k-1}^{+}{F}_k^T+Q_k\\ \\ y_k=z_k-H_k{x}_k^{-}\\ S_k=R_k+H_k{P}_k^{-}{H}_k{}^T\\ K_k=P_k^{-}{H}_k^T{\left(S_k\right)}^{-1}\\ x_k^{+}=x_k^{-}+K_k{y}_k\\ P_k^{+}=P_k{}^{-}-K_k{H}_k{P}_k^{-}\end{array}$$

Ecuaciones de Kalman utilizadas en este algoritmo:

$$\begin{array}{l}{x}_k^{-}=0\\ P_k^{-}=Q_k\\ \\ y_k=z_k\\ S_k=R_k+H_k{P}_k^{-}{H}_k{}^T\\ K_k=P_k^{-}{H}_k^T{\left(S_k\right)}^{-1}\\ x_k^{+}=K_k{y}_k\\ P_k^{+}=P_k{}^{-}-K_k{H}_k{P}_k^{-}\end{array}$$

donde:

• x_k⁻ –– estimación del estado predicho (a priori); el proceso de error

• P_k⁻ –– estimación de covarianza prevista (a priori)

• y_k –– innovación

• S_k –– covarianza de la innovación

• K_k –– Ganancia de Kalman

• x_k⁺ –– estimación de estado actualizada (a posteriori)

• P_k⁺ –– estimación de covarianza actualizada (a posteriori)

k representa la iteración, el superíndice ⁺ representa una estimación a posteriori y el superíndice ⁻ representa una estimación a priori.

El gráfico y los siguientes pasos describen una iteración basada en un solo cuadro a través del algoritmo.

Antes de la primera iteración, las entradas accelReadings, gyroReadings y magReadings se dividen en cuadros DecimationFactor por 3. Para cada fragmento, el algoritmo utiliza las lecturas más actuales del acelerómetro y magnetómetro correspondientes al fragmento de lecturas del giroscopio.

Descripción detallada

Lea el algoritmo para obtener una explicación de cada etapa de la descripción detallada.

Modelo

El algoritmo modela la aceleración y el cambio angular como procesos lineales.

Predecir la orientación

La orientación del cuadro actual se predice estimando primero el cambio angular respecto del cuadro anterior:

$$\Delta {\phi }_{N\times 3}=\frac{\left(gyroReading{s}_{N\times 3}-gyroOffse{t}_{1\times 3}\right)}{fs}$$

donde N es el factor de diezmado especificado por la propiedad DecimationFactor y fs es la frecuencia de muestreo especificada por la propiedad SampleRate.

El cambio angular se convierte en cuaterniones utilizando la sintaxis de construcción rotvec quaternion:

$$\Delta Q_{N\times 1}=\mathrm{quaternion}(\Delta {\phi }_{N\times 3},\text{'}rotvec\text{'})$$

La estimación de orientación anterior se actualiza girándola por Δ Q:

$$q_{1\times 1}^{-}=\left(q_{1\times 1}^{+}\right)\left({\displaystyle \prod _{n=1}^N\Delta Q_n}\right)$$

Durante la primera iteración, la estimación de orientación, q⁻, se inicializa mediante ecompass.

Estimar la gravedad a partir de la orientación

El vector de gravedad se interpreta como la tercera columna del cuaternión, q⁻, en forma de matriz de rotación:

$$g_{1\times 3}={\left(rPrior(:,3)\right)}^T$$

Consulte [1] para obtener una explicación de por qué la tercera columna de rPrior puede interpretarse como el vector de gravedad.

Estimar la gravedad a partir de la aceleración

Se realiza una segunda estimación del vector de gravedad restando la estimación de aceleración lineal decaida de la iteración anterior de las lecturas del acelerómetro:

$$gAcce{l}_{1\times 3}=accelReading{s}_{1\times 3}-linAccelprio{r}_{1\times 3}$$

Estimar el vector magnético de la Tierra

El vector magnético de la Tierra se estima rotando la estimación del vector magnético de la iteración anterior por la estimación de orientación a priori, en forma de matriz de rotación:

$$mGyr{o}_{1\times 3}={\left(\left(rPrior\right)\left(m^T\right)\right)}^T$$

Modelo de error

El modelo de error combina dos diferencias:

• La diferencia entre la estimación de la gravedad a partir de las lecturas del acelerómetro y la estimación de la gravedad a partir de las lecturas del giroscopio: $$z_g=g-gAccel$$

• La diferencia entre la estimación del vector magnético a partir de las lecturas del giroscopio y la estimación del vector magnético a partir del magnetómetro:$$z_m=mGyro-magReadings$$

Magnetómetro correcto

El magnetómetro calcula correctamente el error en la estimación del vector magnético y detecta interferencias magnéticas.

Error de perturbación del magnetómetro

El error de perturbación magnética se calcula multiplicando la matriz de la ganancia de Kalman asociada al vector magnético con la señal de error:

$$mErro{r}_{3\times 1}={\left(\left(K{(\text{10:12},:)}_{3\times 6}\right){\left(z_{1\times 6}\right)}^T\right)}^T$$

La ganancia de Kalman, K, es la ganancia de Kalman calculada en la iteración actual.

Detección de interferencias magnéticas

La interferencia magnética se determina verificando que la potencia de la perturbación magnética detectada sea menor o igual a cuatro veces la potencia de la intensidad del campo magnético esperado:

$$tf=\{\begin{array}{c}\text{true}\\ \text{false}\end{array}\begin{array}{c}\text{if}\\ \text{else}\end{array}\begin{array}{c}{{\displaystyle \sum \left|mError\right|}}^2>\left(4\right){\left(\text{ExpectedMagneticFieldStrength}\right)}^2\\ \end{array}$$

ExpectedMagneticFieldStrength es una propiedad de ahrsfilter.

Ecuaciones de Kalman

Las ecuaciones de Kalman utilizan la estimación de gravedad derivada de las lecturas del giroscopio, g, la estimación del vector magnético derivada de las lecturas del giroscopio, mGyro, y la observación del proceso de error, z, para actualizar las matrices de ganancia de Kalman y de covarianza intermedia. La ganancia de Kalman se aplica a la señal de error, z, para generar una estimación de error a posteriori, x⁺.

Modelo de observación

El modelo de observación asigna los estados observados de 1 por 3, g y mGyro, al estado real de 6 por 12, H.

El modelo de observación se construye como:

$$H_{3\times 9}=\left[\begin{array}{cccccccccccc}0& g_z& -g_y& 0& -\kappa g_z& \kappa g_y& 1& 0& 0& 0& 0& 0\\ -g_z& 0& g_x& \kappa g_z& 0& -\kappa g_x& 0& 1& 0& 0& 0& 0\\ g_y& -g_x& 0& -\kappa g_y& \kappa g_x& 0& 0& 0& 1& 0& 0& 0\\ 0& m_z& -m_y& 0& -\kappa m_z& -\kappa m_y& 0& 0& 0& -1& 0& 0\\ -m_z& 0& m_x& \kappa m_z& 0& -\kappa m_x& 0& 0& 0& 0& -1& 0\\ m_y& -m_x& 0& -\kappa m_y& \kappa m_x& 0& 0& 0& 0& 0& 0& -1\end{array}\right]$$

donde g_x, g_y y g_z son los elementos x, y y z del vector de gravedad estimado a partir de la orientación a priori, respectivamente. m_x, m_y y m_z son los elementos x, y y z del vector magnético estimado a partir de la orientación a priori, respectivamente. κ es una constante determinada por las propiedades SampleRate y DecimationFactor: κ = DecimationFactor/SampleRate.

Consulte las secciones 7.3 y 7.4 de [1] para obtener una derivación del modelo de observación.

Covarianza de la innovación

La covarianza de innovación es una matriz de 6 por 6 que se utiliza para rastrear la variabilidad en las mediciones. La matriz de covarianza de innovación se calcula como:

$$S_{6x6}=R_{6x6}+\left(H_{6x12}\right)\left(P_{{}_{12x12}}^{-}\right){\left(H_{6x12}\right)}^T$$

donde

• H es la matriz del modelo de observación

• P⁻ es la estimación predicha (a priori) de la covarianza del modelo de observación calculado en la iteración anterior

• R es la covarianza del ruido del modelo de observación, calculada como:

  $$R_{6\times 6}=\left[\begin{array}{cccccc}acce{l}_{\text{noise}}& 0& 0& 0& 0& 0\\ 0& acce{l}_{\text{noise}}& 0& 0& 0& 0\\ 0& 0& acce{l}_{\text{noise}}& 0& 0& 0\\ 0& 0& 0& ma{g}_{\text{noise}}& 0& 0\\ 0& 0& 0& 0& ma{g}_{\text{noise}}& 0\\ 0& 0& 0& 0& 0& ma{g}_{\text{noise}}\end{array}\right]$$

  donde

  $$acce{l}_{\text{noise}}=\text{AccelerometerNoise}\text{+}\text{LinearAccelerationNoise}\text{+}{\kappa }^2\left(\text{GyroscopeDriftNoise}\text{+}\text{GyroscopeNoise}\right)$$

  y

  $$ma{g}_{\text{noise}}=\text{MagnetometerNoise}\text{+}\text{MagneticDisturbanceNoise}\text{+}{\kappa }^2\left(\text{GyroscopeDriftNoise}\text{+}\text{GyroscopeNoise}\right)$$

  Las siguientes propiedades definen la variación del ruido del modelo de observación:

  • κ –– DecimationFactor/SampleRate

  • AccelerometerNoise

  • LinearAccelerationNoise

  • GyroscopeDriftNoise

  • GyroscopeNoise

  • MagneticDisturbanceNoise

  • MagnetometerNoise

Actualizar covarianza de estimación de error

La covarianza de la estimación del error es una matriz de 12 por 12 que se utiliza para rastrear la variabilidad en el estado.

La matriz de covarianza de estimación de error se actualiza como:

$$P_{{}_{12\times 12}}^{+}=P_{{}_{12\times 12}}^{-}-\left(K_{12\times 6}\right)\left(H_{6\times 12}\right)\left(P_{{}_{12\times 12}}^{-}\right)$$

donde K es la ganancia de Kalman, H es la matriz de medición y P⁻ es la covarianza de estimación de error calculada durante la iteración anterior.

Predecir el error Estimar la covarianza

La covarianza de la estimación del error es una matriz de 12 por 12 que se utiliza para rastrear la variabilidad en el estado. La covarianza de estimación de error a priori, P⁻, se establece en la covarianza de ruido del proceso, Q, determinada durante la iteración anterior. Q se calcula como una función de la covarianza de estimación de error a posteriori, P⁺. Al calcular Q, se supone que los términos de correlación cruzada son insignificantes en comparación con los términos de autocorrelación y se establecen en cero:

$$Q=\left[\begin{array}{cccccccccccc}{P}^{+}(1)+{\kappa }^2\left(P^{+}(40)+\beta +\eta \right)& 0& 0& -\kappa \left(P^{+}(40)+\beta \right)& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& P^{+}(14)+{\kappa }^2\left(P^{+}(53)+\beta +\eta \right)& 0& 0& -\kappa \left(P^{+}(53)+\beta \right)& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& P^{+}(27)+{\kappa }^2\left(P^{+}(66)+\beta +\eta \right)& 0& 0& -\kappa \left(P^{+}(66)+\beta \right)& 0& 0& 0& 0& 0& 0\\ -\kappa \left(P^{+}(40)+\beta \right)& 0& 0& P^{+}(40)+\beta & 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -\kappa \left(P^{+}(53)+\beta \right)& 0& 0& P^{+}(53)+\beta & 0& 0& 0& 0& 0& 0& 0\\ 0& 0& -\kappa \left(P^{+}(66)+\beta \right)& 0& 0& P^{+}(66)+\beta & 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& {\nu }^2{P}^{+}(79)+\xi & 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& {\nu }^2{P}^{+}(92)+\xi & 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& {\nu }^2{P}^{+}(105)+\xi & 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& {\sigma }^2{P}^{+}(118)+\gamma & 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& {\sigma }^2{P}^{+}(131)+\gamma & 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& {\sigma }^2{P}^{+}(144)+\gamma \end{array}\right]$$

dónde

• P⁺ –– es la covarianza de estimación de error actualizada (a posteriori)

• κ –– DecimationFactor/SampleRate

• β –– GyroscopeDriftNoise

• η –– GyroscopeNoise

• ν –– LinearAccelerationDecayFactor

• ξ –– LinearAccelerationNoise

• σ –– MagneticDisturbanceDecayFactor

• γ –– MagneticDisturbanceNoise

Consulte la sección 10.1 de [1] para obtener una derivación de los términos de la matriz de error del proceso.

Ganancia de Kalman

La matriz de ganancia de Kalman es una matriz de 12 por 6 que se utiliza para ponderar la innovación. En este algoritmo, la innovación se interpreta como el proceso de error, z.

La matriz de ganancia de Kalman se construye como:

$$K_{12\times 6}=\left(P_{{}_{12\times 12}}^{-}\right){\left(H_{6\times 12}\right)}^T{\left({\left(S_{6\times 6}\right)}^T\right)}^{-1}$$

donde

• P⁻ –– covarianza del error previsto

• H –– modelo de observación

• S –– covarianza de la innovación

Actualizar un error posterior

La estimación del error a posterior se determina combinando la matriz de ganancia de Kalman con el error en las estimaciones del vector de gravedad y del vector magnético:

$$x_{12\times 1}=\left(K_{12\times 6}\right){(z_{1\times 6})}^T$$

Si se detecta interferencia magnética en la iteración actual, se ignora la señal de error del vector magnético y la estimación del error a posterior se calcula como:

$$x_{9\times 1}=\left(K(\text{1:9},\text{1:3}\right){(z_g)}^T$$

Correcto

Orientación de estimación

La estimación de orientación se actualiza multiplicando la estimación anterior por el error:

$$q^{+}=\left(q^{-}\right)\left({\theta }^{+}\right)$$

Estimar la aceleración lineal

La estimación de la aceleración lineal se actualiza decayendo la estimación de la aceleración lineal de la iteración anterior y restando el error:

$$linAccelPrior=(linAccelPrio{r}_{k-1})\nu -b^{+}$$

donde

• ν –– LinearAccelerationDecayFactor

Estimar el desplazamiento del giroscopio

La estimación del desplazamiento del giroscopio se actualiza restando el error de desplazamiento del giroscopio del desplazamiento del giroscopio de la iteración anterior:

$$gyroOffset=gyroOffse{t}_{k-1}-a^{+}$$

Calcular la velocidad angular

Para estimar la velocidad angular, se promedia el marco de gyroReadings y se resta el desplazamiento del giroscopio calculado en la iteración anterior:

$$angularVelocit{y}_{1\times 3}=\frac{{\displaystyle \sum gyroReading{s}_{N\times 3}}}{N}-gyroOffse{t}_{1\times 3}$$

donde N es el factor de diezmado especificado por la propiedad DecimationFactor.

La estimación de compensación del giroscopio se inicializa en ceros para la primera iteración.

Actualizar vector magnético

Si no se detectó interferencia magnética en la iteración actual, la estimación del vector magnético, m, se actualiza utilizando el error de perturbación magnética a posteriori y la orientación a posteriori.

El error de perturbación magnética se convierte al marco de navegación:

$$mErrorNE{D}_{1\times 3}={\left({\left(rPos{t}_{3\times 3}\right)}^T{(mErro{r}_{1\times 3})}^T\right)}^T$$

El error de perturbación magnética en el marco de navegación se resta de la estimación del vector magnético anterior y luego se interpreta como inclinación:

$${\rm M}=m-mErrorNED$$

$$inclination=\text{atan2}({\rm M}(3),{\rm M}(1))$$

La inclinación se convierte en una estimación de vector magnético restringido para la siguiente iteración:

$$\begin{array}{l}m(1)=\left(\text{ExpectedMagneticFieldStrength}\right)\left(\cos (inclination)\right)\\ m(2)=0\\ m(3)=\left(\text{ExpectedMagneticFieldStrength}\right)\left(\sin (inclination)\right)\end{array}$$

ExpectedMagneticFieldStrength es una propiedad de ahrsfilter.

Referencias

[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., H.J. Luinge, C.T.M. Baten, and P.H. Veltink. "Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation." IEEE Transactions on Neural Systems and Rehabilitation Engineering. Vol. 13. Issue 3, 2005, pp. 395-405.

Capacidades ampliadas

Generación de código C/C++

Genere código C y C++ mediante MATLAB® Coder™.

Indicaciones y limitaciones de uso:

• Consulte System Objects in MATLAB Code Generation (MATLAB Coder).

• Este System object también admite la generación estricta de código de precisión simple.

Historial de versiones

Introducido en R2018b

R2024a: El objeto ahrsfilter devuelve el residuo y la covarianza residual

Objeto ahrsfilter, FUSE devuelve un tercer argumento de salida, interData, que contiene residuos y covarianza residual almacenados como una estructura.

Consulte también

ecompass | gpsSensor | imufilter | imuSensor