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

insEKF

Navegación inercial mediante filtro de Kalman extendido

Desde R2022a

Descripción

El objeto insEKF crea un filtro de Kalman extendido (EKF) continuo-discreto, en el que la predicción de estado utiliza un modelo de tiempo continuo y la corrección de estado utiliza un modelo de tiempo discreto. El filtro utiliza datos de sensores inerciales para estimar los estados de la plataforma, como la posición, la velocidad y la orientación. La toolbox proporciona algunos modelos de sensores, como insAccelerometer, insGyroscope, insGPS y insMagnetometer., que puede utilizar para habilitar las mediciones correspondientes en el EKF. También puede personalizar sus propios modelos de sensores heredando de la clase de interfaz positioning.insSensorModel . La toolbox también proporciona modelos de movimiento, como insMotionOrientation y insMotionPose, que puede utilizar para habilitar la propagación del estado correspondiente en el EKF. También puede personalizar sus propios modelos de movimiento heredándolos de la clase de interfaz positioning.insMotionModel .

Creación

Sintaxis

filter = insEKF

filter = insEKF(sensor1,sensor2,...,sensorN)

filter = insEKF(___,motionModel)

filter = insEKF(___,options)

Descripción

filter = insEKF crea un objeto de filtro insEKF con valores de propiedad predeterminados. Con la configuración predeterminada, el filtro puede estimar la orientación fusionando datos del acelerómetro y el giroscopio.

filter = insEKF(sensor1,sensor2,...,sensorN) configura el filtro para aceptar y fusionar datos de uno o más sensores. El filtro guarda estos sensores en su propiedad Sensors .

filter = insEKF(___,motionModel) configura el filtro para usar el modelo de movimiento para predecir y estimar el estado, además de cualquier combinación de argumentos de entrada de sintaxis anteriores. El filtro guarda el modelo de movimiento especificado en la propiedad MotionModel .

filter = insEKF(___,options) configura el filtro usando el objeto insOptions options.

Propiedades

State — Vector de estado del filtro de Kalman extendido

N-elemento vector de valor real

Vector de estado del filtro de Kalman extendido, especificado como un vector de valor real de elemento N. N es la dimensión del estado del filtro, determinada por los sensores específicos y el modelo de movimiento utilizados para construir el filtro.

Nota

En la propiedad State , si una variable de estado denominada Orientation tiene una longitud de cuatro, el objeto asume que es un quaternion. En ese caso, el filtro renormaliza el cuaternión y asegura que la parte real del cuaternión sea siempre positiva.

Tipos de datos: single | double

StateCovariance — Covarianza de error de estado del filtro de Kalman extendido

N-por- N matriz definida positiva de valor real

Covarianza de error de estado para el filtro de Kalman extendido, especificada como una matriz definida positiva de valor real N por N . N es la dimensión del estado, especificada en la propiedad State del filtro.

Tipos de datos: single | double

AddtiveProcessNoise — Ruido de proceso aditivo para filtro Kalman extendido

N-por- N matriz definida positiva de valor real

Ruido de proceso aditivo para el filtro de Kalman extendido, especificado como una matriz definida positiva de valor real N por N . N es la dimensión del estado, especificada en el State del filtro.

Tipos de datos: single | double

MotionModel — Modelo de movimiento utilizado en el filtro de Kalman extendido

insMotionOrientation objeto | insMotionPose objeto | objeto heredado de positioning.INSMotionModel clase

Esta propiedad o parámetro es de solo lectura.

Modelo de movimiento utilizado en el filtro de Kalman extendido, especificado como un objeto insMotionOrientation , un objeto insMotionPose o un objeto heredado de positioning.INSMotionModel clase de interfaz. Especifique un modelo de movimiento utilizando el argumento de entrada motionModel .

Tipos de datos: object

Sensors — Sensores fusionados en filtro Kalman extendido

{insAccelerometer,insGyroscope} (predeterminado) | arreglo de células de objetos de sensores inerciales

Esta propiedad o parámetro es de solo lectura.

Sensores fusionados en el filtro de Kalman extendido, especificado como un arreglo de celdas de objetos sensores inerciales. Un objeto de sensor inercial es uno de estos objetos:

• Un objeto insAccelerometer

• Un objeto insMagnetometer

• Un objeto insGPS

• Un objeto insGyroscope

• Un objeto que hereda de la clase de interfaz positioning.INSSensorModel

Tipos de datos: cell

SensorNames — Nombres de sensores

