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

insfilterErrorState

Estimar la pose a partir de datos de IMU, GPS y odometría visual monocular (MVO)

Descripción

El objeto insfilterErrorState implementa la fusión de sensores de IMU, GPS y datos de odometría visual monocular (MVO) para estimar la pose en el marco de referencia NED (o ENU). El filtro utiliza un vector de estado de 17 elementos para rastrear la orientación quaternion, la velocidad, la posición, los sesgos del sensor IMU y el factor de escala MVO. El objeto insfilterErrorState utiliza un filtro Kalman de estado de error para estimar estas cantidades.

Creación

Sintaxis

filter = insfilterErrorState

filter = insfilterErrorState('ReferenceFrame',RF)

filter = insfilterErrorState(___,Name=Value)

Descripción

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

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

filter = insfilterErrorState(___,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 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-6 1e-6 1e-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 escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

GyroscopeBiasNoise — Variación del ruido del proceso aditivo a partir del sesgo del giroscopio ((rad/s)²)

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

Variación del ruido del proceso aditivo 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.

• 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 cada eje

Tipos de datos: single | double

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

[1e-4 1e-4 1e-4] (predeterminado) | escalar | vector fila de 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 — Variación del ruido del proceso aditivo debido al sesgo del acelerómetro ((m/s²)²)

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

Variación del ruido del proceso aditivo 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 acelerómetro, respectivamente.

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

State — Vector de estado del filtro de Kalman

[1;zeros(15,1);1] (predeterminado) | vector columna de 17 elementos

Vector de estado del filtro de Kalman extendido, especificado como un vector columna de 17 elementos. 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
Polarización del giroscopio (XYZ)	rad/s	11:13
Polarización del acelerómetro (XYZ)	m/s2	14:16
Escala de odometría visual (XYZ)	N / A	17

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

Tipos de datos: single | double

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

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

Covarianza de error de estado para el filtro de Kalman, especificada como una matriz de números reales de 16 por 16 elementos. Los valores de covarianza del error de estado representan:

Covarianza estatal	Índice de fila/columna
Vector de rotación δ (XYZ)	1:3
Posición δ (NED o ENU)	4:6
δ Velocidad (NED o ENU)	7:9
δ Sesgo del giroscopio (XYZ)	10:12
δ Polarización del acelerómetro (XYZ)	13:15
Escala de odometría visual δ (XYZ)	16

Tenga en cuenta que, dado que se trata de un filtro de Kalman de estado de error, realiza un seguimiento de los errores en los estados. δ representa el error en el estado correspondiente.

Tipos de datos: single | double

Funciones del objeto

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

Ejemplos

Estimar la pose del vehículo terrestre

Cargue datos registrados de un vehículo terrestre siguiendo una trayectoria circular. El archivo .mat contiene mediciones de sensores IMU y GPS y la orientación y posición de la ground-truth .

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

Cree un filtro INS para fusionar datos IMU y GPS utilizando un filtro Kalman de estado de error.

initialState = [compact(trueOrient(1)),truePos(1,:),-6.8e-3,2.5002,0,zeros(1,6),1].';
filt = insfilterErrorState;
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.State = initialState;
filt.StateCovariance = initialStateCovariance;

Preasigne variables para posición y orientación. Asigne una variable para indexar los datos del GPS.

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

gpsIdx = 1;

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

for idx = 1:numIMUSamples

    % Use predict to estimate the filter state based on the accelData and
    % gyroData arrays.
    predict(filt,accelData(idx,:),gyroData(idx,:));
    
    % GPS data is collected at a lower sample rate than IMU data. Fuse GPS
    % data at the lower rate.
    if mod(idx, imuFs / gpsFs) == 0
        % Correct the filter states based on the GPS data.
        fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [estPos(idx,:), estOrient(idx,:)] = pose(filt);
end

Calcule los errores RMS entre la posición y orientación verdaderas conocidas y la salida del filtro de estado de error.

pErr = truePos - estPos;
qErr = rad2deg(dist(estOrient,trueOrient));

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

fprintf('Position RMS Error\n');

Position RMS Error

fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));

	X: 0.40, Y: 0.24, Z: 0.05 (meters)

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

Quaternion Distance RMS Error

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

	0.30 (degrees)

Visualice la posición real y la posición estimada.

plot(truePos(:,1),truePos(:,2),estPos(:,1),estPos(:,2),'r:','LineWidth',2)
grid on
axis square
xlabel('N (m)')
ylabel('E (m)')
legend('Ground Truth','Estimation')

Algoritmos

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

insfilterErrorState utiliza una estructura de filtro Kalman de estado de error de 17 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}_N\\ positio{n}_E\\ positio{n}_D\\ v_N\\ v_E\\ v_D\\ gyrobia{s}_X\\ gyrobia{s}_Y\\ gyrobia{s}_Z\\ accelbia{s}_X\\ accelbia{s}_Y\\ \begin{array}{l}accelbia{s}_Z\\ \begin{array}{c}scaleFactor\end{array}\end{array}\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.

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

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

• scaleFactor –– Factor de escala de la estimación de la pose.

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)\\ 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\}\\ gyrobia{s}_X\\ gyrobia{s}_Y\\ gyrobia{s}_Z\\ accelbia{s}_X\\ accelbia{s}_Y\\ accelbia{s}_Z\\ scaleFactor\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.

Capacidades ampliadas

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

Historial de versiones

Introducido en R2019a

Consulte también

insfilterAsync | insfilterMARG | insfilterNonholonomic