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

estimateStates

Fusión por lotes y suavizado de datos de sensores

Desde R2022a

Sintaxis

estimates = estimateStates(filter,sensorData,measurementNoise)

[estimates,smoothEstimates] = estimateStates(___)

Descripción

estimates = estimateStates(filter,sensorData,measurementNoise) devuelve las estimaciones de estado basadas en el modelo de movimiento utilizado en el filtro, los datos del sensor y el ruido de medición. La función predice las estimaciones del estado del filtro hacia adelante en el tiempo basándose en los tiempos de fila en sensorData y fusiona los datos de cada columna de la tabla uno por uno.

[estimates,smoothEstimates] = estimateStates(___) además devuelve las estimaciones de estado suavizadas mediante el suavizador de Kalman no lineal Rauch-Tung-Striebel (RTS). Para obtener detalles del algoritmo, consulte Algoritmos y [1].

Sugerencia

El suavizado suele requerir considerablemente más memoria y tiempo de cálculo. Utilice esta sintaxis sólo cuando necesite los estados estimados suavizados.

Ejemplos

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")

Argumentos de entrada

filter — filtro INS

objeto insEKF

Filtro INS, especificado como un objeto insEKF.

sensorData — Datos del sensor

timetable

Datos del sensor, especificados como timetable. Cada nombre de variable (como columna) en el horario debe coincidir con uno de los nombres de sensores especificados en la propiedad SensorNames de filter. Cada entrada en la tabla es la medición del sensor en el momento de la fila correspondiente.

Si un sensor no produce mediciones en un momento dado, especifique la entrada correspondiente como NaN.

measurementNoise — Ruido de medición

estructura

Ruido de medición de los sensores, especificado como estructura. Cada nombre de campo debe coincidir con uno de los nombres de sensores especificados en la propiedad SensorNames de filter. El valor del campo es la matriz de covarianza del ruido de medición correspondiente. Si especifica un valor de campo como escalar, la función extiende el escalar a la diagonal de la matriz.

Tipos de datos: struct

Argumentos de salida

estimates — Estimaciones estatales

timetable

Estimaciones estatales, devueltas como timetable. El nombre de cada variable en la tabla representa un estado. Puede obtener los nombres de las variables utilizando la función de objeto stateinfo del filtro. La última columna de la tabla es la matriz de covarianza del error de estimación de estado para el vector de estado completo del filtro en cada uno de los tiempos de fila.

smoothEstimates — Estimaciones de estado suavizadas

timetable

Estimaciones de estado suavizadas, devueltas como timetable. El nombre de cada variable en la tabla representa un estado. Puede obtener los nombres de las variables utilizando la función de objeto stateinfo del filtro. La última columna de la tabla es la matriz de covarianza del error de estimación de estado para el vector de estado completo del filtro en cada uno de los tiempos de fila.

Algoritmos

Estrategia en tiempo real más suave

Considere un modelo no lineal discreto continuo de la siguiente manera.

$$\begin{array}{l}\frac{d}{dt}x(t)=f(x(t),t)+w(t),w(t)~N(0,Q(t))\\ y_k=h(x_k)+v_k,v_k~N(0,R_k)\end{array}$$

En la ecuación, t representa el tiempo continuo del sistema, x es el estado del sistema, f es la ecuación de estado y w es el ruido del proceso que sigue una distribución normal de media 0 y covarianza Q. k es el paso de tiempo discreto, y es la medición, h es la función de medición, v es el ruido de medición que sigue una distribución normal de media 0 y covarianza R.

Considere un período de tiempo [0, T], donde T es el tiempo total considerado para el suavizado. El suavizador primero realiza un filtrado hacia adelante para t ∈ [0, T] mediante el uso de un filtro de Kalman extendido discreto continuo regular. Finalmente, el suavizador obtiene la estimación del estado directo x_f(T) y la estimación de covarianza directa P_f(T) en el tiempo final. El suavizador también guarda las estimaciones de estado y las covarianzas en los pasos intermedios cuando el suavizador corrige el estado estimado con mediciones.

A continuación, el suavizador obtiene el estado suavizado utilizando un filtro hacia atrás. Para mayor comodidad, defina una variable τ = T - t que represente el tiempo hacia atrás. El filtro hacia atrás obtiene el estado suavizado x_s y la covarianza P_s en cada tiempo de medición utilizando la integración hacia atrás con estas ecuaciones.

$$\begin{array}{l}{K}_f(t)=Q{P}_f^{-1}(t)\\ \frac{d}{d\tau }{P}_s(t)=-\left[F\left(x_f(t),t\right)+K_f(t)\right]P_f(t)-P_f(t){\left[F\left(x_f(t),t\right)+K_f(t)\right]}^T+Q(t),P_s(T)=P_f(T)\\ \frac{d}{d\tau }{x}_s(t)=-\left[F\left(x_f(t),t\right)+K_f(t)\right]\left[x_s(t)-x_f(t)\right]-f(x_f(t),t),x_s(T)=x_f(T)\end{array}$$

En estas ecuaciones, K_f(t) es la ganancia de Kalman y F = ∂f(x,t)/∂x es la matriz jacobiana del modelo de estado.

Referencias

[1] Crassidis, John L., and John L. Junkins. "Optimal Estimation of Dynamic Systems". 2nd ed, CRC Press, pp. 349- 352, 2012.

Capacidades ampliadas

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

Historial de versiones

Introducido en R2022a

R2023a: Estimaciones de estado suaves usando insEKF

Ahora puede obtener estimaciones de estado suavizadas como segunda salida de la función estimateStates. La función utiliza el algoritmo de suavizado Rauch-Tung-Striebel (RTS) para obtener estimaciones de estado suavizadas.

Consulte también

predict | fuse | residual | correct | stateparts | statecovparts | stateinfo | tune | createTunerCostTemplate | tunerCostFcnParam