arreglo de celdas de vectores de caracteres

Esta propiedad o parámetro es de solo lectura.

Nombres de los sensores, especificados como un arreglo de celdas de vectores de caracteres. De forma predeterminada, el filtro nombra los sensores usando el formato 'sensorname_n', donde sensorname es el nombre del sensor, como Accelerometer, y n es el índice para sensores adicionales del mismo tipo.

Para personalizar los nombres de los sensores, especifique la entrada options al construir el filtro.

Ejemplo: {'Accelerometer' 'Accelerometer_1' 'Accelerometer_2' 'Gyroscope'}

Tipos de datos: cell

ReferenceFrame — Marco de referencia del filtro de Kalman extendido

"NED" (predeterminado) | "ENU"

Esta propiedad o parámetro es de solo lectura.

Marco de referencia del filtro de Kalman extendido, especificado como "NED" para el marco noreste abajo o "ENU" para el marco este-norte arriba.

Para especificar el marco de referencia como "ENU", especifique la entrada options al construir el filtro.

Tipos de datos: char | string

Funciones del objeto

predict	Predecir estimaciones de estado hacia adelante en el tiempo para insEKF
fuse	Combinar datos del sensor para estimación de estado en TG
residual	Covarianza residual y residual de la medición estatal para insEKF
correct	Estimaciones de estado correctas en insEKF usando mediciones de estado directas
stateparts	Obtener y establecer parte del vector de estado en insEKF
statecovparts	Obtener y establecer parte de la matriz de covarianza de estado en insEKF
stateinfo	Información del vector de estado para insEKF
estimateStates	Fusión por lotes y suavizado de datos de sensores
tune	Ajuste los parámetros insEKF para reducir el error de estimación
createTunerCostTemplate	Crear plantilla de función de coste del ajustador
tunerCostFcnParam	Primer ejemplo de parámetro para ajustar la función de coste
copy	Crear copia de insEKF
reset	Restablecer estados para insEKF

Ejemplos

Cree insEKF con diferentes configuraciones

Cree un objeto insEKF predeterminado. De forma predeterminada, el filtro fusiona los datos de medición de un acelerómetro y un giroscopio asumiendo un movimiento de solo orientación.

filter1 = insEKF

filter1 = 
  insEKF with properties:

                   State: [13x1 double]
         StateCovariance: [13x13 double]
    AdditiveProcessNoise: [13x13 double]
             MotionModel: [1x1 insMotionOrientation]
                 Sensors: {[1x1 insAccelerometer]  [1x1 insGyroscope]}
             SensorNames: {'Accelerometer'  'Gyroscope'}
          ReferenceFrame: 'NED'

Cree un segundo objeto insEKF que fusione datos de un acelerómetro, un giroscopio y un magnetómetro, además de modelar tanto el movimiento de rotación como el de traslación.

filter2 = insEKF(insAccelerometer,insGyroscope,insMagnetometer,insMotionPose)

filter2 = 
  insEKF with properties:

                   State: [28x1 double]
         StateCovariance: [28x28 double]
    AdditiveProcessNoise: [28x28 double]
             MotionModel: [1x1 insMotionPose]
                 Sensors: {[1x1 insAccelerometer]  [1x1 insGyroscope]  [1x1 insMagnetometer]}
             SensorNames: {'Accelerometer'  'Gyroscope'  'Magnetometer'}
          ReferenceFrame: 'NED'

Cree un tercer objeto insEKF que fusione datos de un giroscopio y un GPS. Especifique el marco de referencia del filtro como el marco este-norte arriba (ENU). Tenga en cuenta que el modelo de movimiento que utiliza el filtro es el objeto insMotionPose porque un GPS mide las posiciones de la plataforma.

option = insOptions(ReferenceFrame="ENU");
filter3 = insEKF(insGyroscope,insGPS,option)

filter3 = 
  insEKF with properties:

                   State: [19x1 double]
         StateCovariance: [19x19 double]
    AdditiveProcessNoise: [19x19 double]
             MotionModel: [1x1 insMotionPose]
                 Sensors: {[1x1 insGyroscope]  [1x1 insGPS]}
             SensorNames: {'Gyroscope'  'GPS'}
          ReferenceFrame: 'ENU'

Fusión secuencial de datos de acelerómetro y giroscopio usando insEKF

Cargue datos de medición desde un acelerómetro y un giroscopio.

load("accelGyroINSEKFData.mat");

