K̂

Kalman Filter

Advanced
Ejecutar el ejemplo en vivo Gratis · sin registro · funciona en tu navegador

El bloque KalmanFilter implementa un filtro de Kalman lineal en tiempo discreto para la estimación óptima de estados en sistemas dinámicos lineales de dos estados con una única medición escalar. Al combinar las predicciones del modelo físico con observaciones ruidosas ponderadas por las covarianzas de ruido de proceso (QQ) y de medida (RR), el bloque estima variables no medibles directamente (como posición y velocidad) a la par que monitoriza la incertidumbre de estimación (σ=Pii\sigma = \sqrt{P_{ii}}). En el Editor de Propiedades, todas las matrices y vectores (a, b, c, q, xInit, pInit) se configuran en cuadrículas visuales matriciales 2×22 \times 2 y vectoriales de 2 estados, organizadas en tres grupos claros (_Tiempo y tipo de modelo_, _Modelo de espacio de estados_ y _Ruido y covarianza inicial_).

Vídeo tutorial

Modelo Matemático

El filtro estima el estado x∈R2\mathbf{x} \in \mathbb{R}^2 a partir de la medición escalar z∈Rz \in \mathbb{R} y la entrada de control opcional u∈Ru \in \mathbb{R}.

En modelos continuos (x˙=Acx+Bcu+w\dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} + \mathbf{B}_c u + \mathbf{w} con densidad de ruido Qc\mathbf{Q}_c), el sistema se discretiza de forma exacta sobre el tiempo de muestreo TsT_s:

A=eAcTs,B=∫0TseAcτdτ Bc,Q=∫0TseAcτQceAcTτdτ\mathbf{A} = e^{\mathbf{A}_c T_s}, \quad \mathbf{B} = \int_0^{T_s} e^{\mathbf{A}_c \tau} d\tau \, \mathbf{B}_c, \quad \mathbf{Q} = \int_0^{T_s} e^{\mathbf{A}_c \tau} \mathbf{Q}_c e^{\mathbf{A}_c^T \tau} d\tau

En cada periodo discreto kk, el filtro ejecuta dos etapas secuenciales:

1. Actualización temporal (Predicción):

x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q\begin{aligned} \hat{\mathbf{x}}_k^- &= \mathbf{A}\hat{\mathbf{x}}_{k-1} + \mathbf{B}u_{k-1} \\ \mathbf{P}_k^- &= \mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^T + \mathbf{Q} \end{aligned}

2. Actualización de medida (Corrección):

Medición esperada z^k=Cx^k−+Duk+offset\hat{z}_k = \mathbf{C}\hat{\mathbf{x}}_k^- + D u_k + \text{offset}, innovación yk=zk−z^ky_k = z_k - \hat{z}_k y varianza de innovación SkS_k:

Sk=CPk−CT+RS_k = \mathbf{C}\mathbf{P}_k^-\mathbf{C}^T + R

La ganancia óptima de Kalman Kk\mathbf{K}_k y el estado corregido x^k\hat{\mathbf{x}}_k se expresan como:

Kk=Pk−CTSk−1,x^k=x^k−+Kkyk\mathbf{K}_k = \mathbf{P}_k^-\mathbf{C}^T S_k^{-1}, \quad \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k y_k

Actualización de covarianza en formulación simétrica estabilizada de Joseph:

Pk=(I−KkC)Pk−(I−KkC)T+KkRKkT\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k\mathbf{C})\mathbf{P}_k^-(\mathbf{I} - \mathbf{K}_k\mathbf{C})^T + \mathbf{K}_k R \mathbf{K}_k^T

Supervisión de consistencia mediante la innovación normalizada al cuadrado: NISk=yk2/Sk\text{NIS}_k = y_k^2 / S_k.

Entradas y Salidas

Direction ID Label Tipo Status
→ In in1 z number Required
→ In in2 u number Optional
→ In in3 Reset number Optional
→ In in4 Dropout number Optional
→ In in5 Offset number Optional
← Out x_hat_0 x̂₀ number Output
← Out x_hat_1 x̂₁ number Output
← Out sigma_0 σ₀ number Output
← Out sigma_1 σ₁ number Output
← Out k_gain_0 K₀ number Output
← Out k_gain_1 K₁ number Output
← Out z_hat ẑ number Output
← Out innovation Innov. number Output
← Out innovation_cov S number Output
← Out nis NIS number Output

