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

insfilterMARG

Estimar la pose a partir de datos MARG y GPS

Descripción

El objeto insfilterMARG implementa la fusión de sensores de datos MARG y GPS para estimar la postura en el marco de referencia NED (o ENU). 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 22 elementos para rastrear el cuaternión de orientación, la velocidad, la posición, los sesgos del sensor MARG y el vector geomagnético. El objeto insfilterMARG utiliza un filtro Kalman extendido para estimar estas cantidades.

Creación

Sintaxis

filter = insfilterMARG

filter = insfilterMARG('ReferenceFrame',RF)

filter = insfilterMARG(___,Name=Value)

Descripción

filter = insfilterMARG crea un objeto insfilterMARG con valores de propiedad predeterminados.

filter = insfilterMARG('ReferenceFrame',RF) le permite especificar el marco de referencia, RF, del filter.

filter = insfilterMARG(___,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 unidad de medida inercial (IMU) en Hz, especificada como escalar positivo.

Tipos de datos: single | double

ReferenceLocation — Ubicación de referencia (grados, grados, metros)

[0 0 0] (predeterminado) | vector fila positiva de 3 elementos

Ubicación de referencia, especificada como un vector fila de 3 elementos en coordenadas geodésicas (latitud, longitud y altitud). La altitud es la altura sobre el modelo de elipsoide de referencia, WGS84. Las unidades de ubicación de referencia son [grados grados metros].

Tipos de datos: single | double

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

1e-9 (predeterminado) | escalar | vector fila de 3 elementos

Varianza del ruido del proceso multiplicativo del giroscopio en (rad/s)², especificada como un vector fila escalar o de 3 elementos de números finitos reales positivos.

• Si GyroscopeNoise se especifica como un vector fila, los elementos corresponden al ruido en los ejes x, y y z del giroscopio, respectivamente.

• Si GyroscopeNoise se especifica como un escalar, el elemento único se aplica a los ejes x, y y z del giroscopio.

Tipos de datos: single | double

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

1e-10 (predeterminado) | escalar positivo | vector fila de 3 elementos

Variación del ruido del proceso multiplicativo a partir del sesgo del giroscopio en (rad/s)², especificado como un vector fila escalar o de 3 elementos de números reales positivos.

• Si GyroscopeBiasNoise se especifica como un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sesgo del giroscopio, respectivamente.

• Si GyroscopeBiasNoise se especifica como escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

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

1e-4 (predeterminado) | escalar | vector fila de 3 elementos

Varianza del ruido del proceso multiplicativo del acelerómetro en (m/s²)², especificada como un vector fila escalar o de 3 elementos de números finitos reales positivos.

• Si AccelerometerNoise se especifica como un vector fila, los elementos corresponden al ruido en los ejes x, y y z del acelerómetro, respectivamente.

• Si AccelerometerNoise se especifica como escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

AccelerometerBiasNoise — Varianza del ruido del proceso multiplicativo a partir del sesgo del acelerómetro (m/s²)²

1e-4 (predeterminado) | escalar positivo | vector fila de 3 elementos

Variación del ruido del proceso multiplicativo a partir del sesgo del acelerómetro en (m/s²)², especificada como un vector fila escalar o de 3 elementos de números reales positivos.

• Si AccelerometerBiasNoise se especifica como un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sesgo del acelerómetro, respectivamente.

• Si AccelerometerBiasNoise se especifica como escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

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

1e-6 (predeterminado) | escalar positivo | vector fila de 3 elementos

Ruido de proceso aditivo para el vector geomagnético en µT², especificado como un vector fila escalar o de 3 elementos de números reales positivos.

• Si GeomagneticVectorNoise se especifica como un vector fila, los elementos corresponden al ruido en los ejes x, y y z del vector geomagnético, respectivamente.

• Si GeomagneticVectorNoise se especifica como escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

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

0.1 (predeterminado) | escalar positivo | vector fila de 3 elementos

Ruido de proceso aditivo para polarización del magnetómetro en µT², especificado como un escalar o un vector fila de 3 elementos.

• Si MagnetometerBiasNoise se especifica como un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sesgo del magnetómetro, respectivamente.

• Si MagnetometerBiasNoise se especifica como escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

State — Vector de estado del filtro de Kalman extendido

vector columna de 22 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
Posición (NED o ENU)	metro	5:7
Velocidad (NED o ENU)	EM	8:10
Sesgo del ángulo delta (XYZ)	rad	11:13
Sesgo de velocidad delta (XYZ)	EM	14:16
Vector de campo geomagnético (NED o ENU)	µT	17:19
Polarización del magnetómetro (XYZ)	µT	20:22

Tipos de datos: single | double

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

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

Covarianza de error de estado para el filtro de Kalman extendido, especificado como una matriz de 22 por 22 elementos o números reales.

Tipos de datos: single | double

Funciones del objeto

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

Ejemplos

Estimar la pose del VANT

Este ejemplo muestra cómo estimar la pose de un vehículo aéreo no tripulado (VANT) a partir de datos de sensores registrados y la pose ground-truth.

Cargue los datos del sensor registrados y la pose ground-truth de un VANT.

load uavshort.mat

Inicializar el objeto de filtro insfilterMARG.

f = insfilterMARG;
f.IMUSampleRate = imuFs;
f.ReferenceLocation = refloc;
f.AccelerometerBiasNoise = 2e-4;
f.AccelerometerNoise = 2;
f.GyroscopeBiasNoise = 1e-16;
f.GyroscopeNoise = 1e-5;
f.MagnetometerBiasNoise = 1e-10;
f.GeomagneticVectorNoise = 1e-12;
f.StateCovariance = 1e-9*ones(22);
f.State = initstate;
 
gpsidx = 1;
magFs = imuFs/2; % Hz
gpuFs = 1;       % Hz
N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

Fusiona datos de acelerómetro, giroscopio, magnetómetro y GPS.

for ii = 1:size(accel,1)               % Fuse IMU
   f.predict(accel(ii,:), gyro(ii,:));
        
   if ~mod(ii,fix(imuFs/magFs))            % Fuse magnetometer
       f.fusemag(mag(ii,:),Rmag);
   end
  
   if ~mod(ii,imuFs/gpuFs)                   % Fuse GPS once per second
       f.fusegps(lla(gpsidx,:),Rpos,gpsvel(gpsidx,:),Rvel);
       gpsidx = gpsidx + 1;
   end
 
   [p(ii,:),q(ii)] = pose(f);           % Log estimated pose
end

Calcule y muestre errores RMS.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));
pRMS = sqrt(mean(posErr.^2));
qRMS = sqrt(mean(qErr.^2));
fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));

