There have been a couple of times over the past few of years where I have had cause to model a noisy value that is changing at a constant rate. One of the standard ways of doing this is by using a Kalman Filter, specifically a one dimensional constant velocity Kalman Filter. Unfortunately whilst this is almost as simple as you can get I’ve never seen the recipe written out. So one every occasion I have to read Greg Welch’s tutorial and knock the rust of my maths crunching through the derivation; it’s not that it’s that hard you just have to remember where you can simplify and assume stuff (and remember useful snippets like covariance matrices are symmetric).

Any how here’s the recipe:

You need to keep track of five numbers, the current estimate of the position x, the current estimate of the velocity v, and the uncertainty of measurements which I’ll call Pxx, Pvv, Pxv (technically these are the variance of x, v and their covariance respectively).

You need to have initial estimates of these five values. An easy way of doing this is to collect a couple of data points at the beginning, set the estimate of x to the second, v to the rate of change between the two and the uncertainty to some arbitrary value (like 1). (Fiddling with how this method will change the filter’s start up behaviour but not the long term effect).

You also need to have three values that stay constant: estimates of the measurement noise, R; and estimates for the noise in the real process for the position and velocity (Nx and Nv respectively). The best way is to get values of these is to get some real data and then run the filter over it at a range of values and pick those that look best. Roughly speaking R tells the filter how much to trust its model against the current estimate. The larger the value the more the model will be trusted so the smoother the predictions, but the slower it will be to update to changes in the process.

When I have a chance to format it I plan to post the derivation for this from the base Kalman equations.

And by way of a demonstration here’s a Python script that you can play with.

from random import normalvariate

##########################################################################
# "Real world" that we're trying to track
class RealWorld:
    def __init__(self):
        self.position = 0.0
        self.velocity = 0.5
        self.time_step = 0.01
        self.time = 0.0
        self.measure = None

        # Noise on the measurements
        self.measurement_variance = 3.0

        # If we want to kink the profile.
        self.change_after = 5
        self.changed_rate = -0.5

    def measure(self):
        if self.measure == None:
            self.measure = (self.position
                            + normalvariate(0, measurement_variance))
        return self.measure

    def step(self):
        self.time += self.time_step
        self.position += self.velocity * self.time_step

        if self.time >= self.change_after:
            self.rate_of_change = self.changed_rate


world = RealWorld()    

##########################################################################
# Model
# Estimates
estimate_position = 0.0
estimate_velocity = 0.0

# Covariance matrix
P_xx = 0.1 # Variance of the temperature
P_xv = 0.1 # Covariance of temperature and rate.
P_vv = 0.1 # Variance of the rate

##########################################################################
# Model parameters
position_process_variance = 0.01
velocity_process_variance = 0.01
R = 30.0 # Measurement noise variance

average_length = 30
data = []

for i in range(10000):
    world.step()
    measurement = world.measurement()

    # We need to boot strap the estimates for temperature and
    # rate
    if i == 0: # First measurement
        estimate_position = measurement        
    elif i == 1: # Second measurement
        estimate_velocity = ( measurement - estimate_position ) / world.time_step
        estimate_position = measurement
    else: # Can apply model
        ##################################################################
        # Temporal update (predictive)
        estimate_position += estimate_velocity * world.time_step

        # Update covariance
        P_xx += world.time_step * ( 2.0 * P_xv + time_step * P_vv )
        P_xv += world.time_step * P_vv
        
        P_xx += world.time_step * position_process_variance
        P_vv += worl.dtime_step * velocity_process_variance
        
        ##################################################################        
        # Observational update (reactive)
        vi = 1.0 / ( P_xx + R )
        kx = P_xx * vi
        kv = P_xv * vi

        estimate_position += (measurement - estimate_position) * kx
        estimate_velocity += (measurement - estimate_position) * kv 

        P_xx *= ( 1 - kx )
        P_xv *= ( 1 - kx )
        P_vv -= kv * P_xv

    print world.time, world.position, measurement, \
          estimate_position, estimate_velocity