kalman-filter

name	kalman-filter
description	Kalman filter for mechanical engineering — linear KF derivation, process/measurement noise covariance Q/R, state estimation, Extended KF (EKF), Unscented KF (UKF), sensor fusion (IMU + GPS + encoder), vibration state estimation, structural health monitoring, MATLAB/Python implementation.
metadata	{"priority":7,"promptSignals":{"phrases":["Kalman filter","state estimation","sensor fusion Kalman","EKF extended Kalman","Kalman navigation","Kalman vibration"],"minScore":3}}

Kalman Filter — Complete Skill

Linear Kalman Filter

State Space Formulation

Discrete linear system: x_k = A × x_(k-1) + B × u_(k-1) + w_(k-1) [state transition; w = process noise] z_k = H × x_k + v_k [measurement equation; v = measurement noise]

Notation: x_k ∈ ℝⁿ: state vector (n states) z_k ∈ ℝᵐ: measurement vector (m measurements) A ∈ ℝⁿˣⁿ: state transition matrix B ∈ ℝⁿˣˡ: control input matrix (l inputs) H ∈ ℝᵐˣⁿ: measurement matrix (maps state to measurement space) u_(k-1) ∈ ℝˡ: control input vector w_k ~ N(0, Q): process noise; Gaussian, zero-mean; covariance Q ∈ ℝⁿˣⁿ v_k ~ N(0, R): measurement noise; Gaussian, zero-mean; covariance R ∈ ℝᵐˣᵐ

Algorithm (Two-Step: Predict + Update)

Predict step: x̂_k⁻ = A × x̂_(k-1) + B × u_(k-1) [a priori state estimate] P_k⁻ = A × P_(k-1) × Aᵀ + Q [a priori error covariance]

Update step (measurement arrives): K_k = P_k⁻ × Hᵀ × (H × P_k⁻ × Hᵀ + R)⁻¹ [Kalman gain matrix; K ∈ ℝⁿˣᵐ] x̂_k = x̂_k⁻ + K_k × (z_k - H × x̂_k⁻) [a posteriori state estimate; innovation = z_k - H × x̂_k⁻] P_k = (I - K_k × H) × P_k⁻ [a posteriori covariance; Joseph form more numerically stable]

Joseph form (numerically stable): P_k = (I - K × H) × P_k⁻ × (I - K × H)ᵀ + K × R × Kᵀ

Optimality condition: K chosen to minimize trace(P_k) (minimum mean-square error) Convergence: P_k → steady state P_ss as k → ∞ (for time-invariant systems); solve DARE (Discrete Algebraic Riccati Equation)

Covariance Matrix Design

Process Noise Q

Physical meaning: uncertainty in the model (unmodeled dynamics, disturbances) Larger Q → filter trusts measurements more → faster response, less smooth Smaller Q → filter trusts model more → smoother, slower to track changes

Continuous white noise model for position-velocity-acceleration states: Q_continuous = q_d × G × Gᵀ [q_d = power spectral density of noise; G = noise gain matrix]

Discrete-time Q from continuous model (Van Loan method): For constant velocity model [x, ẋ]ᵀ with acceleration noise: Q = q_d × [Δt³/3 Δt²/2; Δt²/2 Δt] [Δt = time step; q_d = acceleration noise variance/s]

Engineering rule for Q: σ_process ≈ expected maximum unmodeled acceleration or disturbance Q_position = (σ_process × Δt²/2)²; Q_velocity = (σ_process × Δt)²

Measurement Noise R

From sensor specification: R = σ_sensor² [scalar for single measurement; diagonal matrix for multiple sensors]

GPS: σ_GPS ≈ 1–5 m (horizontal); 3–10 m (vertical); R_GPS = [σ_x² 0; 0 σ_y²] IMU accelerometer: σ_accel ≈ 0.01–0.1 m/s² (tactical grade); noise density × √(sample_rate) = σ Encoder (angular): σ_encoder = resolution / √12 [uniform distribution] Load cell: σ_load = full scale × accuracy% / 100

