BlockWerk Documentación
ENNLDEFRESZH
Abrir Canvas
K̂

Kalman Filter

Advanced

El bloque KalmanFilter estima el estado oculto de un sistema lineal a partir de una medición ruidosa. En cada paso de muestreo predice la evolución del estado basándose en el modelo físico y la entrada conocida, corrigiendo luego esa predicción con la medición disponible ponderada según la confianza relativa: el ruido de proceso Q frente al ruido de medición R.

Describa el sistema en tiempo continuo (dx/dt = A·x + B·u) con Q como densidad espectral por segundo, y el bloque lo discretiza de forma exacta para el tiempo de muestreo fijado, incluyendo el ruido estocástico. Maneja dos estados y una medición, cubriendo el seguimiento de posición/velocidad, plantas de primer orden con sesgo desconocido y modelos de baterías linealizados. Además del vector de estado estimado, entrega su incertidumbre (σ), las ganancias de Kalman y el cuadrado de la innovación normalizada (NIS), métrica estadística que verifica si Q y R están sintonizados adecuadamente. Opera sincrónicamente en cada muestra, continúa prediciendo ante pérdidas de señal de medición y se integra directamente en lazos cerrados de control.

Entradas. z es la medición y u la entrada de control conocida. Reset reinicia el filtro desde su estado inicial. Una entrada Valid menor a 0.5 hace que el filtro prediga sin corrección de medida.

Offset representa el término constante del modelo de medición z = C·x + offset. Al linealizar una curva de sensor no lineal alrededor de un punto de operación se obtiene una tangente afín; conectar la entrada Offset permite gobernar dinámicamente este término desde el diagrama.

Simular en BlockWerk

Simulador interactivo en tu navegador — sin instalación, motor WebAssembly.

Simular en BlockWerk →

Modelo Matemático

Modelo continuo:x˙=Acx+Bcu+w,E[ww⊤]=Qc δ(t−s)A=eAcTs,B=∫0TseAcs ds Bc,Q=∫0TseAcsQc eAc⊤s dsPrediccioˊn:x^−=Ax^+Buk−1,P−=APA⊤+QActualizacioˊn:S=CP−C⊤+R,K=P−C⊤/Sx^=x^−+K (z−z^),z^=Cx^−+Du+offset,P=(I−KC)P−(I−KC)⊤+KRK⊤σi=Pii,NIS=(z−z^)2/S\text{Modelo continuo:}\quad \dot{x} = A_c x + B_c u + w,\quad E[w w^\top] = Q_c\,\delta(t-s) A = e^{A_c T_s},\quad B = \int_0^{T_s} e^{A_c s}\,ds\,B_c,\quad Q = \int_0^{T_s} e^{A_c s} Q_c\, e^{A_c^\top s}\,ds \text{Predicción:}\quad \hat{x}^- = A\hat{x} + B u_{k-1},\qquad P^- = A P A^\top + Q \text{Actualización:}\quad S = C P^- C^\top + R,\qquad K = P^- C^\top / S \hat{x} = \hat{x}^- + K\,(z - \hat{z}),\quad \hat{z} = C\hat{x}^- + D u + \text{offset},\quad P = (I - KC)P^-(I - KC)^\top + K R K^\top \sigma_i = \sqrt{P_{ii}},\qquad \text{NIS} = (z - \hat{z})^2 / S

Donde:

  • x̂ es el vector estimado de dos estados y P su matriz de covarianza.
  • z es la medición y u la entrada de control conocida (retenedor de orden cero por período).
  • El modelo continuo se discretiza de manera exacta mediante exponencial de matriz y el algoritmo de Van Loan para Q.
  • K es la ganancia de Kalman y S la varianza escalar de la innovación.
  • La primera muestra omite la predicción y corrige directamente el estado inicial; una muestra con Valid < 0.5 omite la actualización.

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

Estimación de una señal senoidal ruidosa

Una señal senoidal lenta afectada por ruido gaussiano (σ = 0.15) ingresa a un filtro de Kalman de velocidad constante muestreado a 50 Hz. El filtro reduce el error RMS de 0.150 a 0.040.

Observación de velocidad sin tacómetro

Filtrado de un transductor de posición cuantificado para obtener posición y velocidad simultáneamente sin la amplificación de ruido que produce una diferenciación numérica directa.

Observaciones y Buenas Prácticas

  • Salidas σ: x̂₀ ± 2σ₀ define un intervalo de confianza del 95 % cuando el modelo es acertado. Verifíquelo con la salida NIS (promedio temporal cercano a 1).
  • Entrada Valid: Conéctela al estado del sensor. Por debajo de 0.5, el bloque solo predice y σ crece hasta que se restablezcan lecturas válidas.
  • Lazos cerrados: La estimación puede alimentar un controlador cuya salida regrese como u. Al utilizar la entrada previa en la predicción, para d = 0 no se genera un lazo algebraico.
  • Robustez numérica: Matrices con dimensiones incorrectas activan de forma segura el modelo predeterminado sin detener la simulación.

Bloques Relacionados