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

AHRS

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

• 

Bibliotecas:
Navigation Toolbox / Multisensor Positioning / Navigation Filters
Sensor Fusion and Tracking Toolbox / Multisensor Positioning / Navigation Filters

Descripción

El bloque AHRS Simulink^® fusiona datos del sensor acelerómetro, magnetómetro y giroscopio para estimar la orientación del dispositivo.

Ejemplos

Fusión de sensores IMU con Simulink

Este ejemplo muestra cómo generar y fusionar datos del sensor IMU utilizando Simulink®. Puede modelar con precisión el comportamiento de un acelerómetro, un giroscopio y un magnetómetro y fusionar sus salidas para calcular la orientación.

• Abrir script

Puertos

Entrada

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

Matriz N por 3 de escalares reales

Lecturas del acelerómetro en el sistema de coordenadas del cuerpo del sensor en m/s², especificadas como una matriz N por 3 de escalares reales. N es el número de muestras y las tres columnas de Accel representan las [x y z] mediciones, respectivamente.

Tipos de datos: single | double

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

Matriz N por 3 de escalares reales

Lecturas del giroscopio en el sistema de coordenadas del cuerpo del sensor en rad/s, especificadas como una matriz N por 3 de escalares reales. N es el número de muestras y las tres columnas de Gyro representan las [x y z] mediciones, respectivamente.

Tipos de datos: single | double

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

Matriz N por 3 de escalares reales

Lecturas del magnetómetro en el sistema de coordenadas del cuerpo del sensor en µT, especificadas como una matriz N por 3 de escalares reales. N es el número de muestras y las tres columnas de magReadings representan las [x y z] mediciones, respectivamente.

Tipos de datos: single | double

Salida

Orientation — Orientación del marco del cuerpo del sensor en relación con el marco de navegación

Matriz de escalares M por 4 | Matriz de rotación de 3 por 3 por M elementos

Orientación del marco del cuerpo del sensor con respecto al marco de navegación, devuelto como un arreglo de escalares de M por 4 o un arreglo de matrices de rotación de 3 por 3 por M. Se supone que cada fila de la matriz N por 4 son los cuatro elementos de un quaternion. El número de muestras de entrada, N, y el parámetro Decimation Factor determinan el tamaño de salida M.

Tipos de datos: single | double

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

arreglo de M-por-3 de escalares reales (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 de escalares reales. El número de muestras de entrada, N, y el parámetro Decimation Factor determinan el tamaño de salida M.

Tipos de datos: single | double

Parámetros

Principal

— Marco de referencia de navegación

NED (predeterminado) | ENU

Marco de referencia de navegación, especificado como NED (Noreste-Abajo) o ENU (Este-Norte-Arriba).

— Factor de diezmado

1 (predeterminado) | número entero positivo

Factor de diezmado mediante el cual se reduce la velocidad de datos del sensor de entrada, especificado como un número entero positivo.

El número de filas de las entradas –– Accel, Gyro y Mag –– debe ser un múltiplo del factor de diezmado.

Tipos de datos: single | double

— Ruido del proceso inicial

ahrsfilter.defaultProcessNoise (predeterminado) | Matriz de 12 por 12 de escalar real

Ruido del proceso inicial, especificado como una matriz de escalares reales de 12 por 12. El valor predeterminado, ahrsfilter.defaultProcessNoise, es una matriz diagonal de 12 por 12 como:

  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

Tipos de datos: single | double

— Formato de salida de orientación

'quaternion' (predeterminado) | 'Rotation matrix'

Formato de orientación de salida, especificado como 'quaternion' o 'Rotation matrix':

• 'quaternion' –– La salida es un arreglo M por 4 de escalares reales. Cada fila del arreglo representa los cuatro componentes de un quaternion.

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

El tamaño de salida M depende de la dimensión de entrada N y del parámetro Decimation Factor.

Tipos de datos: char | string

Simular con — Tipo de simulación a ejecutar

Interpreted Execution (predeterminado) | Code Generation

• Interpreted execution — Simular el modelo utilizando el intérprete MATLAB^®. Esta opción acorta el tiempo de inicio. En el modo Interpreted execution, puede depurar el código fuente del bloque.

• Code generation — Simular el modelo utilizando el código C generado. La primera vez que ejecuta una simulación, Simulink genera código C para el bloque. El código C se reutiliza para simulaciones posteriores siempre que el modelo no cambie. Esta opción requiere tiempo de inicio adicional.

Ruido de medición

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

0.0001924722 (predeterminado) | escalar real positivo

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

Tipos de datos: single | double

Ruido del giroscopio ((rad/s)²) — 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.

Tipos de datos: single | double

Ruido del magnetómetro (μT²) — 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.

Tipos de datos: single | double

— 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.

Tipos de datos: single | double

Ruido ambiental

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

0.0096236100000000012 (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.

Tipos de datos: single | double

Ruido de perturbación magnética (μT²) — 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.

Tipos de datos: single | double

— 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, establezca este parámetro en un valor más bajo. Si la aceleración lineal cambia lentamente, establezca este parámetro 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.

Tipos de datos: single | double

— 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.

Tipos de datos: single | double

— Intensidad del campo magnético (μT)

50 (predeterminado) | escalar positivo real

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

Tipos de datos: single | double

Algoritmos

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

El bloque AHRS 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 el factor de diezmado y fs es la frecuencia de muestreo.

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$$

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 de frecuencia de muestreo y factor de diezmado: κ = Decimation factor/Sample rate.

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)$$

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)

• κ –– Factor de diezmado dividido por la frecuencia de muestreo.

• β –– Ruido de deriva del giroscopio.

• η –– Ruido del giroscopio.

• ν –– Factor de disminución de la aceleración lineal.

• ξ –– Ruido de aceleración lineal.

• σ –– Factor de decaimiento por perturbación magnética.

• γ –– Ruido de perturbación magnética.

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

• ν –– Factor de decaimiento de la aceleración lineal

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}$$

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 Simulink® Coder™.

Indicaciones y limitaciones de uso:

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

Historial de versiones

Introducido en R2020a

Consulte también

ahrsfilter | ecompass | imufilter | imuSensor | gpsSensor