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 en función de los tiempos de las filas 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 utilizando el suavizador Kalman no lineal de Rach-Tung-Striebel (RTS). Para obtener detalles sobre el 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 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")

Argumentos de entrada

filter — filtro INS

insEKF objeto

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 cronograma debe coincidir con uno de los nombres de sensor especificados en la propiedad SensorNames del 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 de fila, 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 sensor especificados en la propiedad SensorNames del 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 medida, 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 más suave primero realiza un filtrado directo para t ∈ [0, T] mediante el uso de un filtro Kalman extendido discreto continuo regular. Finalmente, el suavizador obtiene la estimación del estado directo x f (T) y la estimación de la covarianza directa P f (T) en el momento 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. Por conveniencia, 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 mediante el uso de 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 el Matriz jacobiana del modelo estatal.

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 segundo resultado 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