Kalman Filter zero to hero, Math to Code.

After struggling through Milestone 1 with mathematical derivations, Now the question is — how does that mathematics become code that a machine actually runs?

Full code available here:

code

From milestone 1 what we have in hand, so those derivations can be used here, first of all list me those things.

Where We Left Off

In Milestone 1 we derived:

F   →   state transition matrix        (12 × 12)
Q   →   process noise covariance       (12 × 12)
H   →   measurement matrix             (3 × 12)
R   →   sensor noise covariance        (3 × 3)

And the full algorithm — predict, update — for both LKF and EKF.

Now we ask: how do we turn all of this into running C code?

We are given two dataset which are:

• 3D Full Body Humain Gait Walking Dataset (Noisy Values)
• 3D Full Body Humain Gait Walking Dataset (True Values)

What further. we have from these datasets,

3D Full Body Human Gait Walking Dataset
→   3,040 frames at 30 fps
→   23 skeletal joints
→   each frame: 69 position values (23 joints × 3 axes)
→   two versions: noisy and ground truth

What this means for our state:

per joint   →   12 states  [px, vx, ax, jx, py, vy, ay, jy, pz, vz, az, jz]
23 joints   →   23 × 12  =  276 total states
measurement →   23 × 3   =  69 total measurements

So our matrices scale up:

F   →   276 × 276
Q   →   276 × 276
H   →   69  × 276
P   →   276 × 276
x̂   →   276 × 1
z   →   69  × 1

What We Have and What We Need First: Matrix Structure

What we have: mathematical definitions of all matrices.

What we need: a way to represent and operate on matrices in C.

C has no built-in matrix type. So we build one.

typedef struct {
    int rows;
    int cols;
    double** data;
} Matrix;

Every matrix in the filter — F, Q, H, R, P, x_hat — is this struct. The data field is a 2D array of doubles allocated on the heap.

This architecture is also available at the end in words.

Take operations from the above equations.

Collecting all the operations that are used in above equations, and make one file that has all these operations.

What we get: Matrixoperations.c — a complete matrix algebra library with:

matrix_create(r, c)        →   allocate a new matrix
matrix_multiply(A, B)      →   A × B
matrix_transpose(A)        →   Aᵀ
matrix_add(A, B)           →   A + B
matrix_inverse_3x3(A)      →   explicit 3×3 inverse (used in EKF)
cholesky_decompose(A)       →   for solving linear systems stably
matrix_set_block(dst, src) →   place a small block inside a large matrix

The most important one for us is matrix_set_block — because F and Q are block-diagonal, we build them by tiling small 4×4 blocks into large 276×276 matrices.

— Next, Next…

What We Have and What We Need: The KalmanFilter Struct

What we have: all matrix variables defined mathematically.

What we need: a single structure that holds everything the filter needs at every step.

typedef struct {
    Matrix* x_hat;   // 276×1  — state estimate
    Matrix* P;       // 276×276 — estimation uncertainty
    Matrix* F;       // 276×276 — state transition
    Matrix* Q;       // 276×276 — process noise
    Matrix* H;       // 69×276  — measurement (LKF only)
    Matrix* R;       // 69×69   — sensor noise
    int n_states;
    int n_meas;
} KalmanFilter;

All of this lives in Kalman2.h — the shared header. Every other file includes it. So in simple this file contains all the initializations of every matrix and every constant and variables that we are going to use throughout the code.

What We Have and What We Need: Building F in Code

What we have: the mathematical Faxis block:

Faxis = | 1   Δt   Δt²/2   Δt³/6  |
        | 0    1     Δt    Δt²/2  |
        | 0    0      1      Δt   |
        | 0    0      0       1   |

What we need: a 276×276 block-diagonal matrix with 69 copies of this block.

What we do in build_F_matrix():

// create one 4×4 block
F_block->data[0][0] = 1.0;
F_block->data[0][1] = dt;
F_block->data[0][2] = dt*dt/2.0;
F_block->data[0][3] = dt*dt*dt/6.0;
F_block->data[1][1] = 1.0;
F_block->data[1][2] = dt;
F_block->data[1][3] = dt*dt/2.0;
F_block->data[2][2] = 1.0;
F_block->data[2][3] = dt;
F_block->data[3][3] = 1.0;

// tile it 69 times (23 joints × 3 axes)
for (int joint = 0; joint < n_joints; joint++)
    for (int axis = 0; axis < 3; axis++)
        matrix_set_block(F_full, F_block,
            joint*12 + axis*4,
            joint*12 + axis*4);

