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

insfilterAsync

Estimar la pose a partir de datos asíncronos de MARG y GPS

Descripción

El objeto insfilterAsync 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 datos de magnetómetro, giroscopio y acelerómetro, respectivamente. El filtro utiliza un vector de estado de 28 elementos para rastrear la orientación quaternion, la velocidad, la posición, los sesgos del sensor MARG y el vector geomagnético. El objeto insfilterAsync utiliza un filtro de Kalman extendido continuo-discreto para estimar estas cantidades.

Creación

Sintaxis

filter = insfilterAsync

filter = insfilterAsync('ReferenceFrame',RF)

filter = insfilterAsync(___,Name=Value)

Descripción

filter = insfilterAsync crea un objeto insfilterAsync para fusionar datos MARG y GPS asincrónicos con valores de propiedad predeterminados.

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

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

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

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

Ubicación de referencia, especificada como un vector fila de tres 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

QuaternionNoise — Variación del ruido del proceso de cuaterniones aditivos

[1e-6 1e-6 1e-6 1e-6] (predeterminado) | escalar | vector fila de cuatro elementos

Variación aditiva del ruido del proceso de cuaterniones, especificada como un vector escalar o de cuatro elementos de partes de cuaterniones.

Tipos de datos: single | double

AngularVelocityNoise — Ruido aditivo del proceso de velocidad angular en el sistema de coordenadas de navegación local ((rad/s)²)

[0.005 0.005 0.005] (predeterminado) | escalar | vector fila de tres elementos

Ruido de proceso de velocidad angular aditivo en el sistema de coordenadas de navegación local en (rad/s)², especificado como un vector fila escalar o de tres elementos de números finitos reales positivos.

• Si AngularVelocityNoise es un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sistema de coordenadas de navegación local, respectivamente.

• Si AngularVelocityNoise es un escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

PositionNoise — Variación del ruido del proceso de posición aditivo en el sistema de coordenadas de navegación local (m²)

[1e-6 1e-6 1e-6] (predeterminado) | escalar | vector fila de tres elementos

Ruido del proceso de posición aditivo en el sistema de coordenadas de navegación local en m², especificado como un vector fila escalar o de tres elementos de números finitos reales positivos.

• Si PositionNoise es un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sistema de coordenadas de navegación local, respectivamente.

• Si PositionNoise es un escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

VelocityNoise — Variación del ruido del proceso de velocidad aditiva en el sistema de coordenadas de navegación local ((m/s)²)

[1e-6 1e-6 1e-6] (predeterminado) | escalar | vector fila de tres elementos

Ruido de proceso de velocidad aditivo en el sistema de coordenadas de navegación local en (m/s)², especificado como un vector fila escalar o de tres elementos de números finitos reales positivos.

• Si VelocityNoise es un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sistema de coordenadas de navegación local, respectivamente.

• Si VelocityNoise es un escalar, el elemento individual se aplica a cada eje.

Tipos de datos: single | double

AccelerationNoise — Variación del ruido del proceso de aceleración aditiva en el sistema de coordenadas de navegación local ((m/s²)²)

[50 50 50] (predeterminado) | escalar | vector fila de tres elementos

Ruido de proceso de aceleración aditivo en (m/s²)², especificado como un vector fila escalar o de tres elementos de números finitos reales positivos.

• Si AccelerationNoise es un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sistema de coordenadas de navegación local, respectivamente.

• Si AccelerationNoise es un 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-10 1e-10 1e-10] (predeterminado) | escalar | vector fila de tres 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 tres elementos de números finitos reales positivos.

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

• Si GyroscopeBiasNoise es un 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 positivo | vector fila de tres 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 tres elementos de números reales positivos.

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

• Si AccelerometerBiasNoise es un escalar, el elemento individual se aplica a cada eje.

GeomagneticVectorNoise — Varianza del ruido de proceso aditivo del vector geomagnético en el sistema de coordenadas de navegación local (μT²)

[1e-6 1e-6 1e-6] (predeterminado) | escalar positivo | vector fila de tres elementos