Cree un objeto de filtro insEKF . Especifique la parte de orientación del estado en el filtro utilizando la orientación inicial de los datos de medición. Especifique los elementos diagonales de la matriz de covarianza del error de estimación del estado correspondiente al estado de orientación como 0.01.

accel = insAccelerometer;
gyro = insGyroscope;
filt = insEKF(accel,gyro);
stateparts(filt,"Orientation",compact(initOrient));
statecovparts(filt,"Orientation",1e-2);

Especifique el ruido de medición y el ruido del proceso aditivo. Puede obtener estos valores utilizando la función de objeto tune del objeto de filtro.

accNoise = 0.1739;
gyroNoise = 1.1129;
processNoise = diag([ ...
    2.8586 1.3718 0.8956 3.2148 4.3574 2.5411 3.2148 0.5465 0.2811 ...
    1.7149 0.1739 0.7752 0.1739]);
filt.AdditiveProcessNoise = processNoise;

Fusione secuencialmente los datos de medición utilizando las funciones de objeto predict y fuse del objeto de filtro.

N = size(sensorData,1);
estOrient = quaternion.zeros(N,1);
dt = seconds(diff(sensorData.Properties.RowTimes));
for ii = 1:N
    if ii ~= 1
        % Step forward in time.
        predict(filt,dt(ii-1));
    end
    % Fuse accelerometer data.
    fuse(filt,accel,sensorData.Accelerometer(ii,:),accNoise);
    % Fuse gyroscope data.
    fuse(filt,gyro,sensorData.Gyroscope(ii,:),gyroNoise);
    % Extract the orientation state estimate using the stateparts object
    % function.
    estOrient(ii) = quaternion(stateparts(filt,"Orientation"));
end

Visualice el error de estimación, en distancia de cuaternión, utilizando la función de objeto dist del objeto quaternion .

figure
plot(rad2deg(dist(estOrient,groundTruth.Orientation)))
xlabel("Samples")
ylabel("Distance (degrees)")
title("Orientation Estimate Error")

Fusión por lotes y suavizado de estado usando insEKF

Cargue datos de medición desde un acelerómetro y un giroscopio.

load("accelGyroINSEKFData.mat");

Cree un objeto de filtro insEKF . Especifique la parte de orientación del estado en el filtro utilizando la orientación inicial de los datos de medición. Especifique los elementos diagonales de la matriz de covarianza del error de estimación del estado correspondiente al estado de orientación como 0.01.

filt = insEKF;
stateparts(filt,"Orientation",compact(initOrient));
statecovparts(filt,"Orientation",1e-2);

Especifique el ruido de medición y el ruido del proceso aditivo. Puede obtener estos valores utilizando la función de objeto tune del objeto de filtro.

measureNoise = struct("AccelerometerNoise", 0.1739, ...
    "GyroscopeNoise", 1.1129);
processNoise = diag([ ...
    2.8586 1.3718 0.8956 3.2148 4.3574 2.5411 3.2148 0.5465 0.2811 ...
    1.7149 0.1739 0.7752 0.1739]);
filt.AdditiveProcessNoise = processNoise;

Estime por lotes los estados utilizando la función de objeto estimateStates . Además, obtenga las estimaciones después del suavizado.

[estimates,smoothEstimates] = estimateStates(filt,sensorData,measureNoise);

Visualice la orientación estimada en ángulos de Euler.

figure
t = estimates.Properties.RowTimes;
plot(t,eulerd(estimates.Orientation,"ZYX","frame"));
title("Estimated Orientation");
ylabel("Degrees")

Visualice la orientación estimada después del suavizado en ángulos de Euler.

figure
plot(t,eulerd(smoothEstimates.Orientation,"ZYX","frame"));
title("Smoothed Orientation");
ylabel("Degrees")

Visualice el error de estimación, en distancia de cuaternión, utilizando la función de objeto dist del objeto quaternion .

trueOrient = groundTruth.Orientation;
plot(t,rad2deg(dist(estimates.Orientation, trueOrient)), ...
     t,rad2deg(dist(smoothEstimates.Orientation, trueOrient)));
title("Estimated and Smoother Error");
legend("Estimation Error","Smoothed Error")
xlabel("Time");
ylabel("Degrees")

Capacidades ampliadas

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

Historial de versiones

Introducido en R2022a

Consulte también

insOptions | insAccelerometer | insGPS | insGyroscope | insMagnetometer | insMotionOrientation | insMotionPose | positioning.INSMotionModel | positioning.INSSensorModel | tunerconfig | tunernoise | tunerPlotPose