Kalman Filter
AdvancedEl 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 () y de medida (), el bloque estima variables no medibles directamente (como posición y velocidad) a la par que monitoriza la incertidumbre de estimación (). En el Editor de Propiedades, todas las matrices y vectores (a, b, c, q, xInit, pInit) se configuran en cuadrículas visuales matriciales 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 a partir de la medición escalar y la entrada de control opcional .
En modelos continuos ( con densidad de ruido ), el sistema se discretiza de forma exacta sobre el tiempo de muestreo :
En cada periodo discreto , el filtro ejecuta dos etapas secuenciales:
1. Actualización temporal (Predicción):
2. Actualización de medida (Corrección):
Medición esperada , innovación y varianza de innovación :
La ganancia óptima de Kalman y el estado corregido se expresan como:
Actualización de covarianza en formulación simétrica estabilizada de Joseph:
Supervisión de consistencia mediante la innovación normalizada al cuadrado: .
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 y velocidad de un vehículo a partir de un radar con dispersión (). 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 suaves y reduce el ruido de posición en más del .
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 crece suavemente hasta que la comunicación se reanuda.
Observaciones y Buenas Prácticas
- Cuadrículas visuales matriciales y vectoriales: Configure cada celda de directamente en la interfaz interactiva sin lidiar con formatos de texto.
- Ajuste mediante diagnóstico NIS: La innovación normalizada al cuadrado () debe promediar en condiciones normales. Valores superiores denotan exceso de confianza ( o 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 conserve su condición de semidefinida positiva.
- Desvío de linealización afín: El parámetro y puerto
offsetpermiten operar alrededor de puntos de consigna no nulos sin distorsionar la matriz de transición . - Independencia de cálculo temporal: El filtro opera a periodos exactos , 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?
is directly obtained from sensor characterization (variance of stationary sensor noise ). is a design tuning parameter representing unmodeled dynamics and input disturbances. If , the filter places high trust in sensor measurements (fast dynamic tracking, less noise filtering). If , 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.