When You Only Measure Angle: Building a Kalman Filter That Reconstructs Velocity
I built an Extended Kalman Filter to stabilize an inverted pendulum using only a noisy angle sensor. No velocity encoder. The filter has to figure out angular velocity by integrating what it learns from repeated measurements. This post walks through why that works, why most implementations get it wrong, and what the math actually looks like when you stop approximating.
The Setup
An inverted pendulum is unstable. Gravity pulls the rod downward. A small angular deviation grows exponentially unless you apply corrective torque at the pivot. In the real world:
- You have a torque actuator (limited: ±15 Nm)
- You have an angle sensor (noisy, reads at 10 Hz)
- You need to decide what torque to apply, right now, based on imperfect information
- A simple feedback controller could use position error:
u = -Kp * θ - Kd * θ̇ - But you don’t have θ̇. You have to estimate it.
That’s where the Kalman filter comes in.
Why Not Just Differentiate Measurements?
The naive approach: measure angle at times t and t+1, compute (θ[t+1] - θ[t]) / dt, call that velocity, feed it to the controller.
This breaks immediately. Noise in the measurements gets amplified by the differentiation. High-frequency sensor noise becomes high-frequency controller commands, which saturate the actuators and destabilize the system. You end up fighting your own noise.
The Kalman filter takes a different tack: instead of trying to extract velocity from pairs of measurements, it builds a predictive model of the full system state. It knows:
– How the pendulum physics evolve over time
– How much process noise (unmodeled friction, air resistance, modeling error) to expect
– How noisy the measurements are
– How to optimally blend predictions with observations
The result is a smooth, continuously-updating estimate of both angle and angular velocity, along with a quantified uncertainty bound for each.
The Math (Without Handwaving)
The EKF operates in two phases every timestep:
Predict
Given the current state estimate x̂[k-1] and its covariance P[k-1], predict the next state using the nonlinear model:
x̂_pred = f(x̂[k-1], u[k])
where f is the pendulum dynamics. For a pendulum with torque input:
θ̇ = ω
ω̇ = (g/L) * sin(θ) - b * ω + u / (m*L²)
The covariance grows due to model uncertainty:
P_pred = F * P[k-1] * F^T + Q
Here’s the critical part: F is the Jacobian of the state-transition model. It tells you how sensitive the next state is to perturbations in the current state:
F = [
[1, dt],
[cos(θ)*dt, 1 - b*dt]
]
Most implementations approximate this numerically (finite differences), which:
1. Requires multiple model evaluations
2. Introduces truncation error
3. Breaks down near singular points
I computed it analytically. It takes one evaluation and is exact. For a research/portfolio demo, that difference signals you understand what you’re doing.
Update
A measurement arrives (in this case, a noisy angle reading z):
innovation = z - H * x̂_pred
where H is the observation matrix (we measure only θ, not ω):
H = [1, 0]
The innovation tells us how far off our prediction was. The Kalman gain decides how much to trust that measurement versus our prediction:
S = H * P_pred * H^T + R
K = P_pred * H^T / S
x̂[k] = x̂_pred + K * innovation
P[k] = (I - K * H) * P_pred
S is the innovation covariance—it combines our uncertainty (P_pred) with measurement noise (R). If we’re very uncertain, we trust the measurement more. If the measurement is very noisy, we trust our model more. The Kalman gain automatically balances this.
Why This Actually Works for Velocity
You’re probably thinking: “We’re only measuring angle. How can we possibly know velocity?”
The answer is in the dynamics. A pendulum at angle θ with no applied torque will move in a predictable way determined by gravity and damping. If we measure θ at time t and θ’ at time t + 0.02s, the controller can infer what ω must have been, because the physics constrain the relationship. The EKF formalizes this: it builds a recursive model of that constraint and updates it with each measurement.
Over time, as measurements accumulate and the filter learns the system’s natural frequencies, the velocity estimate converges to something reliable. The ±2σ confidence bands in the video show this—they start wide and compress as the filter collects evidence.
Implementation Details That Matter
1. Analytical Jacobian (already covered)
2. Actuator saturation – The filter’s prediction model assumes torque can range from -∞ to +∞. In reality, the actuator clips at ±15 Nm. If you ignore this, the filter’s internal model diverges from reality, covariance becomes meaningless, and recovery after disturbances fails. I clip u after computing it but before passing it to f(). This ensures P[k] reflects the actual constraints the system operates under.
3. Low measurement rate – The sensor runs at 10 Hz; the filter predicts at 50 Hz. This means most timesteps have no measurement update. The covariance grows during prediction-only steps. This is realistic (many sensors are slow) and tests whether the filter can extrapolate confidently. You see it in the video: the confidence band widens slightly between measurements, then tightens when one arrives.
4. Covariance tuning – Q (process noise) and R (measurement noise) aren’t physical; they’re tuning knobs. Set Q too high and the filter ignores its own model, chasing every measurement. Too low and it ignores measurements, diverging. I set Q to 4x the actual noise standard deviation, which is conservative but justified: there are always unmodeled dynamics. R is calibrated to the actual sensor noise (0.06 rad standard deviation).
The Disturbance Kick
At t=6s, I inject a +2.2 rad/s impulse into the true system’s angular velocity. The filter sees angle measurements start to deviate from its prediction. Its uncertainty (the ellipse in the phase-space plot) expands, then contracts as it re-converges. This tests whether the filter believes itself less confident when something unexpected happens—i.e., whether uncertainty estimates are honest.
If you build an EKF carelessly, P can become inconsistent: it claims the state is certain but the true error is much larger. Conversely, if P is conservative (overestimating uncertainty), the controller becomes cautious and slow. The sweet spot is: P truthfully represents what you know.
Why This Matters at Scale
This is a 2-state toy problem. Industrial applications have 20+ states. The principles don’t change:
- Sensor fusion in autonomous vehicles: fusing lidar, radar, and odometry to estimate position/velocity/acceleration when each sensor has different noise characteristics and update rates.
- Joint velocity estimation in industrial robots: motors have encoders, but they’re expensive and wear out. Estimating velocity from current draw + encoder readings scales to 6+ DOF arms.
- Adaptive control: if the filter is tracking covariance honestly, a controller can adjust its aggressiveness based on how confident it is. High confidence → aggressive corrections. Low confidence → conservative.
Open Questions (Intentionally)
I kept this focused on a pedagogical demo, but there are extensions:
- Nonlinear uncertainty: The EKF linearizes. For very nonlinear systems, the Unscented Kalman Filter (UKF) or Ensemble Kalman Filter (EnKF) can be better. The EKF is good enough for small perturbations; larger disturbances favor UKF.
- Actuator modeling: I modeled the actuator as a direct torque source. In reality, torque commands go through PID loops, electrical dynamics, friction. Richer actuator models → better EKF performance.
- Adaptive noise covariances: I fixed Q and R. Some systems benefit from adapting them online based on innovation statistics (ROSE filter, etc.).
Code & Reproducibility
The code is publicly available with:
– Full analytical Jacobian derivation in comments
– Configurable noise levels, controller gains, sensor rates
– Instrumentation (prints convergence time, covariance stats)
– Test cases (saturation, measurement dropout, disturbances)
The intent: this isn’t a black box. Every equation is traceable to first principles. Fork it, break it, learn from it.
Wrapping Up
The Kalman filter is the most widely deployed state estimator in the world. GPS/INS fusion, active noise cancellation, weather forecasting—all use variants of this math. The reason it’s so popular: it works and it’s understandable.
What separates a portfolio project that demonstrates competence from one that just looks impressive is how deeply you’ve thought through the details. Analytical Jacobians, actuator constraints, uncertainty propagation, disturbance recovery—these aren’t optional flourishes. They’re the difference between something you could hand to an engineer and something you’re proud to show an interviewer.
This project took ~10 hours to think through, implement, and debug. Every hour was about getting one more detail right. That’s the work.
Tags: #KalmanFilter #StateEstimation #ControlTheory #ExtendedKalmanFilter #EKF #Robotics #Estimation #NonlinearDynamics #ControlSystems #Engineering #Sensor Fusion #Uncertainty Propagation