Newton vs Kalman; Estimating Motion

Surprisingly often I'm faced with estimating the position of something that moves according to Newtonian laws of motion in one dimension and with random acceleration.

A fast and pretty accurate way of doing this is to use a Kalman filter. I've finally gotten around to implementing a specialized version of this general method, dedicated to my simple estimations needs. You are welcome to use it (it's HPL licensed).

Have a look at the image if you want to see how it performs in a sample case.

/**
 * Copyright (c) 2010, Anders '4zM' Sundman
 * 
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions 
 * are met:
 * 
 *     * Redistributions of source code must retain the above copyright notice, 
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Using this software in source or binary format, with or without 
 *       modification, to develop, modify or improve products with an intended 
 *       purpose of causing bodily harm to humans, without their consent, 
 *       or of causing grievous bodily harm to any other animal from the phylum 
 *       Chordata (including, but not limited to, all mammals, fishes and birds), 
 *       is not permitted without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS IS" AND 
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * Kalman filter impl. for estimating processes that resemble Newtonian motion with random acceleration:
 * 
 * x_k = F x_k-1 + w_k, where F = [1 1; 0 1] and where w_k is N(0,Q) with Q = [1/4 1/2; 1/2 1] * acc_sig
 * 
 * with a measurement model that is described by (only scalar position measurement):
 *
 * z_k = x_k + u_k, where u_k is N(0,meas_sig)
 * 
 * Create the filter with the standard distribution of the random acceleration and optionally 
 * with an initial position and velocity. Then update the estimate for every measurement with the 
 * update function and access the estimate with the [] op or the current_estimate function.
 */ 
struct newtonian_motion_kalman {
    newtonian_motion_kalman(double acc_sigma) : 
        p11(large_val_init), pxx(0), p22(large_val_init), acc_sigma_sq(acc_sigma * acc_sigma) { 
        x[0] = 0; 
        x[1] = 0; 
    }
    
    newtonian_motion_kalman(const double init_pos[], double acc_sigma) : 
        p11(0.0), pxx(0), p22(0.0), acc_sigma_sq(acc_sigma * acc_sigma) { 
        x[0] = init_pos[0]; 
        x[1] = init_pos[1]; 
    }

    double operator[] (int i) { assert(i >= 0 && i < 2); return x[i]; }

    const double* current_estimate(void) {
        return x;
    }

    void update(double measure, double meas_sigma) {

        // Predict
        double x_pred[2] = { x[0] + x[1], x[1] };

        double p11_pred = p11 + 2*pxx + p22 + 1/4 * acc_sigma_sq;
        double pxx_pred  = pxx + p22 + 1/2 * acc_sigma_sq;
        double p22_pred = p22 + acc_sigma_sq;

        // Update
        double meas_sigma_sq = meas_sigma * meas_sigma;
        double s = p11_pred + 2*pxx_pred + p22_pred + meas_sigma_sq;
        double err = measure - x[0];
        x[0] = x_pred[0] + err/s * p11_pred; 
        x[1] = x_pred[1] + err/s * pxx_pred; 

        p11 = p11_pred * ( 1 - p11_pred/s );
        pxx = pxx_pred * ( 1 - p11_pred/s );
        p22 = p22_pred - pxx*pxx/s;
    }

private:
    double x[2];
    double p11, pxx, p22;
    double acc_sigma_sq;

    static const double large_val_init = 1000.0;
};