Parámetros

Parámetros Label Tipo Por defecto Descripción
sampleTime Sample Time number 1 Filter period in seconds
model Model select continuous Continuous: dx/dt = A·x + B·u, discretised exactly for the sample time. Discrete: x[k+1] = A·x[k] + B·u[k], already for this sample time
a A (2x2) vector 0 1; 0 0 State matrix, row by row
b B (2x1) vector 0 1 Input matrix
c C (1x2) vector 1 0 Measurement row
d D number 0 Direct feedthrough from u to the measurement
offset Measurement Offset number 0 Constant term in z = C·x + D·u + offset, e.g. the intercept of a linearised curve
q Q vector 0 0.01 Process noise: two numbers for the diagonal, four for the full matrix. Per second (a density) for a continuous model, per sample for a discrete one
r R number 1 Measurement noise variance
xInit Initial Estimate vector 0 0
pInit Initial Covariance vector 1 1 Two numbers for the diagonal, four for the full matrix

Ejemplos de Uso

Ejemplo 1: Seguimiento de posición y velocidad ante radar ruidoso

Estimación de la posición xx y velocidad vv de un vehículo a partir de un radar con dispersión (R=4.0 m2R = 4.0\text{ m}^2). Con matrices continuas a = "0 1; 0 0", b = "0; 1", c = "1 0" y ruido de proceso q = "0.01 0.1". El filtro infiere la velocidad con bandas de incertidumbre 2σ2\sigma suaves y reduce el ruido de posición en más del 70%70\%.

Ejemplo 2: Estimación de orientación giroscópica tolerante a fallos de enlace

Estimación angular ante pérdidas temporales de telemetría. Al conectar el sensor a z y un bit de diagnóstico a Dropout. Ante pérdidas de señal (Dropout > 0.5), el filtro omite la corrección y predice según su modelo; la incertidumbre σ\sigma crece suavemente hasta que la comunicación se reanuda.

Observaciones y Buenas Prácticas

  • Cuadrículas visuales matriciales 2×22 \times 2 y vectoriales: Configure cada celda de A,B,C,Q,x0,P0\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{Q}, \mathbf{x}_0, \mathbf{P}_0 directamente en la interfaz interactiva sin lidiar con formatos de texto.
  • Ajuste mediante diagnóstico NIS: La innovación normalizada al cuadrado (NIS=y2/S\text{NIS} = y^2 / S) debe promediar 1.01.0 en condiciones normales. Valores superiores denotan exceso de confianza (QQ o RR muy pequeños); valores inferiores reflejan conservadurismo.
  • Formulación estabilizada de Joseph: La covarianza se actualiza mediante la ecuación simétrica de Joseph, garantizando que P\mathbf{P} conserve su condición de semidefinida positiva.
  • Desvío de linealización afín: El parámetro y puerto offset permiten operar alrededor de puntos de consigna no nulos sin distorsionar la matriz de transición A\mathbf{A}.
  • Independencia de cálculo temporal: El filtro opera a periodos exactos TsT_s, permitiendo la integración de plantas físicas continuas con solucionadores ODE de alto orden (RK4, TR-BDF2).

Preguntas Frecuentes (FAQ)

Q: How to tune process noise covariance Q vs measurement noise covariance R?

RR is directly obtained from sensor characterization (variance of stationary sensor noise σ2\sigma^2). QQ is a design tuning parameter representing unmodeled dynamics and input disturbances. If Q≫RQ \gg R, the filter places high trust in sensor measurements (fast dynamic tracking, less noise filtering). If R≫QR \gg Q, the filter places high trust in the system model (smooth estimates, but slower response to unexpected physical maneuvers).

Bloques Relacionados

  • StateSpace: Modelo continuo multivariable de espacio de estados.
  • RandomNumber: Inyección de ruido blanco gaussiano para ensayos de filtrado.
  • NumericDisplay: Lectura numérica instantánea en pantalla.
  • uPlotDisplay: Osciloscopio multicanal para seguir estimaciones y bandas de incertidumbre.