Varianza del ruido del proceso aditivo del vector geomagnético en μT², especificado como un vector fila escalar o de tres elementos de números reales positivos.

• Si GeomagneticVectorNoise es un vector fila, los elementos corresponden al ruido en los ejes x, y y z del sistema de coordenadas de navegación local, respectivamente.

• Si GeomagneticVectorNoise es un escalar, el elemento individual se aplica a cada eje.

MagnetometerBiasNoise — Variación del ruido del proceso aditivo a partir del sesgo del magnetómetro (μT²)

[0.1 0.1 0.1] (predeterminado) | escalar positivo | vector fila de tres elementos

Variación del ruido del proceso aditivo a partir del sesgo del magnetómetro en μT², especificada como un vector fila escalar o de tres elementos de números reales positivos.

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

• Si MagnetometerBiasNoise es un escalar, el elemento individual se aplica a cada eje.

State — Vector de estado del filtro de Kalman extendido

vector columna de 28 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
Velocidad angular (XYZ)	rad/s	5:7
Posición (NED o ENU)	metro	8:10
Velocidad (NED o ENU)	EM	11:13
Aceleración (NED o ENU)	m/s2	14:16
Polarización del acelerómetro (XYZ)	m/s2	17:19
Polarización del giroscopio (XYZ)	rad/s	20:22
Vector de campo geomagnético (NED o ENU)	µT	23:25
Polarización del magnetómetro (XYZ)	µT	26:28

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 extendido

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

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

Tipos de datos: single | double

Funciones del objeto

predict	Actualizar estados según el modelo de movimiento para insfilterAsync
fuseaccel	Estados correctos utilizando datos del acelerómetro para insfilterAsync
fusegyro	Estados correctos utilizando datos de giroscopio para insfilterAsync
fusemag	Estados correctos utilizando datos del magnetómetro para insfilterAsync
fusegps	Estados correctos utilizando datos GPS para insfilterAsync
correct	Estados correctos utilizando mediciones de estado directas para insfilterAsync
residual	Residuos y covarianzas residuales de mediciones de estado directas para insfilterAsync
residualaccel	Residuos y covarianza residual de las mediciones del acelerómetro para insfilterAsync
residualgps	Residuos y covarianza residual de las mediciones GPS para insfilterAsync
residualmag	Residuos y covarianza residual de las mediciones del magnetómetro para insfilterAsync
residualgyro	Residuos y covarianza residual de las mediciones del giroscopio para insfilterAsync
pose	Estimación de la posición, orientación y velocidad actuales para insfilterAsync
reset	Restablecer estados internos para insfilterAsync
stateinfo	Mostrar información del vector de estado para insfilterAsync
copy	Crear copia de insfilterAsync
tune	Ajuste los parámetros insfilterAsync para reducir el error de estimación
tunernoise	Estructura de ruido del filtro de fusión.

Ejemplos

Estimar la pose del VANT

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

load('uavshort.mat','refloc','initstate','imuFs', ...
    'accel','gyro','mag','lla','gpsvel', ...
    'trueOrient','truePos')

Cree un filtro INS para fusionar datos MARG y GPS asincrónicos para estimar la pose.

filt = insfilterAsync;
filt.ReferenceLocation = refloc;
filt.State = [initstate(1:4);0;0;0;initstate(5:10);0;0;0;initstate(11:end)];

Definir ruidos de medición del sensor. Los ruidos se determinaron a partir de hojas de datos y experimentación.

Rmag  = 80;
Rvel  = 0.0464;
Racc  = 800;
Rgyro = 1e-4;
Rpos  = 34;

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

N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

gpsIdx = 1;

Fusiona datos de acelerómetro, giroscopio, magnetómetro y GPS. El bucle externo predice el avance del filtro en un paso de tiempo y fusiona los datos del acelerómetro y el giroscopio a la frecuencia de muestreo de la IMU.

