Reading the implementation — follow a minimal 1D version

When you put everything so far into code, a 1-dimensional Kalman filter turns out to be extremely short. What matters is not the line count but being able to trace which formula each line corresponds to.

Minimal Python implementation

The state (x_hat, P) and the parameters (Q, R) are bundled into a class. We avoid using global to mutate module-level variables, because that would make it impossible to run multiple filter instances in parallel and would also make the code harder to test.

class KalmanFilter1D:
    def __init__(self, x_hat=0.0, P=10.0, Q=0.05, R=2.0):
        self.x_hat = x_hat
        self.P = P
        self.Q = Q
        self.R = R

    def step(self, z):
        x_pred = self.x_hat
        P_pred = self.P + self.Q

        K = P_pred / (P_pred + self.R)
        self.x_hat = x_pred + K * (z - x_pred)
        self.P = (1.0 - K) * P_pred
        return self.x_hat, self.P

We write (1.0 - K) rather than (1 - K) to make the arithmetic clearly float, matching the C version's behavior. In Python the result is the same as long as K is a float, but writing 1.0 explicitly keeps the behavior consistent when the code is ported to other languages such as C.

Minimal C implementation

The C version also bundles the state and parameters into a struct, and the function takes a pointer to that struct and updates it in place. No global variables are used.

typedef struct {
    double x_hat;
    double P;
    double Q;
    double R;
} KalmanFilter1D;

double kf_step(KalmanFilter1D *kf, double z) {
    double x_pred = kf->x_hat;
    double P_pred = kf->P + kf->Q;

    double K = P_pred / (P_pred + kf->R);
    kf->x_hat = x_pred + K * (z - x_pred);
    kf->P = (1.0 - K) * P_pred;
    return kf->x_hat;
}

Usage: initialize with KalmanFilter1D kf = { .x_hat = 0.0, .P = 10.0, .Q = 0.05, .R = 2.0 }; and then call kf_step(&kf, z) for every new observation.

Formulas and variables

Points to keep in mind when reading this code

1. First split the two prediction lines from the three update lines.
2. Check what K depends on.
3. Finally, confirm that the variance P is properly updated at the end.