Kalman Filter
AdvancedThe KalmanFilter block implements a linear discrete-time Kalman filter for optimal state estimation in two-state linear dynamical systems with a single scalar measurement. By blending physics-based model predictions with noisy sensor observations weighted by their respective process noise () and measurement noise () covariances, the block estimates hidden states (e.g. position and velocity) while tracking estimation uncertainty (). In the Property Editor, all matrices and state vectors (a, b, c, q, xInit, pInit) are edited via visual matrix and 2-state vector grids organized into three clear parameter groups (_Timing & Model Type_, _State-Space Model_, and _Noise & Initial Covariance_).
Video tutorial
Mathematical Model
The filter estimates state from scalar measurement and optional control input .
For continuous models ( with noise spectral density ), the system is discretized over sample time :
At each discrete time step , the filter executes two sequential phases:
1. Time Update (Predict):
2. Measurement Update (Correct):
Predicted measurement , innovation , and innovation variance :
The optimal Kalman gain and updated state estimate are:
Covariance update via Joseph stabilized form:
Filter consistency is monitored via Normalized Innovation Squared: .
Inputs & Outputs
| Direction | ID | Label | Type | 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 |
Parameters
| Parameters | Label | Type | Default | Description |
|---|---|---|---|---|
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 |
Usage Examples
Example 1: Constant-Velocity Position and Speed Tracking from Noisy Radar
Tracking vehicle position and velocity from a noisy position radar (). Setting continuous matrices a = "0 1; 0 0", b = "0; 1", c = "1 0", and process noise q = "0.01 0.1". The filter tracks vehicle velocity accurately with smooth uncertainty bounds while reducing position noise by over .
Example 2: Sensor Dropout Resilient Gyroscope Attitude Estimation
Estimating angular position during intermittent sensor telemetry loss. Routing an IMU angle sensor to port z and sensor status flag to port Dropout. When communication drops (Dropout > 0.5), the filter skips the measurement update and relies on model prediction; estimation uncertainty expands smoothly to reflect missing data until communication resumes.
Remarks & Best Practices
- Visual Matrix & Vector Grids: Instead of typing semicolon-separated text strings, edit each cell of directly in the interactive grid editor.
- NIS Diagnostic Tuning: The Normalized Innovation Squared () should average over time for a statistically consistent filter. Values persistently above indicate overconfidence ( or too small); values below indicate underconfidence.
- Joseph Stabilized Covariance Form: The measurement update calculates covariance via the symmetric Joseph formula, ensuring remains strictly positive semi-definite and preventing numerical divergence.
- Affine Linearization Offset: The
offsetparameter and port enable tracking around non-zero linearization operating points without corrupting the state transition matrix . - Decoupled Simulation Solvers: The discrete Kalman filter advances once per sample time , allowing continuous plants to be integrated by high-order ODE solvers (RK4, TR-BDF2) without solver substep interference.
Frequently Asked Questions (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).
Related Components
- StateSpace: General continuous-time multi-input multi-output state-space plant model.
- RandomNumber: Injects Gaussian white noise into sensor channels to test filter estimation.
- NumericDisplay: Real-time LCD digital scalar readout.
- uPlotDisplay: Multi-channel waveform scope for tracking state estimates and uncertainty bands.