What we get: a 276×276 matrix that is 97% zeros — only 69 small blocks on the diagonal carry data. This is the block-diagonal structure and it is the key to everything fast.

Building Q in Code

Q1D = σ²j ×  | Δt⁶/36   Δt⁵/12   Δt⁴/6   Δt³/6 |
             | Δt⁵/12   Δt⁴/4    Δt³/2   Δt²/2 |
             | Δt⁴/6    Δt³/2    Δt²     Δt    |
             | Δt³/6    Δt²/2    Δt      1     |

What we do in *build_Q_matrix()*: exactly the same tiling pattern as F — build one 4×4 block, scale it by σ²a, tile it 69 times.

Tuned parameter:

σa = 0.01 m/s³

Starting with 0.1 caused 4× worse RMSE. The smaller value means the filter trusts the smooth constant-jerk model more — which is correct for periodic walking gait.

Understand the 23 joints in a body.

We have 23 joints and one state vector is 12 and we have the 12*23 possibilities for all joints. and further we can consider for other matrices and use them as blocks.

Building H and R in Code

For LKF — H matrix:

What we have: H extracts px, py, pz from each joint’s 12-state block.

What we do in *build_H_matrix_lkf()*:

// For each joint j and axis α (0=x, 1=y, 2=z):
H->data[j*3 + α][j*12 + α*4] = 1.0;
// Everything else is zero

One line per joint per axis. 69 ones in a 69×276 matrix.

For LKF — R matrix:

// σr = 0.36 m  (measured from dataset)
R->data[i][i] = 0.36 * 0.36;   // diagonal, one per measurement

For EKF — R matrix (different):

The EKF sensor speaks in spherical coordinates — range, azimuth, elevation — which have completely different noise magnitudes:

// per joint, 3×3 diagonal block
R[m+0][m+0] = 0.55  * 0.55;    // range noise      (meters)
R[m+1][m+1] = 0.022 * 0.022;   // azimuth noise    (radians)
R[m+2][m+2] = 0.012 * 0.012;   // elevation noise  (radians)

This was the most critical bug found — using a uniform σ=0.05 for all three spherical components made the EKF wildly miscalibrated. Range noise is 11× larger than the original assumption. Once fixed, RMSE dropped from over 1 meter to under 0.2 meters.

Initialization

What we have: mathematical initialization:

x̂₀|₀  =  initial guess
P₀|₀   =  large uncertainty

What we do:

// P₀ = 100 × I  (large = we are very uncertain at start)
for (int i = 0; i < TOTAL_STATES; i++)
    kf->P->data[i][i] = 100.0;

// x̂₀ seeded from first frame measurement
kf->x_hat->data[j*12 + 0][0] = first_frame_px;
kf->x_hat->data[j*12 + 4][0] = first_frame_py;
kf->x_hat->data[j*12 + 8][0] = first_frame_pz;
// velocity, acceleration, jerk start at zero

Seeding from the first frame is important for EKF — starting at the origin causes division by zero in the Jacobian because r=0 and ρ=0.

LKF Predict in Code

What we have mathematically:

x̂n|n-1 = F · x̂n-1|n-1
Pn|n-1 = F · Pn-1|n-1 · Fᵀ + Q

The naive approach and its problem:

Multiplying full 276×276 matrices takes 27⁶³ = 21 million operations per frame. Over 3,040 frames that becomes slow.

What we actually do — exploit block-diagonal structure:

Since F is block-diagonal, the full matrix multiplication decomposes into 23 independent 12×12 operations per joint — on the stack, no heap allocation:

for (int j = 0; j < N_JOINTS; j++) {
    // extract 12-state slice for this joint
    // multiply: x_new = Fj × xj   (12×12 × 12×1)
    // multiply: Pnew = Fj×Pj×Fjᵀ + Qj  (12×12 operations)
    // write back
}

What we get:

Original naive approach:   25 seconds for 3,040 frames
Block-diagonal approach:    1.7 seconds
Speedup:                   15×
Zero heap allocations in the hot path

LKF Update in Code

What we have mathematically:

Kn = Pn|n-1 · Hᵀ · (H · Pn|n-1 · Hᵀ + R)⁻¹
x̂n|n = x̂n|n-1 + Kn · (zn - H·x̂n|n-1)
Pn|n = (I-KH)·Pn|n-1·(I-KH)ᵀ + K·R·Kᵀ

