Kalman Filter — Control Systems/State Estimation

Kalman Filter

Control Systems / State Estimation

Estimates the states of a discrete-time linear plant from a noisy measurement, running the optimal recursive estimator for

x[k+1] = A·x[k] + B·u[k] + G·w[k], y[k] = C·x[k] + D·u[k] + v[k], with cov(w) = Q and cov(v) = R.

Each step forms the innovation covariance S = C·P·C' + R, the gain M = P·C'·S⁻¹, corrects the estimate with the measurement, and propagates both the estimate and its covariance P.

The gain is computed once, not per step. For a time-invariant plant the Riccati recursion has a fixed point, and this block iterates to it when the configuration is loaded and then uses that steady-state gain from the very first sample. That is what Simulink's block does, and it is observable: its first-sample gain corresponds to the steady-state covariance rather than to P[0], and the estimate error decays at exactly A·(1−M·C). A filter that instead ran the covariance transient converges to the same place but disagrees for the first few hundred samples – which is precisely what the Simulink parity run measured before this was corrected.

Ports

• u – the known plant input, a column of m entries (m = B's column count).
• y – the measurement, a column of p entries (p = C's row count).
• xhat (x̂) – the state estimate, a column of n entries (n = A's order).

Parameters

• A, B, C, D – the plant, discrete-time. A is [n,n] and sets the state count; B is [n,m], C is [p,n], D is [p,m].
• G – how process noise enters the state, [n,q]. Defaults to the identity, i.e. noise on every state.
• Q – process-noise covariance, [q,q]. A scalar is taken as that value times the identity.
• R – measurement-noise covariance, [p,p]. A scalar expands the same way. Larger R means the measurement is trusted less and the filter leans on the model.
• Initial State Estimate – x̂[0], [n,1].
• Initial Covariance – P[0], [n,n] or a scalar. It seeds the iteration that finds the steady-state gain. Because that iteration runs to its fixed point, P[0] does not change the running filter – it is kept because Simulink has it and because a badly scaled value can slow the search, not because it shifts the answer.
• Estimator Type – Current publishes x̂[k|k], corrected with the measurement of the SAME step; Delayed publishes x̂[k|k−1], the prediction made before it. Both run the identical recursion – the difference is which estimate reaches the port, and a wrong choice tracks convincingly while sitting one sample out.
• Sampling Time (s) – zero or less inherits the solver's rate; a positive value runs the block at that period.

Code export

All ten targets: Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog and PLC Structured Text. The plant matrices are baked in at export time; the gain is baked in beside them, so the generated core carries no covariance and no matrix solve at all – only innov = y − C·x̂⁻ − D·u, x̂ = x̂⁻ + M·innov and the state propagation.

The three HDL targets are simulation-only, emitted in real arithmetic and quantized only at the port boundary, as Recursive IIR Identification is. With the gain precomputed the remaining arithmetic would in fact sit comfortably in Q16.16; the real form is kept because the gain itself is derived from a covariance spanning many decades, and a core whose constants were rounded to Q16.16 at export would no longer be the filter the tool designed.

Simulink bridge

Import and export, mapped to cstblocks/State Estimation/Kalman Filter – the library is cstblocks, not the "Control System Toolbox" display name, which add_block rejects. A/B/C/D, G, Q, R map by name; "Initial State Estimate" to X0, "Initial Covariance" to P0, "Estimator Type" to UseCurrentEstimator. The rate crosses as Ts, not SampleTime – this block names it differently from the rest of the library, and emitting the standard name would be a hard set_param error.

Notes

• Discrete only, and stateful: the estimate x̂ and the covariance P both persist.
• Carries no state space. The A/B/C/D it holds describe the PLANT it estimates, not this block – whose own behaviour depends on P and therefore is not time-invariant. Model reduction reports it as unmergeable rather than merging the plant's matrices by mistake.