The Sensors We Trust (And Why We Shouldn’t)
An accelerometer tells you which way is “down” by measuring gravitational pull. In a perfectly still world, this gives you exact roll and pitch angles. But the moment the device moves — a hand tremor, a vibration from walking, a door closing nearby — the accelerometer’s reading gets contaminated by dynamic acceleration. It becomes jittery and unreliable on short timescales.
A gyroscope measures how fast the device is rotating, in degrees per second. Integrating that rate over time gives you an angle. The result is smooth and responsive to fast motion. But gyroscopes have a fatal flaw: bias drift. A tiny, persistent error in the rate measurement accumulates over time. After a few minutes, the integrated angle has drifted degrees away from reality. After an hour, it’s useless.
The accelerometer is noisy but doesn’t drift. The gyroscope is smooth but drifts relentlessly. Neither sensor alone can produce a reliable angle estimate for a system that needs to run indefinitely at 100 Hz. The Kalman filter in orientation_filter.h solves this by fusing both sensors into a single estimate that is both smooth and drift-free — using a custom matrix library built specifically for this purpose.
What You Need Before Diving Into State Estimation
Knowledge Base:
- C++ classes, operator overloading, and
constexpr if - Basic linear algebra: what a matrix is, matrix multiplication, transpose, and inverse (2x2 is all we need)
- Conceptual familiarity with the idea of “state” in a system (a variable that evolves over time)
Environment:
- C++17 compiler (required for
if constexpr) - No external libraries — the matrix class and the filter are entirely self-contained
Two Sensors, One Truth: The Predict-Correct Feedback Loop
The Kalman filter operates as a two-phase loop that runs once per sensor reading (100 times per second). Think of it as a debate between two unreliable witnesses, mediated by a judge who tracks how much to trust each one.
graph TD
A["Gyroscope Reading<br/>(rate: rad/s)"] --> B["PREDICT Phase<br/>Integrate rate to estimate angle<br/>Increase uncertainty"]
B --> C{"How uncertain<br/>are we?"}
C --> D["Kalman Gain<br/>High uncertainty → trust accelerometer more<br/>Low uncertainty → trust prediction more"]
E["Accelerometer Reading<br/>(angle from gravity)"] --> D
D --> F["CORRECT Phase<br/>Blend prediction with measurement<br/>Decrease uncertainty"]
F --> G["Output: Filtered Angle"]
G --> B
style A fill:#1e40af,color:#fff
style E fill:#065f46,color:#fff
style G fill:#38bdf8,color:#000
The analogy: Imagine you’re walking through fog with two navigation tools. A compass (accelerometer) that gives you a rough bearing but shakes wildly in your hand. And a step counter with a direction sensor (gyroscope) that smoothly tracks your heading but slowly drifts off course. Every second, you check both: you take your step-counter heading (predict), glance at the compass (measure), and adjust your believed heading toward the compass — but only proportionally to how much you trust it right now. If your step counter has been running for a while and probably drifted, you trust the compass more. If you just calibrated, you trust your step counter. The Kalman filter automates exactly this tradeoff.
Building the Filter: From Raw Math to Running Code
Part A: The Matrix Library (matrix.h)
Before we can write the Kalman filter, we need a way to express its math. The filter operates on 2×2 and 2×1 matrices. Rather than pulling in Eigen (a 15,000+ header-file library), the codebase implements a minimal, compile-time-dimensioned matrix class.
The template declaration and storage:
The matrix dimensions are template parameters, meaning the compiler knows the exact size at compile time. Storage is a std::array on the stack — no heap allocation.
template <size_t Rows, size_t Cols>
class Matrix {
public:
// Flat array on the stack. A 2x2 matrix uses 4 doubles = 32 bytes.
std::array<double, Rows * Cols> data;
Matrix() {
data.fill(0.0); // Zero-initialize all elements
}
// Element access via (row, col) syntax
double& operator()(size_t r, size_t c) {
return data[r * Cols + c]; // Row-major layout
}
};The identity matrix factory:
A static_assert prevents calling Identity() on non-square matrices — this is a compile-time error, not a runtime check.
static Matrix Identity() {
static_assert(Rows == Cols, "Identity matrix must be square");
Matrix m;
for (size_t i = 0; i < Rows; ++i) {
m(i, i) = 1.0;
}
return m;
}Compile-time-specialized multiplication:
This is the most important optimization. The Kalman filter multiplies 2×2 by 2×2 and 2×2 by 2×1 matrices hundreds of times per second. The generic triple-nested loop works for any dimensions, but if constexpr lets the compiler select an unrolled specialization at compile time — no runtime branching.
template <size_t OtherCols>
Matrix<Rows, OtherCols> operator*(const Matrix<Cols, OtherCols>& other) const {
Matrix<Rows, OtherCols> res;
if constexpr (Rows == 2 && Cols == 2 && OtherCols == 2) {
// Unrolled 2x2 * 2x2: exactly 4 multiply-accumulate operations
res(0,0) = (*this)(0,0)*other(0,0) + (*this)(0,1)*other(1,0);
res(0,1) = (*this)(0,0)*other(0,1) + (*this)(0,1)*other(1,1);
res(1,0) = (*this)(1,0)*other(0,0) + (*this)(1,1)*other(1,0);
res(1,1) = (*this)(1,0)*other(0,1) + (*this)(1,1)*other(1,1);
}
else if constexpr (Rows == 2 && Cols == 2 && OtherCols == 1) {
// Unrolled 2x2 * 2x1: common in Kalman gain calculation
res(0,0) = (*this)(0,0)*other(0,0) + (*this)(0,1)*other(1,0);
res(1,0) = (*this)(1,0)*other(0,0) + (*this)(1,1)*other(1,0);
}
else {
// General case with cache-friendly i,k,j loop ordering
for (size_t i = 0; i < Rows; ++i)
for (size_t k = 0; k < Cols; ++k) {
double temp = (*this)(i, k);
for (size_t j = 0; j < OtherCols; ++j)
res(i, j) += temp * other(k, j);
}
}
return res;
}🔵 Deep Dive: if constexpr is a C++17 feature. Unlike a regular if, the compiler discards the branches that don’t match at compile time. The generated machine code for a 2×2 multiplication contains zero loop instructions — just eight multiplies and four adds. This is critical when the function is called 100 times per second inside a timing-critical loop.
The 2×2 inverse:
The Kalman filter needs to invert a matrix to compute the gain. For a 2×2 matrix, the closed-form inverse is trivial — no iterative algorithms needed.
Matrix<Rows, Cols> inverse() const {
static_assert(Rows == Cols, "Only square matrices can be inverted");
if constexpr (Rows == 2) {
double det = (*this)(0,0) * (*this)(1,1) - (*this)(0,1) * (*this)(1,0);
if (std::abs(det) < 1e-9) throw std::runtime_error("Matrix is singular");
Matrix<2, 2> res;
res(0,0) = (*this)(1,1) / det;
res(0,1) = -(*this)(0,1) / det;
res(1,0) = -(*this)(1,0) / det;
res(1,1) = (*this)(0,0) / det;
return res;
}
}Part B: The Kalman Filter (orientation_filter.h)
With the matrix library in place, the filter itself is concise. The state vector has two components: the estimated angle and the estimated gyroscope bias.
Initialization — defining uncertainty:
Three tuning parameters control the filter’s behavior. q_angle and q_bias define how much the filter expects the angle and bias to change between samples (process noise). r_measure defines how much noise the filter expects in the accelerometer reading (measurement noise).
class OrientationFilter {
public:
OrientationFilter(double q_angle = 0.001, double q_bias = 0.003, double r_measure = 0.03) {
// State vector: [estimated_angle, estimated_gyro_bias]
x(0, 0) = 0.0; // Start with angle = 0
x(1, 0) = 0.0; // Start with bias = 0
P = Matrix<2, 2>::Identity(); // High initial uncertainty
// Process noise covariance
Q(0, 0) = q_angle; // How much the angle might change unexpectedly
Q(1, 1) = q_bias; // How much the bias might drift
// Measurement noise covariance
R(0, 0) = r_measure; // How noisy the accelerometer is
}🔵 Deep Dive: The ratio between Q and R is what really matters. If r_measure is large relative to q_angle, the filter trusts the gyroscope more and responds slowly to accelerometer changes — smooth but potentially slow to correct drift. If r_measure is small, the filter trusts the accelerometer more and responds quickly — but becomes jittery. The defaults here (0.001, 0.003, 0.03) produce a filter that is responsive yet stable at 100 Hz.
The update cycle — predict then correct:
This single method is called once per sensor sample. It receives the accelerometer-derived angle, the gyroscope rate, and the time delta.
void update(double angle_m, double gyro_rate, double dt) {
// --- PREDICT ---
// State transition: angle changes by (gyro_rate - estimated_bias) * dt
// F = [1, -dt; 0, 1] — the bias is assumed constant between samples
Matrix<2, 2> F = Matrix<2, 2>::Identity();
F(0, 1) = -dt;
// Predicted state: new angle = old angle + (rate - bias) * dt
x(0, 0) += (gyro_rate - x(1, 0)) * dt;
// Predicted covariance: uncertainty grows
P = F * P * F.transpose() + Q;After the prediction step, the filter has a new angle estimate based purely on the gyroscope. But the uncertainty (P) has grown — the filter knows that every prediction step accumulates a little more error.
// --- CORRECT ---
// Observation matrix: we can only measure the angle, not the bias directly
Matrix<1, 2> H;
H(0, 0) = 1.0; // H = [1, 0]
// Innovation: difference between what we measured and what we predicted
double y = angle_m - x(0, 0);
// Innovation covariance: how uncertain is this discrepancy?
double S = (H * P * H.transpose())(0, 0) + R(0, 0);
// Kalman gain: how much to correct based on the measurement
Matrix<2, 1> PHt = P * H.transpose();
Matrix<2, 1> K;
K(0, 0) = PHt(0, 0) / S;
K(1, 0) = PHt(1, 0) / S;
// Corrected state: blend prediction toward measurement
x(0, 0) += K(0, 0) * y; // Adjust angle
x(1, 0) += K(1, 0) * y; // Adjust bias estimate
// Corrected covariance: uncertainty shrinks after incorporating measurement
Matrix<2, 2> I = Matrix<2, 2>::Identity();
P = (I - (K * H)) * P;
}The Kalman gain K is the core of the algorithm. When the prediction is uncertain (large P), K is large, and the measurement pulls the estimate strongly. When the prediction is confident (small P), K is small, and the measurement has little effect. The filter automatically learns the optimal blend ratio at every timestep.
Why This Filter Runs Entirely on the Stack and What That Means at 100 Hz
Every variable in the OrientationFilter — the state vector x, the covariance P, the noise matrices Q and R, and every temporary matrix created inside update() — is stack-allocated. The Matrix class uses std::array, which is a fixed-size container stored inline in the object.
At 100 Hz, the update() method is called 100 times per second, per axis (roll and pitch), for a total of 200 calls per second. Each call creates several temporary Matrix objects: F, H, K, PHt, the identity I, and several intermediate products. Because these are all stack variables, their allocation and deallocation costs are exactly zero — the stack pointer moves by a few hundred bytes and moves back when the function returns. No malloc, no free, no heap fragmentation.
The total memory footprint of one OrientationFilter is:
x(2×1): 16 bytesP(2×2): 32 bytesQ(2×2): 32 bytesR(1×1): 8 bytes- Total: 88 bytes per filter, 176 bytes for roll + pitch
The if constexpr specializations in the matrix multiplication mean that every * operation between 2×2 matrices compiles down to 8 multiplications and 4 additions — no loop overhead, no branch prediction misses. The generated assembly for a full update() call is a straight-line sequence of floating-point instructions that fits entirely within the CPU’s instruction cache.
Singular Matrices, Noise Tuning, and the Drift That Never Stops
What if the covariance becomes singular? The matrix inverse in the gain calculation (S^-1) would fail if S approaches zero. In practice, this cannot happen because R(0,0) (the measurement noise) is always added to S. As long as r_measure > 0, the denominator is always positive. The 1e-9 singularity check in matrix.h is a safety net, not a routine code path.
Tuning gone wrong. Setting r_measure to an extremely small value (e.g., 0.0001) tells the filter to trust the accelerometer almost completely, effectively bypassing the gyroscope. The result is a jittery angle estimate that follows every vibration. Setting it too high (e.g., 10.0) makes the filter sluggish — it will take seconds to converge to the true angle after a rapid tilt. The defaults in the codebase were empirically tuned for the LSM6DSO32 at 100 Hz.
🔴 Danger: The filter estimates gyro bias as a slowly-changing constant. If the bias changes rapidly (e.g., due to temperature shifts or mechanical shock), the filter’s bias estimate will lag behind reality, producing angle errors until it catches up. In safety-critical deployments, periodic recalibration or a bias-rate term in the state vector would be necessary.
No magnetometer, no yaw. This filter only estimates roll and pitch. Yaw (rotation around the vertical axis) cannot be determined from an accelerometer because gravity provides no reference in the horizontal plane. The codebase approximates yaw by integrating the Z-axis gyroscope directly (m_yaw += cur.gz * dt), which drifts over time. A magnetometer would be needed for drift-free yaw.
You Now Know How to Fuse Imperfect Sensors With a Kalman Filter
This tutorial covered the engineering behind real-time sensor fusion: why neither an accelerometer nor a gyroscope is sufficient alone, how the Kalman filter’s predict-correct loop automatically balances their contributions, and how a purpose-built matrix library keeps the entire computation on the stack with zero allocations. The skill you’ve acquired extends beyond IMU sensors — the same 2-state Kalman architecture applies to any domain where you have a noisy measurement and a drifting model: GPS smoothing, financial time-series filtering, temperature estimation, and more.
