Kalman Filter (KF)\n\n← f_h Functions\n\nDerivation, argument reference, and worked example for the kf block.
Overview
The Kalman filter is the optimal linear estimator for discrete-time systems with Gaussian noise. Given noisy measurements \(y_k\) and a known system model, it computes the minimum mean-square-error estimate \(\hat{x}_k\) of the true state \(x_k\).
Each time step it alternates between two steps:
Predict — project the current estimate forward using the system model
Update — correct the prediction with the new measurement
State-Space Model
The KF assumes the system evolves as:
Symbol |
Space |
Role |
|---|---|---|
\(x_k\) |
\(\mathbb{R}^n\) |
True (hidden) state |
\(u_k\) |
\(\mathbb{R}\) |
Known control input |
\(y_k\) |
\(\mathbb{R}^m\) |
Observed measurement |
\(w_k\) |
\(\mathbb{R}^n\) |
Process noise, covariance \(Q_k\) |
\(v_k\) |
\(\mathbb{R}^m\) |
Measurement noise, covariance \(R_k\) |
Predict Step
Starting from the previous estimate \(\hat{x}_{k-1}\) and covariance \(P_{k-1}\), propagate forward:
The covariance grows because of added process noise \(Q_k\) — the filter becomes less certain the further it predicts without a measurement.
Update Step
Given a new measurement \(y_k\), compute the innovation and correct the prediction:
The gain \(K_k\) balances trust between the model and the sensor:
Small \(R_k\) (accurate sensor) → large \(K_k\) → measurement dominates
Small \(Q_k\) (accurate model) → small \(K_k\) → prediction dominates
Argument Reference
zk — KF state tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]]
The internal state of the KF block. A (x̂, P) pair:
Field |
Symbol |
Shape |
Description |
|---|---|---|---|
|
\(\hat{x}_k\) |
|
Current state estimate |
|
\(P_k\) |
|
Error covariance — symmetric, positive semi-definite |
Initialisation: choose x0 as your best prior guess for the state and P0 to reflect initial uncertainty. A common default:
zk = (np.zeros(n), np.eye(n))
A large P0 (e.g. np.eye(n) * 100) tells the filter it starts with low confidence and should weight early measurements heavily.
uk — control input float
The scalar control input applied at step \(k\). Enters the predict step as \(B_k u_k\).
If your system has no control input, pass uk=0.0.
yk — measurement Float[np.ndarray, "m"]
The raw sensor reading at step \(k\), shape (m,). Must live in the same space as \(H_k x_k\).
If your sensor returns a scalar, wrap it:
yk = np.array([reading])
Fk — state transition matrix Float[np.ndarray, "n n"]
Shape (n, n). Encodes how the state evolves over one time step without control or noise:
Example — constant-velocity model (x = [position, velocity], timestep dt):
Fk = np.array([[1.0, dt],
[0.0, 1.0]])
Bk — control input matrix Float[np.ndarray, "n"]
Shape (n,). Maps the scalar control input \(u_k\) into the state space:
Example — acceleration input acting on velocity only:
Bk = np.array([0.0, dt]) # u_k is acceleration; only velocity component is driven
Hk — observation matrix Float[np.ndarray, "m n"]
Shape (m, n). Maps the state to measurement space — defines what the sensor observes:
Example — observe velocity only from x = [position, velocity]:
Hk = np.array([[0.0, 1.0]]) # shape (1, 2)
Example — observe both position and velocity:
Hk = np.eye(2) # shape (2, 2)
Qk — process noise covariance Float[np.ndarray, "n n"]
Shape (n, n), symmetric positive semi-definite. Encodes uncertainty in the system model.
Larger entries mean the filter trusts the model less and adapts faster to new measurements.
A diagonal \(Q_k\) is the most common choice — each diagonal entry is the expected variance of the corresponding state component per step:
Qk = np.diag([0.01, 0.1]) # position variance < velocity variance
Rk — measurement noise covariance Float[np.ndarray, "m m"]
Shape (m, m), symmetric positive definite. Encodes sensor noise.
Larger entries mean the filter trusts the sensor less and relies more on the model.
Start with the manufacturer-specified noise variance, or calibrate by collecting stationary measurements:
Rk = np.array([[1.0]]) # scalar observation, variance = 1.0
Implementation
kf_f runs the full predict→update cycle and returns the new KF state \((\hat{x}_k, P_k)\).
kf_h extracts \(\hat{x}_k\) so it can be passed directly to a controller or plotter.
Per DynamicalSystem convention, h is evaluated on the state before f updates it,
so x_est from kf.step(...) is the estimate from the previous step’s update — effectively \(\hat{x}_{k-1}\).
Use zk_next[0] if you need the freshly updated estimate within the same loop iteration.
import numpy as np
from jaxtyping import Float, jaxtyped
from beartype import beartype
from dynamicalnodes import DynamicalSystem
@jaxtyped(typechecker=beartype)
def kf_f(
zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
uk: float,
yk: Float[np.ndarray, "m"],
Fk: Float[np.ndarray, "n n"],
Bk: Float[np.ndarray, "n"],
Hk: Float[np.ndarray, "m n"],
Qk: Float[np.ndarray, "n n"],
Rk: Float[np.ndarray, "m m"],
) -> tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]]:
x, P = zk
x_pred = Fk @ x + Bk * uk
P_pred = Fk @ P @ Fk.T + Qk
S = Hk @ P_pred @ Hk.T + Rk
K = P_pred @ Hk.T @ np.linalg.inv(S)
yk = np.atleast_1d(yk)
x_upd = x_pred + K @ (yk - Hk @ x_pred)
P_upd = (np.eye(len(x)) - K @ Hk) @ P_pred
return (x_upd, P_upd)
@jaxtyped(typechecker=beartype)
def kf_h(
zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
) -> Float[np.ndarray, "n"]:
return zk[0]
kf = DynamicalSystem(f=kf_f, h=kf_h)
Worked Example — Constant-Velocity Model
State x = [position, velocity], scalar control input (acceleration), velocity observed.
dt = 0.1
Fk = np.array([[1.0, dt], [0.0, 1.0]])
Bk = np.array([0.0, dt])
Hk = np.array([[0.0, 1.0]])
Qk = np.diag([0.01, 0.1])
Rk = np.array([[1.0]])
zk = (np.zeros(2), np.eye(2)) # x0=[0,0], P0=I
zk_next, x_est = kf.step(
zk=zk, uk=1.0, yk=np.array([0.12]), Fk=Fk, Bk=Bk, Hk=Hk, Qk=Qk, Rk=Rk
)
print("estimate (pre-update):", x_est) # x̂ carried from zk[0]
print("updated estimate: ", zk_next[0]) # x̂_k after predict+update
print("updated covariance:\n", zk_next[1])
DynamicalSystem → ROSNode
from dynamicalnodes import ROSNode
from dynamicalnodes.ros2py_py2ros import (
ros2py_float64_multiarray,
ros2py_float64,
py2ros_float64_multiarray,
)
from std_msgs.msg import Float64MultiArray, Float64
kf_node = ROSNode(
dynamical_system=kf,
subscribes_to=[
{
"topic": "/measurement",
"msg_type": Float64MultiArray,
"arg": "yk",
"ros2py": ros2py_float64_multiarray,
},
{
"topic": "/control",
"msg_type": Float64,
"arg": "uk",
"ros2py": ros2py_float64,
},
],
publishes_to=[
{
"topic": "/state_estimate",
"msg_type": Float64MultiArray,
"py2ros": py2ros_float64_multiarray,
},
],
)
ROSNode.write_ROSNode_to_rclpy()
dt = 0.1
kf_node.write_ROSNode_to_rclpy(
"kf_node.py",
node_name="kalman_filter",
initial_state=(
np.array([0.0, 0.0]), # x̂_0: [position, velocity]
np.eye(2), # P_0: initial error covariance
),
static_params={
"Fk": np.array([[1.0, dt], [0.0, 1.0]]),
"Bk": np.array([0.0, dt]),
"Hk": np.array([[1.0, 0.0]]),
"Qk": np.diag([0.01, 0.1]),
"Rk": np.array([[1.0]]),
},
initial_inputs={
"yk": np.array([0.0]),
"uk": 0.0,
},
)