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 continuo-discreto (EKF), en el que la predicción del estado utiliza un modelo de tiempo continuo y la corrección del 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 (Sensor Fusion and Tracking Toolbox). 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 puedes personalizar tus propios modelos de movimiento heredando de la clase de interfaz positioning.INSMotionModel (Sensor Fusion and Tracking Toolbox).

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 utilizar 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 utilizando el objeto insOptions options.

Propiedades

State — Vector de estado del filtro de Kalman extendido

vector de valor real de N-elemento

Vector de estado del filtro Kalman extendido, especificado como un vector de valor real de elementos 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 llamada Orientation tiene una longitud de cuatro, el objeto asume que es una 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

Matriz definida positiva de valor real N por N

Covarianza de error de estado para el filtro de Kalman extendido, especificada como una matriz positiva definida 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

AdditiveProcessNoise — Ruido de proceso aditivo para filtro Kalman extendido

Matriz definida positiva de valor real N por N

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

Tipos de datos: single | double

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

Solo lectura: objeto insMotionOrientation | objeto insMotionPose | objeto que hereda de la clase positioning.INSMotionModel

Esta propiedad o parámetro es de solo lectura.

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

Tipos de datos: object

Sensors — Sensores fusionados en filtro Kalman extendido

Solo lectura: {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

Solo lectura: 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 utilizando el formato 'sensorname_n', donde sensorname es el nombre del sensor, como Accelerometer, y n es el índice de 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

Solo lectura: "NED" (predeterminado) | "ENU"

Esta propiedad o parámetro es de solo lectura.

Marco de referencia del filtro Kalman extendido, especificado como "NED" para el marco noreste hacia abajo o "ENU" para el marco este-norte hacia 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	Datos del sensor de fusibles para la estimación del estado en insEKF
residual	Covarianza residual y residual de la medición del estado para insEKF
correct	Estimaciones de estado correctas en insEKF utilizando 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 estados 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

Crear insEKF con diferentes configuraciones

Crea 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: [13×1 double]
         StateCovariance: [13×13 double]
    AdditiveProcessNoise: [13×13 double]
             MotionModel: [1×1 insMotionOrientation]
                 Sensors: {[1×1 insAccelerometer]  [1×1 insGyroscope]}
             SensorNames: {'Accelerometer'  'Gyroscope'}
          ReferenceFrame: 'NED'

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

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

filter2 = 
  insEKF with properties:

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

Crea un tercer objeto insEKF que fusiona 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: [19×1 double]
         StateCovariance: [19×19 double]
    AdditiveProcessNoise: [19×19 double]
             MotionModel: [1×1 insMotionPose]
                 Sensors: {[1×1 insGyroscope]  [1×1 insGPS]}
             SensorNames: {'Gyroscope'  'GPS'}
          ReferenceFrame: 'ENU'

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

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

load("accelGyroINSEKFData.mat");

Crea 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 de error de estimación de 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;

Fusionar 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 estados con insEKF

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

load("accelGyroINSEKFData.mat");

Crea 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 de error de estimación de 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