Rule of thumb: Increase R → more filtering (trust model); decrease R → more responsive (trust measurement) Ratio Q/R determines steady-state Kalman gain balance

Mechanical Engineering Applications

Position-Velocity State Estimation (Encoder + IMU)

State vector: x = [position, velocity, bias_accel]ᵀ [3 states]

Transition matrix (constant acceleration model, Δt = 0.01 s): A = [1 Δt Δt²/2; 0 1 Δt; 0 0 1]

B (control input — known force/acceleration): B = [Δt²/2; Δt; 0] [integrates known input acceleration]

H (measurement: encoder measures position; IMU measures acceleration): H_encoder = [1 0 0]; H_IMU = [0 0 1] Combined: H = [1 0 0; 0 0 1] [2 measurements]

R: R = diag([σ_encoder², σ_accel²]) = diag([0.001², 0.01²]) [diagonal for independent sensors]

Result: smooth position from noisy encoder; drift-free integration via encoder correction of IMU

Vibration State Estimation (Structural)

Application: estimate full state from limited sensor placement; structural health monitoring

Modal state formulation: ẍ + 2ζωẋ + ω²x = f(t)/m [single DOF; ω = natural frequency; ζ = damping ratio] State: x = [displacement, velocity]ᵀ Continuous A_c = [0 1; -ω² -2ζω]; discretize: A = e^(A_c × Δt)

Accelerometer measurement: z = ẍ = -ω²x - 2ζωẋ + f/m → H = [-ω² -2ζω] + direct feedthrough from u

Benefits: estimate displacement from only acceleration measurement; especially useful when displacement sensors are impractical (bridges, wind turbines, offshore)

IMU + GPS Sensor Fusion (Navigation)

INS/GPS integration (loosely coupled): State: x = [pos_N, pos_E, pos_D, vel_N, vel_E, vel_D, roll, pitch, yaw, b_a, b_g]ᵀ [15 states] b_a, b_g = accelerometer and gyroscope biases

Predict: integrate IMU at 100–1000 Hz; grow uncertainty between GPS updates Update: GPS provides position (3–5 m accuracy); correct state and reduce P

Error state formulation (preferred for navigation): Estimate error δx rather than full state x; avoid nonlinearities in rotation representation δx = x_estimated - x_true; update: x = x_IMU + K × (z_GPS - H × x_IMU)

Extended Kalman Filter (EKF)

Nonlinear System

Nonlinear dynamics: x_k = f(x_(k-1), u_(k-1)) + w_(k-1) [f = nonlinear state function] z_k = h(x_k) + v_k [h = nonlinear measurement function]

EKF linearization: Jacobians: F_k = ∂f/∂x |(x̂(k-1)); H_k = ∂h/∂x |_(x̂_k⁻)

EKF predict: x̂_k⁻ = f(x̂_(k-1), u_(k-1)) [propagate nonlinearly] P_k⁻ = F_k × P_(k-1) × F_kᵀ + Q [covariance with Jacobian]

EKF update: K_k = P_k⁻ × H_kᵀ × (H_k × P_k⁻ × H_kᵀ + R)⁻¹ x̂_k = x̂_k⁻ + K_k × (z_k - h(x̂_k⁻)) P_k = (I - K_k × H_k) × P_k⁻

EKF limitation: first-order linearization; fails for highly nonlinear systems (large uncertainty or strong nonlinearity) Divergence check: if innovation covariance S_k = H × P⁻ × Hᵀ + R grows unbounded → filter diverged; reinitialize

Application: Robot Arm Joint State

Nonlinear pendulum dynamics: θ̈ = -(g/L) × sin(θ) - b/(mL²) × θ̇ + τ/(mL²) [g, L, b, m = physical parameters; τ = torque] State: x = [θ, θ̇]ᵀ Jacobian F = ∂f/∂x = [0 1; -(g/L)cos(θ) -b/(mL²)] evaluated at x̂

Unscented Kalman Filter (UKF)

Sigma Point Method

Motivation: avoid Jacobian computation (numerical; brittle for complex nonlinear h, f) Better accuracy than EKF for highly nonlinear systems (captures 3rd order moments)

