K̂

Kalman Filter

Advanced

The Kalman Filter block estimates the hidden state of a linear system from a noisy

measurement. Each sample it predicts where the state should be from the model and the

known input, then corrects that prediction with the measurement, weighting the two by

how much it trusts each: the process noise Q against the measurement noise R.

Describe the system the way you would on paper — continuous-time dx/dt = A·x + B·u,

with Q as a noise density per second — and the block discretises it exactly for its

sample time, noise included. It handles two states and one measurement, which covers

position/velocity tracking, first-order plants with an unknown bias and linearised

battery models. Besides the estimate it outputs its own uncertainty (σ), the gains and

the normalised innovation squared (NIS), the number that tells you whether Q and R are

tuned right. It runs once per sample whatever solver drives the diagram, keeps

predicting when the measurement drops out, and can sit inside a feedback loop.

Inputs. z is the measurement and u the known input, both required to be wired

only where the model needs them. Reset restarts the filter from its initial estimate,

Valid below 0.5 makes it predict without correcting, and both are optional — an

unconnected Valid means the measurement is good, which is what a diagram that

measures every sample wants.

Offset is the constant term of the measurement model, and it is the one port whose

reason to exist is arithmetic rather than a signal. The measurement equation is

z = C·x + offset, and a linearised curve is _affine_: linearising V(SOC) around one

operating point gives V(s) + H·(SOC − s), which is a slope H plus a constant

V(s) − H·s. C cannot carry that constant, because a row of C multiplies the state

and the state is what you are trying to estimate. So the offset normally comes from the

offset parameter — but when it is derived rather than chosen, wire it and it overrides

the parameter. A LookupTable on the operating point and a Gain of the slope feed it

directly, and then moving the operating point moves the offset with it, which typing a

number does not do.

Mathematical Model

\text{Continuous model:}\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{Predict:}\quad \hat{x}^- = A\hat{x} + B u_{k-1},\qquad P^- = A P A^\top + Q \text{Update:}\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

Where:

x̂ is the two-element state estimate and P its covariance
z is the measurement and u the known input, held over each sample (zero-order hold)
A continuous model is discretised exactly (matrix exponential; Van Loan's method for Q),

so changing the sample time does not change the filter; a discrete model is used as given

K is the Kalman gain and S the innovation variance; with one measurement S is a

scalar, so no matrix inverse is needed

The first sample skips the prediction and corrects the initial estimate directly; a

sample with Valid < 0.5 skips the update instead

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

Tracking a Noisy Sine

A slow sine wave plus Gaussian noise (σ = 0.15) feeds a continuous constant-velocity

Kalman filter sampled at 50 Hz (white acceleration noise, density 0.05). The estimate

follows the sine with about a quarter of the measurement's error: RMS 0.040 against 0.150

from 2 s on, measured in the manual's demo engine.

Remarks & Best Practices

σ outputs: x̂₀ ± 2σ₀ is a 95 % band — if the filter's model is right. Check that

with NIS: its average over time is 1 for a consistent filter, clearly above 1 when

the filter is overconfident (Q or R too small), below 1 when it is too cautious.

Valid input: wire a sensor's status to it. Below 0.5 the block only predicts, the

gains are 0 and σ grows until measurements return.

Feedback: the estimate may drive a controller whose output returns as u; the

prediction uses the input of the previous sample, so with d = 0 the loop is not

algebraic (checked against an independent reference to 1e-6).

A malformed matrix (wrong number of entries) falls back to the block's default model

rather than a partly read one. A degenerate innovation variance (S ≤ 0 or not finite)

skips that update instead of producing NaN.

The filter is linear. On a nonlinear sensor curve linearised at one point it converges

to a biased estimate away from that point — see BatterySocEkf, which re-linearises

every sample.

Verified against an independent NumPy/SciPy implementation to 1e-9 and by a Monte-Carlo

NEES/NIS consistency test (scripts/kalman-reference.py).

Reset (> 0) restarts from xInit and pInit.

Related Components

BatterySocEkf" class="bw-link">BatterySocEkf
DiscreteStateSpace" class="bw-link">DiscreteStateSpace
RandomNumber" class="bw-link">RandomNumber