Position RMS Error
	X: 0.57, Y: 0.53, Z: 0.69 (meters)

    
fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',qRMS);

Quaternion Distance RMS Error
	0.20 (degrees)

Algoritmos

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

insfilterMARG utiliza una estructura de filtro Kalman extendida de 22 ejes para estimar la pose en el marco de referencia NED. El estado se define como:

$$x=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\\ positio{n}_{\text{N}}\\ positio{n}_{\text{E}}\\ positio{n}_{\text{D}}\\ {\nu }_{\text{N}}\\ {\nu }_{\text{E}}\\ {\nu }_{\text{D}}\\ \Delta \theta bia{s}_{\text{X}}\\ \Delta \theta bia{s}_{\text{Y}}\\ \Delta \theta bia{s}_{\text{Z}}\\ \Delta \nu bia{s}_{\text{X}}\\ \Delta \nu bia{s}_{\text{Y}}\\ \Delta \nu bia{s}_{\text{Z}}\\ geomagneticFieldVecto{r}_{\text{N}}\\ geomagneticFieldVecto{r}_{\text{E}}\\ geomagneticFieldVecto{r}_{\text{D}}\\ magbia{s}_{\text{X}}\\ magbia{s}_{\text{Y}}\\ magbia{s}_{\text{Z}}\end{array}\right]$$

donde

• q₀, q₁, q₂, q₃ –– Partes del cuaternión de orientación. El cuaternión de orientación representa una rotación del marco desde la orientación actual de la plataforma hasta el sistema de coordenadas NED local.

• position_N, position_E, position_D –– Posición de la plataforma en el sistema de coordenadas NED local.

• ν_N, ν_E, ν_D –– Velocidad de la plataforma en el sistema de coordenadas NED local.

• Δ θbias_X, Δ θbias_Y, Δ θbias_Z –– Sesgo en la lectura del giroscopio integrado.

• Δ νbias_X, Δ νbias_Y, Δ νbias_Z –– Sesgo en la lectura del acelerómetro integrado.

• geomagneticFieldVector_N, geomagneticFieldVector_E, geomagneticFieldVector_D –– Estimación del vector del campo geomagnético en la ubicación de referencia.

• magbias_X, magbias_Y, magbias_Z –– Sesgo en las lecturas del magnetómetro.

Dada la formación convencional de la estimación del estado previsto,

$$x_{k|k-1}=f({\widehat{x}}_{k-1|k-1},u_k)$$

u_k está controlado por datos del acelerómetro y del giroscopio que se han convertido en velocidad delta y ángulo delta a través de integración trapezoidal. La estimación del estado previsto es:

$$x_{k|k-1}=\left[\begin{array}{c}{q}_0{q}_0^{\text{'}}-q_1{q}_1^{\text{'}}-q_2{q}_2^{\text{'}}-q_3{q}_3^{\text{'}}\\ q_1{q}_0^{\text{'}}+q_0{q}_1^{\text{'}}-q_3{q}_2^{\text{'}}+q_2{q}_3^{\text{'}}\\ q_2{q}_0^{\text{'}}+q_3{q}_1^{\text{'}}+q_0{q}_2^{\text{'}}-q_1{q}_3^{\text{'}}\\ q_3{q}_0^{\text{'}}-q_2{q}_1^{\text{'}}+q_1{q}_2^{\text{'}}+q_0{q}_3^{\text{'}}\\ positio{n}_{\text{N}}+\left(\Delta t\right)\left({\nu }_{\text{N}}\right)\\ positio{n}_{\text{E}}+\left(\Delta t\right)\left({\nu }_{\text{E}}\right)\\ positio{n}_{\text{D}}+\left(\Delta t\right)\left({\nu }_{\text{D}}\right)\\ {\nu }_{\text{N}}+\left(\Delta t\right)\left(g_{\text{N}}\right)+\left(\Delta {\nu }_{\text{X}}-\Delta \nu bia{s}_{\text{X}}\right)\left(q_0^2+q_{{}_1}^2-q_2^2-q_3^2\right)-2\left(\Delta {\nu }_{\text{Y}}-\Delta \nu bia{s}_{\text{Y}}\right)\left(q_0{q}_3-q_1{q}_2\right)+2\left(\Delta {\nu }_{\text{Z}}-\Delta \nu bia{s}_{\text{Z}}\right)\left(q_0{q}_2+q_1{q}_3\right)\\ {\nu }_{\text{E}}+\left(\Delta t\right)\left(g_{\text{E}}\right)+\left(\Delta {\nu }_{\text{Y}}-\Delta \nu bia{s}_{\text{Y}}\right)\left(q_0^2-q_{{}_1}^2+q_2^2-q_3^2\right)+2\left(\Delta {\nu }_{\text{X}}-\Delta \nu bia{s}_{\text{X}}\right)\left(q_0{q}_3+q_1{q}_2\right)-2\left(\Delta {\nu }_{\text{Z}}-\Delta \nu bia{s}_{\text{Z}}\right)\left(q_0{q}_1-q_2{q}_3\right)\\ {\nu }_{\text{D}}+\left(\Delta t\right)\left(g_{\text{D}}\right)+\left(\Delta {\nu }_{\text{Z}}-\Delta \nu bia{s}_{\text{Z}}\right)\left(q_0^2-q_{{}_1}^2-q_2^2+q_3^2\right)-2\left(\Delta {\nu }_{\text{X}}-\Delta \nu bia{s}_{\text{X}}\right)\left(q_0{q}_2-q_1{q}_3\right)+2\left(\Delta {\nu }_{\text{Y}}-\Delta \nu bia{s}_{\text{Y}}\right)\left(q_0{q}_1+q_2{q}_3\right)\\ \Delta \theta bia{s}_{\text{X}}\\ \Delta \theta bia{s}_{\text{Y}}\\ \Delta \theta bia{s}_{\text{Z}}\\ \Delta \nu bia{s}_{\text{X}}\\ \Delta \nu bia{s}_{\text{Y}}\\ \Delta \nu bia{s}_{\text{Z}}\\ geomagneticFieldVecto{r}_{\text{N}}\\ geomagneticFieldVecto{r}_{\text{E}}\\ geomagneticFieldVecto{r}_{\text{D}}\\ magbia{s}_{\text{X}}\\ magbia{s}_{\text{Y}}\\ magbia{s}_{\text{Z}}\end{array}\right]$$

En la ecuación, (q₀^', q₁^', q₂^', q₃^') es el cuaternión que explica el cambio de orientación de un paso al siguiente. Suponiendo que el cambio de orientación es pequeño, entonces el vector de rotación se puede aproximar como (Δ θ_X − Δ θbias_X, Δ θ_Y − Δ θbias_Y, Δ θ_Z − Δ θbias_Z), donde Δ θ_X, Δ θ_Y, Δ θ_Z son las lecturas del giroscopio integrado. Luego se obtiene (q₀^', q₁^', q₂^', q₃^') convirtiendo el vector de rotación aproximado en un cuaternión. En cada cálculo, el cuaternión se normaliza de modo que su longitud sea 1 y su parte real q₀ no sea negativa.

Además,

• Δ ν_X, Δ ν_Y, Δ ν_Z –– Lecturas del acelerómetro integrado.

• Δ t –– Tiempo de muestreo de IMU.

• g_N, g_E, g_D –– Vector de gravedad constante en el marco NED.

Capacidades ampliadas

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

Historial de versiones

Introducido en R2018b

Consulte también

insfilterNonholonomic | insfilterErrorState | insfilterAsync