Sigma points (2n+1 points): χ_0 = x̂ [mean] χ_i = x̂ + (√((n+λ)P))i [i = 1 to n; positive direction] χ(n+i) = x̂ - (√((n+λ)P))_i [i = 1 to n; negative direction] λ = α² × (n + κ) - n [tuning: α = 0.001–1.0; κ = 0 or 3-n]

Weights: W_0^m = λ/(n+λ); W_i^m = 1/(2(n+λ)) [mean weights] W_0^c = λ/(n+λ) + (1 - α² + β); W_i^c = 1/(2(n+λ)) [covariance weights; β=2 for Gaussian]

Predict: propagate all sigma points through nonlinear f; reconstruct mean and covariance Update: propagate through h; compute cross-covariance; apply standard Kalman update

UKF advantage over EKF: no Jacobians; more accurate for highly curved nonlinear functions; preferred for attitude estimation, chemical process control

MATLAB/Python Implementation

MATLAB (Control Systems Toolbox)

% State space: A, B, H, Q, R matrices defined
n = size(A,1); m = size(H,1);
x_est = zeros(n,1); P = eye(n); % initialize

for k = 1:N_steps
    % Predict
    x_pred = A*x_est + B*u(k);
    P_pred = A*P*A' + Q;

    % Innovation
    y_innov = z(:,k) - H*x_pred;
    S = H*P_pred*H' + R;

    % Kalman gain
    K = P_pred*H'/S;

    % Update
    x_est = x_pred + K*y_innov;
    P = (eye(n) - K*H)*P_pred*(eye(n) - K*H)' + K*R*K'; % Joseph form

    x_history(:,k) = x_est;
end

Python (filterpy library)

from filterpy.kalman import KalmanFilter
import numpy as np

kf = KalmanFilter(dim_x=n, dim_z=m)
kf.F = A           # state transition
kf.H = H           # measurement matrix
kf.Q = Q           # process noise
kf.R = R           # measurement noise
kf.x = x0          # initial state
kf.P = P0          # initial covariance

for z_k in measurements:
    kf.predict()
    kf.update(z_k)
    x_estimated = kf.x

Tuning Guidelines

Q too small → filter lag: state estimate tracks measurements too slowly → increase Q Q too large → noisy estimate: estimate jumps with each measurement → reduce Q R too small → noisy estimate (trusts sensor too much): → increase R R too large → lagging estimate: → reduce R

Adaptive tuning methods: Innovation covariance adaptive: Q̂ = K × (y_innov × y_innonᵀ - S) × Kᵀ (ALS — Adaptive Least Squares) Sage-Husa: recursive covariance estimation; adjusts Q and R during operation

Filter consistency check: Normalized innovation squared (NIS): NIS = y_innov ᵀ × S⁻¹ × y_innov ~ χ²(m) distribution If NIS consistently > m: filter overconfident (Q too small or R too large) If NIS consistently < m: filter underconfident

Standards and References

Standard/Reference	Scope
Kalman (1960) ASME J. Basic Eng.	Original Kalman filter paper
Julier & Uhlmann (2004) IEEE	UKF derivation
Crassidis & Junkins (2004) Optimal Estimation	Comprehensive reference text
IEEE Std 952-1997	Inertial sensor specification (relates to Q/R design)
DO-229E (RTCA)	GNSS avionics — Kalman integration
ISO 20930	Inertial navigation (references sensor fusion methods)

Output

Provide: state vector definition x (n states; physical meaning), measurement vector z (m sensors; what is measured), A matrix (state transition; derived from physics Δt), H matrix (state-to-measurement mapping), Q matrix diagonal elements [units²] with justification (process noise source), R matrix diagonal elements [units²] from sensor specs (σ_sensor), initial conditions x̂₀ and P₀, filter type (linear KF/EKF/UKF) and justification, Kalman gain K at steady state (if applicable), expected state estimation error σ_x [units] after convergence, NIS consistency check result, MATLAB/Python code snippet, and applicable reference (Kalman 1960, Crassidis 2004, IEEE 952).