The key insight: H has exactly one non-zero per row. So H·P·Hᵀ produces a block-diagonal 69×69 matrix — 23 independent 3×3 blocks. Each 3×3 block is inverted analytically (explicit formula, no Cholesky needed):

// For each joint — all on stack, 12×3 arrays
// 1. compute innovation:  y = z - H·x̂  (direct index lookup)
// 2. compute S = Pj[pos,pos] + Rj       (3×3)
// 3. invert S analytically              (3×3 explicit inverse)
// 4. compute K = Pj·Hᵀ·S⁻¹            (12×3)
// 5. update x̂ += K·y
// 6. update P using Joseph form

The Joseph form covariance update:

Pn|n = (I - K·H) · Pn|n-1 · (I - K·H)ᵀ + K·R·Kᵀ

Is used instead of the simpler (I-KH)·P because it keeps P symmetric and positive definite even when floating point errors accumulate over thousands of frames.

The EKF Problem: Converting Measurements

What we have: raw measurements in Cartesian coordinates from the dataset.

What EKF needs: measurements in spherical coordinates.

What we do in *cartesian_to_spherical()*:

*r     = my_sqrt(px*px + py*py + pz*pz);
*theta = my_arctan2(py, px);
*phi   = my_arctan2(pz, my_sqrt(px*px + py*py));

Notice my_sqrt and my_arctan2 — these are hand-implemented using Newton-Raphson (sqrt) and a 15-term Taylor series (arctan). This satisfies the requirement to implement core math functions from scratch.

Before every EKF update step, all 69 Cartesian measurements are converted to spherical:

Matrix* z_sph = measurements_to_spherical(data->frames[frame]);
ekf_predict(kf);
ekf_update(kf, z_sph);

EKF Update: The Jacobian in Code

What we have mathematically:

         | px/r       0 0 0   py/r      0 0 0  pz/r     0 0 0 |
Hn  =    | -py/ρ²     0 0 0   px/ρ²     0 0 0   0       0 0 0 |
         | -pxpz/r²ρ  0 0 0  -pypz/r²ρ  0 0 0  ρ/r²     0 0 0 |

What we do in compute_jacobian():

double r   = sqrt(px*px + py*py + pz*pz);
double rho = sqrt(px*px + py*py);

// guard against division by zero near origin
if (r   < 1e-6) r   = 1e-6;
if (rho < 1e-6) rho = 1e-6;

// row 0 — range
J[0][0] = px/r;      J[0][4] = py/r;      J[0][8] = pz/r;

// row 1 — azimuth
J[1][0] = -py/(rho*rho);   J[1][4] = px/(rho*rho);

// row 2 — elevation
J[2][0] = -(px*pz)/(r*r*rho);
J[2][4] = -(py*pz)/(r*r*rho);
J[2][8] =  rho/(r*r);

All other entries are zero. The Jacobian is recomputed fresh at every time step using the current predicted state.

What we get: Hn — the local linear approximation of the nonlinear sensor function. It replaces the constant H in all update equations.

The EKF innovation uses the actual nonlinear function:

// NOT:  y = z - H·x̂     (that is LKF)
// YES:  y = z - h(x̂)    (actual nonlinear conversion)
y[0] = z_r     - r_predicted;
y[1] = z_theta - theta_predicted;
y[2] = z_phi   - phi_predicted;

// angle wrapping — critical for correctness
while (y[1] >  M_PI) y[1] -= 2*M_PI;
while (y[1] < -M_PI) y[1] += 2*M_PI;

Angle wrapping prevents the innovation from jumping ±2π when the object crosses the ±180° azimuth boundary.

The Main Pipeline

The Main Pipeline

What we have: all pieces built.

What the main loop does for each frame:

LKF pipeline:
─────────────────────────────────────────────
for each frame:
    lkf_predict(kf)              →  F·x̂ and F·P·Fᵀ+Q
    lkf_update(kf, z_cartesian)  →  K, innovation, x̂, P
    save estimate to CSV

EKF pipeline:
─────────────────────────────────────────────
for each frame:
    z_sph = cartesian_to_spherical(z)   →  convert measurement
    ekf_predict(kf)                     →  same as LKF predict
    ekf_update(kf, z_sph)               →  Jacobian, K, innovation, x̂, P
    save estimate to CSV

Output CSVs have 277 columns: one frame index + 276 state values per frame. State order per joint: px, vx, ax, jx, py, vy, ay, jy, pz, vz, az, jz.

Error Analysis

What we have: LKF estimates, EKF estimates, ground truth.

What we compute:

RMSE = √( (1/N) × Σ (true_position - estimated_position)² )

