1D filter for speed input and noisy position sensor

Question

I was tasked with designing a filter to smooth out a translating table. The system, we command a speed and a desired position, and measure it with two noisy position sensors. The commanded speed is also not always perfect, but it's usually within 10%. The slide can only move left and right.

My initial thought was to simply use a moving average (via circular buffer), and grow / shrink the array based on the speed. For high speeds, I would have a smaller array (10 or so measurements), and for lower speeds, I would have a large array (maybe 40 measurements). When stopped, I would have it grow to 100 or so. The reason for this is to decrease the phase offset at high speeds where resolution is not as important, and decrease the noise at lower speeds, when accuracy is more important. However, this doesn't really take into account the speed, so I feel like I'm throwing away useful information.

I'm figuring there has to be a better way. Any thoughts on how I would approach this problem more elegantly?

Just so we're on the same page, "high speed" is only about 3 inches per second, and I am sampling at 20Hz (but each time, I get two measurements). There is a ramp up/ ramp down portion of the translation such that whenever the table approaches the desired position, it slows to a crawl of 0.5 inches per second.

Thanks!

Answer 1

Sounds like an Extended Kalman Filter is the ideal candidate.

Bayes++ is a really simple drop-in library for implementing one, and the example here (https://github.com/Exadios/Bayes-/blob/master/PV/PV.cpp implements the question exactly (except it only takes in 1 observation)

Below is the code, in case it gets moved:

/* 
 * Bayes++ the Bayesian Filtering Library 
 * Copyright (c) 2002 Michael Stevens 
 * See accompanying Bayes++.htm for terms and conditions of use. 
 * 
 * $Header: /cvsroot/bayesclasses/Bayes++/PV/PV.cpp,v 1.17.2.3 2005/04/07 06:39:22 mistevens Exp $ 
 */  

/* 
 * Example of using Bayesian Filter Class to solve a simple problem. 
 *  The example implements a Position and Velocity Filter with a Position observation. 
 *  The motion model is the so called IOU Integrated Ornstein-Uhlenbeck Process Ref[1] 
 *    Velocity is Brownian with a trend towards zero proportional to the velocity 
 *    Position is just Velocity integrated. 
 *  This model has a well defined velocity and the mean squared speed is parameterised. Also 
 *  the velocity correlation is parameterised. 
 *   
 * Two implementations are demonstrated 
 *  1) A direct filter 
 *  2) An indirect filter where the filter is preformed on error and state is estimated indirectly 
 * Reference 
 * [1] "Bayesian Multiple Target Tracking" Lawrence D Stone, Carl A Barlow, Thomas L Corwin 
 */  

#include "BayesFilter/UDFlt.hpp"  
#include "BayesFilter/filters/indirect.hpp"  
#include "Test/random.hpp"  
#include <cmath>  
#include <iostream>  
#include <boost/numeric/ublas/io.hpp>  

namespace  
{  
    using namespace Bayesian_filter;  
    using namespace Bayesian_filter_matrix;  

    // Choose Filtering Scheme to use  
    typedef UD_scheme FilterScheme;  

    // Square   
    template <class scalar>  
    inline scalar sqr(scalar x)  
    {  
        return x*x;  
    }  

    // Random numbers from Boost  
    Bayesian_filter_test::Boost_random localRng;  

    // Constant Dimensions  
    const unsigned NX = 2;          // Filter State dimension   (Position, Velocity)  

    // Filter Parameters  
    // Prediction parameters for Integrated Ornstein-Uhlembeck Process  
    const Float dt = 0.01;  
    const Float V_NOISE = 0.1;  // Velocity noise, giving mean squared error bound  
    const Float V_GAMMA = 1.;   // Velocity correlation, giving velocity change time constant  
    // Filter's Initial state uncertainty: System state is unknown  
    const Float i_P_NOISE = 1000.;  
    const Float i_V_NOISE = 10.;  
    // Noise on observing system state  
    const Float OBS_INTERVAL = 0.10;  
    const Float OBS_NOISE = 0.001;  

}//namespace  

/* 
 * Prediction model 
 * Linear state predict model 
 */  
class PVpredict : public Linear_predict_model  
{  
public:  
    PVpredict();  
};  

PVpredict::PVpredict() : Linear_predict_model(NX, 1)  
{  
    // Position Velocity dependance  
    const Float Fvv = exp(-dt*V_GAMMA);  
    Fx(0,0) = 1.;  
    Fx(0,1) = dt;  
    Fx(1,0) = 0.;  
    Fx(1,1) = Fvv;  
    // Setup constant noise model: G is identity  
    q[0] = dt*sqr((1-Fvv)*V_NOISE);  
    G(0,0) = 0.;  
    G(1,0) = 1.;  
}  


/* 
 * Position Observation model 
 * Linear observation is addative uncorrelated model 
 */  
class PVobserve : public Linrz_uncorrelated_observe_model  
{  
    mutable Vec z_pred;  
public:  
    PVobserve ();  
    const Vec& h(const Vec& x) const  
    {  
        z_pred[0] = x[0];  
        return z_pred;  
    };  
};  

PVobserve::PVobserve () :  
    Linrz_uncorrelated_observe_model(NX,1), z_pred(1)  
{  
    // Linear model  
    Hx(0,0) = 1;  
    Hx(0,1) = 0.;  
    // Observation Noise variance  
    Zv[0] = sqr(OBS_NOISE);  
}  


void initialise (Kalman_state_filter& kf, const Vec& initState)  
/* 
 * Initialise Kalman filter with an initial guess for the system state and fixed covariance 
 */  
{  
    // Initialise state guess and covarince  
    kf.X.clear();  
    kf.X(0,0) = sqr(i_P_NOISE);  
    kf.X(1,1) = sqr(i_V_NOISE);  

    kf.init_kalman (initState, kf.X);  
}  


int main()  
{  
    // global setup  
    std::cout.flags(std::ios::scientific); std::cout.precision(6);  

    // Setup the test filters  
    Vec x_true (NX);  

    // True State to be observed  
    x_true[0] = 1000.;  // Position  
    x_true[1] = 1.0;    // Velocity  

    std::cout << "Position Velocity" << std::endl;  
    std::cout << "True Initial  " << x_true << std::endl;  

    // Construct Prediction and Observation model and filter  
    // Give the filter an initial guess of the system state  
    PVpredict linearPredict;  
    PVobserve linearObserve;  
    Vec x_guess(NX);  
    x_guess[0] = 900.;  
    x_guess[1] = 1.5;  
    std::cout << "Guess Initial " << x_guess << std::endl;  

    // f1 Direct filter construct and initialize with initial state guess  
    FilterScheme f1(NX,NX);  
    initialise (f1, x_guess);  

    // f2 Indirect filter construct and Initialize with initial state guess  
    FilterScheme error_filter(NX,NX);  
    Indirect_kalman_filter<FilterScheme> f2(error_filter);  
    initialise (f2, x_guess);  


    // Iterate the filter with test observations  
    Vec u(1), z_true(1), z(1);  
    Float time = 0.; Float obs_time = 0.;  
    for (unsigned i = 0; i < 100; ++i)  
    {  
        // Predict true state using Normally distributed acceleration  
        // This is a Guassian  
        x_true = linearPredict.f(x_true);  
        localRng.normal (u);        // normally distributed mean 0., stdDev for stationary IOU  
        x_true[1] += u[0]* sqr(V_NOISE) / (2*V_GAMMA);  

        // Predict filter with known pertubation  
        f1.predict (linearPredict);  
        f2.predict (linearPredict);  
        time += dt;  

        // Observation time  
        if (obs_time <= time)  
        {  
            // True Observation  
            z_true[0] = x_true[0];  

            // Observation with addative noise  
            localRng.normal (z, z_true[0], OBS_NOISE);  // normally distributed mean z_true[0], stdDev OBS_NOISE.  

            // Filter observation  
            f1.observe (linearObserve, z);  
            f2.observe (linearObserve, z);  

            obs_time += OBS_INTERVAL;  
        }  
    }  

    // Update the filter to state and covariance are available  
    f1.update ();  
    f2.update ();  

    // Print everything: filter state and covariance  
    std::cout <<"True     " << x_true << std::endl;  
    std::cout <<"Direct   " << f1.x << ',' << f1.X <<std::endl;  
    std::cout <<"Indirect " << f2.x << ',' << f2.X << std::endl;;  
    return 0;  
}