K̂

Kalman Filter

Advanced
Run the live example Free · no sign-up · runs in your browser

The 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 (QQ) and measurement noise (RR) covariances, the block estimates hidden states (e.g. position and velocity) while tracking estimation uncertainty (σ=Pii\sigma = \sqrt{P_{ii}}). In the Property Editor, all matrices and state vectors (a, b, c, q, xInit, pInit) are edited via visual 2×22 \times 2 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 x∈R2\mathbf{x} \in \mathbb{R}^2 from scalar measurement z∈Rz \in \mathbb{R} and optional control input u∈Ru \in \mathbb{R}.

For continuous models (x˙=Acx+Bcu+w\dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} + \mathbf{B}_c u + \mathbf{w} with noise spectral density Qc\mathbf{Q}_c), the system is discretized over sample time 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

At each discrete time step kk, the filter executes two sequential phases:

1. Time Update (Predict):

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. Measurement Update (Correct):

Predicted measurement z^k=Cx^k−+Duk+offset\hat{z}_k = \mathbf{C}\hat{\mathbf{x}}_k^- + D u_k + \text{offset}, innovation yk=zk−z^ky_k = z_k - \hat{z}_k, and innovation variance SkS_k:

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

The optimal Kalman gain Kk\mathbf{K}_k and updated state estimate x^k\hat{\mathbf{x}}_k are:

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

Covariance update via Joseph stabilized form:

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

Filter consistency is monitored via Normalized Innovation Squared: NISk=yk2/Sk\text{NIS}_k = y_k^2 / S_k.

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 xx and velocity vv from a noisy position radar (R=4.0 m2R = 4.0\text{ m}^2). 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 2σ2\sigma uncertainty bounds while reducing position noise by over 70%70\%.

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 σ\sigma expands smoothly to reflect missing data until communication resumes.

Remarks & Best Practices

  • Visual 2×22 \times 2 Matrix & Vector Grids: Instead of typing semicolon-separated text strings, edit each cell of A,B,C,Q,x0,P0\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{Q}, \mathbf{x}_0, \mathbf{P}_0 directly in the interactive grid editor.
  • NIS Diagnostic Tuning: The Normalized Innovation Squared (NIS=y2/S\text{NIS} = y^2 / S) should average 1.01.0 over time for a statistically consistent filter. Values persistently above 1.01.0 indicate overconfidence (QQ or RR too small); values below 1.01.0 indicate underconfidence.
  • Joseph Stabilized Covariance Form: The measurement update calculates covariance via the symmetric Joseph formula, ensuring P\mathbf{P} remains strictly positive semi-definite and preventing numerical divergence.
  • Affine Linearization Offset: The offset parameter and port enable tracking around non-zero linearization operating points without corrupting the state transition matrix A\mathbf{A}.
  • Decoupled Simulation Solvers: The discrete Kalman filter advances once per sample time TsT_s, 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?

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).

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 2σ2\sigma uncertainty bands.