What we get:

Method       x-RMSE    y-RMSE    z-RMSE    3D Total
──────────────────────────────────────────────────────
Noisy input    —         —         —        0.627 m
LKF          0.159     0.104     0.020     0.190 m
EKF          0.155     0.106     0.025     0.189 m

The Three Bugs That Were Fixed

These are worth knowing because they show how sensitive the filter is to calibration:

Bug 1 — EKF R matrix (most critical):
    Wrong:  uniform σ=0.05 for range, azimuth, elevation
    Fixed:  σr=0.55m, σθ=0.022rad, σϕ=0.012rad per component
    Impact: RMSE dropped from >1m to 0.189m

Bug 2 — Process noise σa:
    Wrong:  σa = 0.1 m/s³  (10× too large)
    Fixed:  σa = 0.01 m/s³
    Impact: 0.04m RMSE reduction, smoother trajectories

Bug 3 — Error analysis column indices:
    Wrong:  comparing wrong CSV columns
    Fixed:  axis offsets {0,4,8} within each 12-state block
    Impact: error numbers were meaningless before fix

Software Architecture — What File Does What

Kalman2.h           →   all structs, constants, prototypes
                        Matrix, KalmanFilter, MotionData
                        N_JOINTS=23, STATES_PER_JOINT=12

Matrixoperations.c  →   complete matrix algebra library
                        create, multiply, transpose, inverse,
                        Cholesky, set_block, get_block

Test_matho.c        →   CSV file I/O
                        my_sqrt (Newton-Raphson)
                        my_arctan2 (Taylor series)
                        cartesian_to_spherical

Lkf.c               →   build_F_matrix, build_Q_matrix
                        build_H_matrix_lkf, build_R_matrix
                        lkf_predict, lkf_update

Ekf.c               →   compute_jacobian
                        ekf_predict (identical to LKF)
                        ekf_update (Jacobian + nonlinear innovation)

main.c              →   run_lkf_pipeline, run_ekf_pipeline
                        calculate_errors
                        main()

The Complete Chain — Math to Code to Result

MATH                    CODE                        RESULT
────────────────────────────────────────────────────────────────
State vector x          KalmanFilter.x_hat          276×1 estimate
F matrix                build_F_matrix()            276×276 block-diag
Q matrix                build_Q_matrix()            276×276 block-diag
H matrix                build_H_matrix_lkf()        69×276 sparse
R matrix                build_R_matrix()            69×69 diagonal
P matrix                init as 100×I               276×276 covariance

Predict step            lkf_predict() / ekf_predict()
  x̂ = F·x̂            →  per-joint 12×12 stack ops  →  predicted state
  P = FPFᵀ+Q          →  per-joint 12×12 stack ops  →  grown uncertainty

Update step LKF         lkf_update()
  z - H·x̂             →  direct index lookup        →  innovation
  K = PHᵀ(HPHᵀ+R)⁻¹  →  3×3 analytic inverse       →  Kalman gain
  x̂ += K·ν            →  12×3 stack multiply        →  corrected state
  Joseph form P        →  12×12 stack ops            →  shrunk uncertainty

Update step EKF         ekf_update()
  h(x̂)                →  cartesian_to_spherical()   →  predicted meas
  z - h(x̂)            →  spherical subtraction      →  innovation
  Hn = ∂h/∂x          →  compute_jacobian()         →  3×12 Jacobian
  K = PHnᵀ(HnPHnᵀ+R)⁻¹ → 3×3 analytic inverse      →  Kalman gain
  x̂ += K·ν            →  12×3 stack multiply        →  corrected state
  Joseph form P        →  12×12 stack ops            →  shrunk uncertainty

Error analysis          calculate_errors()
  RMSE per axis        →  compare with ground truth  →  0.190m (LKF)
                                                         0.189m (EKF)

As above is quite an intricate while full understanding so try to understand the maths behind it, while keeping the logic in mind if code doesn’t make sense on some places.

Converting pure math to C code in milestone 2 has come to end, and above fully understandable implementation is available on github account, you can follow the below given link.

What Remains

The math is implemented. The filters run. The results validate.

What comes next is assembly language — implementing the same predict and update steps in RISC-V assembly to go even deeper into how the machine executes these matrix operations at the instruction level.

For the full C source code, visit: Link.

If you like this article while reading clap for this article, reshare and comment to make it even better and simpler so some difficult spot can eliminated.

we finished the second milestone, congratulations.

bye bye thank you for reading.

clap and comment.