Discrete Kalman Filtering: State Estimation & Sensor Fusion

1. Overview & Concept Definition

An algorithm that produces optimal estimates of unknown dynamic variables that tend to be more accurate than those based on a single measurement alone, by estimating a joint probability distribution over the variables for each timeframe.

2. Engineering & Industrial Significance

Standard estimation filter for missile tracking, autonomous car state estimation, satellite orbit determination, drone attitude heading reference systems (AHRS), and GPS dead reckoning.

3. Physical Working Principle & Transducer Dynamics

Operates in a two-step cycle: 1. **Predict:** The state estimate $\hat{x}$ and error covariance $P$ are projected forward in time using a physical state-transition model $F$ and control input $u$. 2. **Update:** A noisy sensor measurement $z$ is incorporated, weighted by the optimal Kalman Gain $K$. If sensor noise $R$ is large, $K$ is small (trust model); if model uncertainty $Q$ is large, $K$ is large (trust sensor).

// ANSI C: 1D Scalar Kalman Filter Implementation
typedef struct {
    float q; // Process noise covariance
    float r; // Measurement noise covariance
    float x; // State estimate
    float p; // Estimation error covariance
    float k; // Kalman gain
} KalmanFilter1D;

void kalman_init(KalmanFilter1D *kf, float q, float r, float initial_val) {
    kf->q = q;
    kf->r = r;
    kf->x = initial_val;
    kf->p = 1.0f; // Initial covariance
}

float kalman_update(KalmanFilter1D *kf, float measurement) {
    // 1. Predict
    kf->p = kf->p + kf->q;

    // 2. Update
    kf->k = kf->p / (kf->p + kf->r);
    kf->x = kf->x + kf->k * (measurement - kf->x);
    kf->p = (1.0f - kf->k) * kf->p;

    return kf->x;
}

6. Common Engineering Pitfalls & Troubleshooting

7. Applications & Physical Constraints