for ii = 1:N
    
    % Predict the filter forward one time step
    predict(filt,1./imuFs);
    
    % Fuse accelerometer and gyroscope readings
    fuseaccel(filt,accel(ii,:),Racc);
    fusegyro(filt,gyro(ii,:),Rgyro);
    
    % Fuse magnetometer at 1/2 the IMU rate
    if ~mod(ii, fix(imuFs/2))
        fusemag(filt,mag(ii,:),Rmag);
    end
    
    % Fuse GPS once per second
    if ~mod(ii,imuFs)
        fusegps(filt,lla(gpsIdx,:),Rpos,gpsvel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [p(ii,:),q(ii)] = pose(filt);
    
end

Calcule los errores RMS entre la posición y orientación verdaderas conocidas y la salida del filtro IMU asíncrono.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));

pRMS = sqrt(mean(posErr.^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.55, Y: 0.71, Z: 0.74 (meters)

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

Quaternion Distance RMS Error

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

	4.72 (degrees)

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

plot3(truePos(:,1),truePos(:,2),truePos(:,3),'LineWidth',2)
hold on
plot3(p(:,1),p(:,2),p(:,3),'r:','LineWidth',2)
grid on
xlabel('N (m)')
ylabel('E (m)')
zlabel('D (m)')

Algoritmos

Modelo dinámico utilizado en insfilterAsync

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

insfilterAsync implementa un filtro Kalman extendido continuo-discreto de 28 ejes utilizando fusión secuencial. El filtro se basa en el supuesto de que las mediciones de los sensores individuales no están correlacionadas. El filtro utiliza un modelo de movimiento omnidireccional y supone una velocidad angular constante y una aceleración constante. El estado se define como:

$$x=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\\ angVe{l}_{\text{X}}\\ angVe{l}_{\text{Y}}\\ angVe{l}_{\text{Z}}\\ positio{n}_{\text{N}}\\ positio{n}_{\text{E}}\\ positio{n}_{\text{D}}\\ {\nu }_{\text{N}}\\ {\nu }_{\text{E}}\\ {\nu }_{\text{D}}\\ acce{l}_{\text{N}}\\ acce{l}_{\text{E}}\\ acce{l}_{\text{D}}\\ accelbia{s}_{\text{X}}\\ accelbia{s}_{\text{Y}}\\ accelbia{s}_{\text{Z}}\\ gyrobia{s}_{\text{X}}\\ gyrobia{s}_{\text{Y}}\\ gyrobia{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.

• angVel_X, angVel_Y, angVel_Z –– Velocidad angular relativa al bastidor de la carrocería de la plataforma.

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

• accel_N, accel_E, accel_D –– Aceleración de la plataforma en el sistema de coordenadas NED local.

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

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

• 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 ecuación del proceso, $$\dot{x}=f\left(x\right)+w$$, w es el ruido del proceso, $$\dot{x}$$ es la derivada de x, y:

$$f\left(x\right)=\left[\begin{array}{c}\frac{-\left(q_1\right)\left(angVe{l}_{\text{X}}\right)-\left(q_2\right)\left(angVe{l}_{\text{Y}}\right)-\left(q_3\right)\left(angVe{l}_{\text{Z}}\right)}{2}\\ \frac{\left(q_0\right)\left(angVe{l}_{\text{X}}\right)-\left(q_3\right)\left(angVe{l}_{\text{Y}}\right)+\left(q_1\right)\left(angVe{l}_{\text{Z}}\right)}{2}\\ \frac{\left(q_3\right)\left(angVe{l}_{\text{X}}\right)+\left(q_0\right)\left(angVe{l}_{\text{Y}}\right)-\left(q_1\right)\left(angVe{l}_{\text{Z}}\right)}{2}\\ \frac{\left(q_1\right)\left(angVe{l}_{\text{X}}\right)-\left(q_2\right)\left(angVe{l}_{\text{Y}}\right)+\left(q_0\right)\left(angVe{l}_{\text{Z}}\right)}{2}\\ 0\\ 0\\ 0\\ {\nu }_{\text{N}}\\ {\nu }_{\text{E}}\\ {\nu }_{\text{D}}\\ acce{l}_{\text{N}}\\ acce{l}_{\text{E}}\\ acce{l}_{\text{D}}\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$$

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

insfilterErrorState | insfilterNonholonomic | insfilterMARG