Kalman Filters Explained by Tracking a Single Radar Target

CFAR tells you where a target is in one frame. It says nothing about whether the target in frame 47 is the same physical object as the target in frame 46. That is the job of a tracker. The simplest useful tracker is a Kalman filter, which I spent the last two weeks implementing on top of the CFAR pipeline. This post is the explanation I needed when I started.

What problem the Kalman filter actually solves

You have a thing moving through space. You have noisy measurements of where it is. You have an imperfect model of how it moves. You want the best estimate of where it actually is right now, and the best prediction of where it will be next.

'Best' here means: minimises the squared error, assuming the noise is Gaussian and your motion model is linear. Those two assumptions are wrong in almost every real system, and the Kalman filter still works disturbingly well.

The state-space model

I model the target as a point with a 4-D state: range, range-rate (radial velocity), Doppler bin, Doppler-rate. At each timestep the state evolves linearly:

x_{k+1} = F x_k + w_k
z_k     = H x_k + v_k

F = [ 1  dt  0   0 ]      H = [ 1  0  0  0 ]
    [ 0   1  0   0 ]          [ 0  0  1  0 ]
    [ 0   0  1  dt ]
    [ 0   0  0   1 ]

`F` says: next range = current range + dt × range-rate; range-rate is constant; same for Doppler. `H` says: I can measure range and Doppler directly, but not their rates. `w_k` is process noise (the world is not really constant-velocity); `v_k` is measurement noise (CFAR detections wobble around the truth).

The two equations that matter

Predict step: project the state forward using the motion model, and grow the uncertainty covariance because you have not measured anything new yet.

x_pred = F x
P_pred = F P F^T + Q

Update step: receive a measurement, compute the Kalman gain (how much to trust the measurement vs the prediction), correct the state, shrink the covariance.

y = z - H x_pred                  // innovation
S = H P_pred H^T + R              // innovation covariance
K = P_pred H^T S^{-1}             // Kalman gain
x = x_pred + K y
P = (I - K H) P_pred

That is the entire filter. Two equations to predict, four to update. Everything else is choosing Q and R, the process and measurement noise covariances.

Choosing Q and R

R you can measure directly: collect a few hundred CFAR detections of a stationary target, compute the sample covariance. Mine came out to about 0.04 m² in range and 0.1 bin² in Doppler.

Q is the harder one. Too small and the filter does not believe the measurements, the state drifts. Too large and the filter overreacts to every detection wobble, defeating the point. I started with Q = R × dt² (a rough heuristic from Bar-Shalom) and tuned by running on recorded data until the residuals looked like white noise.

The implementation

class KalmanTracker {
    Eigen::Matrix<float, 4, 1> x_;
    Eigen::Matrix<float, 4, 4> P_;
    Eigen::Matrix<float, 4, 4> F_;
    Eigen::Matrix<float, 4, 4> Q_;
    Eigen::Matrix<float, 2, 4> H_;
    Eigen::Matrix<float, 2, 2> R_;

public:
    void predict(float dt) {
        F_(0, 1) = dt;
        F_(2, 3) = dt;
        x_ = F_ * x_;
        P_ = F_ * P_ * F_.transpose() + Q_;
    }

    void update(const Eigen::Vector2f& z) {
        auto y = z - H_ * x_;
        auto S = H_ * P_ * H_.transpose() + R_;
        auto K = P_ * H_.transpose() * S.inverse();
        x_ += K * y;
        P_ = (Eigen::Matrix4f::Identity() - K * H_) * P_;
    }
};

Sixty lines including the headers. I used Eigen for the matrix math because writing a 4×4 matrix inverse by hand is a bug source and Eigen's is faster than anything I would write.

Data association is the hard part

A single-target Kalman filter is trivial. The hard problem is: this frame has three CFAR detections, I have two existing tracks; which detection updates which track, and is the third a new target or a false alarm?

The simplest answer is nearest-neighbour gating: for each track, find the detection within the predicted measurement gate (an ellipse defined by S, the innovation covariance) closest in Mahalanobis distance, and assign it. Detections with no track within gate become candidate new tracks; tracks with no detection for N frames die.

Better answers exist (Joint Probabilistic Data Association, Multi-Hypothesis Tracking) and are required when targets cross paths. For my five-target test scene, nearest-neighbour gating with a 9.21 (chi-squared 99% for 2-DoF) gate works.

What it looks like

Before tracking, the dashboard showed five jittery dots, each jumping a few bins frame to frame as the CFAR detection landed on slightly different cells. After tracking, the dots glide. The track IDs persist across the borderline T5 target's dropouts, because even when CFAR fails to detect it for two or three frames the predict step keeps the estimate alive until a measurement returns.

Mistakes I made

Forgot to update `F` with the actual dt between frames. The filter assumed dt = 1 forever. State drifted, I blamed the measurement noise for two days.

Used double precision for state, single for measurements. The implicit conversions in Eigen worked but the gain matrix had ugly numerics. Pick one.

Initialised P to the identity matrix. The first few frames had wildly wrong gains. Initialising P to a large diagonal (10× R) means the filter trusts the first measurement, which is what you want.

The Kalman filter is what you get when you ask: given a model that is wrong and measurements that are noisy, what is the least-bad estimate I can write down? The answer is two matrix equations and a willingness to tune two covariances.