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

insfilterNonholonomic

Estimar la pose con restricciones no holonómicas.

Descripción

El objeto insfilterNonholonomic implementa la fusión de sensores de la unidad de medición inercial (IMU) y datos GPS para estimar la postura en el marco de referencia NED (o ENU). Los datos de IMU se derivan de datos de giroscopio y acelerómetro. El filtro utiliza un vector de estado de 16 elementos para rastrear los sesgos del cuaternión de orientación, la velocidad, la posición y el sensor IMU. El objeto insfilterNonholonomic utiliza un filtro Kalman extendido para estimar estas cantidades.

Creación

Sintaxis

filter = insfilterNonholonomic

filter = insfilterNonholonomic('ReferenceFrame',RF)

filter = insfilterNonholonomic(___,Name=Value)

Descripción

filter = insfilterNonholonomic crea un objeto insfilterErrorState con valores de propiedad predeterminados.

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

filter = insfilterNonholonomic(___,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

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

DecimationFactor — Factor de diezmado para la corrección de restricciones cinemáticas

2 (predeterminado) | escalar entero positivo

Factor de diezmado para la corrección de restricciones cinemáticas, especificado como un escalar entero positivo.

Tipos de datos: single | double

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

[4.8e-6 4.8e-6 4.8e-6] (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)²

[4e-14 4e-14 4e-14] (predeterminado) | escalar | 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 finitos reales positivos. El sesgo del giroscopio se modela como un proceso de ruido blanco filtrado de paso bajo.

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

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

Tipos de datos: single | double

GyroscopeBiasDecayFactor — Factor de caída para el sesgo del giroscopio

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

Factor de caída para el sesgo del giroscopio, especificado como un escalar en el rango [0,1]. Un factor de decaimiento de 0 modela el sesgo del giroscopio como un proceso de ruido blanco. Un factor de decaimiento de 1 modela el sesgo del giroscopio como un proceso de caminata aleatoria.

Tipos de datos: single | double

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

[4.8e-2 4.8e-2 4.8e-2] (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²)²

[4e-14 4e-14 4e-14] (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. El sesgo del acelerómetro se modela como un proceso de ruido blanco filtrado de paso bajo.

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

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

AccelerometerBiasDecayFactor — Factor de caída para el sesgo del acelerómetro

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

Factor de caída para el sesgo del acelerómetro, especificado como un escalar en el rango [0,1]. Un factor de decaimiento de 0 modela el sesgo del acelerómetro como un proceso de ruido blanco. Un factor de decaimiento de 1 modela el sesgo del acelerómetro como un proceso de caminata aleatoria.

Tipos de datos: single | double

State — Vector de estado del filtro de Kalman extendido

[1;zeros(15,1)] | vector columna de 16 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
Polarización del giroscopio (XYZ)	rad/s	5:7
Posición (NED o ENU)	metro	8:10
Velocidad (NED o ENU)	EM	11:13
Polarización del acelerómetro (XYZ)	m/s2	14:16

Tipos de datos: single | double

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

eye(16) (predeterminado) | matriz de 16 por 16

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

Tipos de datos: single | double

ZeroVelocityConstraintNoise — Restricciones de velocidad de ruido (m/s)²

1e-2 (predeterminado) | escalar no negativo

Restricciones de velocidad de ruido en (m/s)², especificado como un escalar no negativo.

Tipos de datos: single | double

Funciones del objeto

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

Ejemplos

Estimar la pose del vehículo terrestre

Este ejemplo muestra cómo estimar la pose de un vehículo terrestre a partir de las mediciones registradas de los sensores IMU y GPS y la orientación y posición ground-truth.

Cargue los datos registrados de un vehículo terrestre siguiendo una trayectoria circular.

load('loggedGroundVehicleCircle.mat','imuFs','localOrigin','initialState','initialStateCovariance','accelData',...
      'gyroData','gpsFs','gpsLLA','Rpos','gpsVel','Rvel','trueOrient','truePos');

Inicializar el objeto insfilterNonholonomic.

filt = insfilterNonholonomic;
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.State = initialState;
filt.StateCovariance = initialStateCovariance;
    
imuSamplesPerGPS = imuFs/gpsFs;

Registre datos para el cálculo de métricas finales. Utilice la función de objeto predict para estimar el estado del filtro según los datos del acelerómetro y del giroscopio. Luego corrija el estado del filtro según los datos del GPS.

numIMUSamples = size(accelData,1);
estOrient = quaternion.ones(numIMUSamples,1);
estPos = zeros(numIMUSamples,3);
    
gpsIdx = 1;

for idx = 1:numIMUSamples
    predict(filt,accelData(idx,:),gyroData(idx,:));       %Predict filter state
    
    if (mod(idx,imuSamplesPerGPS) == 0)                   %Correct filter state
        fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    [estPos(idx,:),estOrient(idx,:)] = pose(filt);        %Log estimated pose
end

Calcule y muestre errores RMS.

posd = estPos - truePos;
quatd = rad2deg(dist(estOrient,trueOrient));
msep = sqrt(mean(posd.^2));

fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',msep(1),msep(2),msep(3));   

Position RMS Error
	X: 0.15, Y: 0.11, Z: 0.01 (meters)

    
fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',sqrt(mean(quatd.^2)));

Quaternion Distance RMS Error
	0.26 (degrees)

Algoritmos

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

insfilterNonholonomic utiliza una estructura de filtro Kalman de estado de error de 16 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\\ gyrobia{s}_X\\ gyrobia{s}_Y\\ gyrobia{s}_Z\\ positio{n}_N\\ positio{n}_E\\ positio{n}_D\\ v_N\\ v_E\\ v_D\\ accelbia{s}_X\\ accelbia{s}_Y\\ accelbia{s}_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.

• gyrobias_X, gyrobias_Y, gyrobias_Z –– Sesgo en la lectura del giroscopio.

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

• accelbias_X, accelbias_Y, accelbias_Z –– Sesgo en la lectura del acelerómetro.

Dada la formulación convencional de la función de transición de estado,

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

la estimación del estado previsto es:

$$x_{k|k-1}=\left[\begin{array}{c}{q}_0+\Delta t\ast q_1(gyrobia{s}_X/2-gyr{o}_X/2)+\Delta t\ast q_2\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)+\Delta t\ast q_3\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ q_1-\Delta t\ast q_0(gyrobia{s}_X/2-gyr{o}_X/2)+\Delta t\ast q_3\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)-\Delta t\ast q_2\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ q_2-\Delta t\ast q_3(gyrobia{s}_X/2-gyr{o}_X/2)-\Delta t\ast q_0\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)+\Delta t\ast q_1\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ q_3+\Delta t\ast q_2(gyrobia{s}_X/2-gyr{o}_X/2)-\Delta t\ast q_1\ast (gyrobia{s}_Y/2-gyr{o}_Y/2)-\Delta t\ast q_0\ast (gyrobia{s}_Z/2-gyr{o}_Z/2)\\ -gryobia{s}_X\ast (\Delta t\ast {\lambda }_{gyro}-1)\\ -gryobia{s}_Y\ast (\Delta t\ast {\lambda }_{gyro}-1)\\ -gryobia{s}_Z\ast (\Delta t\ast {\lambda }_{gyro}-1)\\ positio{n}_N+\Delta t\ast v_N\\ positio{n}_E+\Delta t\ast v_E\\ positio{n}_D+\Delta t\ast v_D\\ v_N+\Delta t\ast \left\{\begin{array}{l}{q}_0\ast \left(q_0\ast \left(accelbia{s}_X-acce{l}_X\right)-q_3\ast \left(accelbia{s}_Y-acce{l}_Y\right)+q_2\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)-g_N+\\ q_2\ast \left(q_1\ast \left(accelbia{s}_Y-acce{l}_Y\right)-q_2\ast \left(accelbia{s}_X-acce{l}_X\right)+q_0\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)+\\ q_1\ast \left(q_1\ast \left(accelbia{s}_X-acce{l}_X\right)+q_2\ast \left(accelbia{s}_Y-acce{l}_Y\right)+q_3\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)-\\ q_3\ast \left(q_3\ast \left(accelbia{s}_X-acce{l}_X\right)+q_0\ast \left(accelbia{s}_Y-acce{l}_Y\right)-q_1\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)\end{array}\right\}\\ v_E+\Delta t\ast \left\{\begin{array}{l}{q}_0\ast \left(q_3\ast \left(accelbia{s}_X-acce{l}_X\right)+q_0\ast \left(accelbia{s}_Y-acce{l}_Y\right)-q_1\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)-g_E-\\ q_1\ast \left(q_1\ast \left(accelbia{s}_Y-acce{l}_Y\right)-q_2\ast \left(accelbia{s}_X-acce{l}_X\right)+q_0\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)+\\ q_2\ast \left(q_1\ast \left(accelbia{s}_X-acce{l}_X\right)+q_2\ast \left(accelbia{s}_Y-acce{l}_Y\right)+q_3\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)+\\ q_3\ast \left(q_0\ast \left(accelbia{s}_X-acce{l}_X\right)-q_3\ast \left(accelbia{s}_Y-acce{l}_Y\right)+q_2\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)\end{array}\right\}\\ v_D+\Delta t\ast \left\{\begin{array}{l}{q}_0\ast \left(q_1\ast \left(accelbia{s}_Y-acce{l}_Y\right)-q_2\ast \left(accelbia{s}_X-acce{l}_X\right)+q_0\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)-g_D+\\ q_1\ast \left(q_3\ast \left(accelbia{s}_X-acce{l}_X\right)+q_0\ast \left(accelbia{s}_Y-acce{l}_Y\right)-q_1\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)-\\ q_2\ast \left(q_0\ast \left(accelbia{s}_X-acce{l}_X\right)-q_3\ast \left(accelbia{s}_Y-acce{l}_Y\right)+q_2\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)+\\ q_3\ast \left(q_1\ast \left(accelbia{s}_X-acce{l}_X\right)+q_2\ast \left(accelbia{s}_Y-acce{l}_Y\right)+q_3\ast \left(accelbia{s}_Z-acce{l}_Z\right)\right)\end{array}\right\}\\ -accelbia{s}_X\ast (\Delta t\ast {\lambda }_{accel}-1)\\ -accelbia{s}_Y\ast (\Delta t\ast {\lambda }_{accel}-1)\\ -accelbia{s}_Z\ast (\Delta t\ast {\lambda }_{accel}-1)\end{array}\right]$$

dónde

• Δ t –– Tiempo de muestreo de IMU.

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

• accel_X, accel_Y, accel_Z –– Vector de aceleración en el bastidor de la carrocería.

• gyro_X, gyro_Y, gyro_Z –– Componentes de velocidad angular en el bastidor de la carrocería.

• λ_accel –– Factor de disminución de la polarización del acelerómetro.

• λ_gyro –– Factor de decaimiento de sesgo del giroscopio.

Referencias

[1] Munguía, R. "A GPS-Aided Inertial Navigation System in Direct Configuration." Journal of applied research and technology. Vol. 12, Number 4, 2014, pp. 803 – 814.

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

insfilterMARG | insfilterErrorState | insfilterAsync