# 1D filter for speed input and noisy position sensor

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!

• Have you considered a more sophisticated filter such as Kalman? What do you know about the spectral characteristics of the signals? – Moti Apr 1 '15 at 4:51
• @Moti Yes, Kalman was the first thing to came to mind, but I'm not overly familiar with it. I thought Kalman filters only considered observations, and did not take in any other inputs, such as predictions. Is that the difference between a Kalman filter and the Extended Kalman Filter? – dberm22 Apr 1 '15 at 12:29
• I don't recall the differences between the two, but actually they also may allow you some prediction. – Moti Apr 2 '15 at 11:48

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];

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;
}