Initial commit of code implementing the FOAW velocity estimation technique.

Author: radarsat1

COPYING

@@ -0,0 +1,31 @@
+Copyright (c) 2008, Stephen Sinclair
+All rights reserved.
+
+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.
+
+    * Neither the name of McGill University, nor the Input Devices and
+      Music Interaction Lab, nor the names of its contributors may be
+      used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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
+OWNER 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.

Makefile

@@ -0,0 +1,5 @@
+
+foaw: foaw.o
+
+run: foaw
+	./foaw | ./plot.py

foaw.c

@@ -0,0 +1,203 @@
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+/*
+ * Perform the FOAW velocity estimation routine.
+ * This algorithm is described here:
+ *
+ * Janabi-Sharifi, F.; Hayward, V.; Chen, C.-S.J., "Discrete-time
+ * adaptive windowing for velocity estimation," Control Systems
+ * Technology, IEEE Transactions on , vol.8, no.6, pp.1003-1009, Nov
+ * 2000
+ *
+ * http://www.cim.mcgill.ca/~haptic/pub/FS-VH-CSC-TCST-00.pdf 
+ *
+ * This implementation (C)2008 Stephen Sinclair, IDMIL, McGill
+ * University.  This work is covered by the GPL-compatible version of
+ * the BSD license, please see the following URL for more information:
+ *
+ * http://www.opensource.org/licenses/bsd-license.html
+ *
+ * The exact license is listed in the file COPYING, which you should
+ * have received with this source code.
+ */
+
+#define SR 1000
+#define T (1.0f/SR)
+#define SIZE 1000
+#define NOISE (0.1*T)
+
+float vel[SIZE];
+float pos[SIZE];
+float mpos[SIZE];
+float fdvel[SIZE];
+float foawvel[SIZE];
+float dif[SIZE];
+
+#ifndef min
+#define min(X,Y) ((X) < (Y) ? (X) : (Y))
+#endif
+
+void generate_velocity()
+{
+    int k;
+    for (k=0; k<50; k++)
+        vel[k] = 0;
+    for (; k<150; k++)
+        vel[k] = 1;
+    for (; k<450; k++)
+        vel[k] = 2;
+    for (; k<600; k++)
+        vel[k] = 0;
+    for (; k<700; k++)
+        vel[k] = -1;
+    for (; k<1000; k++)
+        vel[k] = -2;
+}
+
+void integrate_position()
+{
+    int k;
+    pos[0] = 0;
+    for (k=1; k<SIZE; k++)
+        pos[k] = (vel[k]*T+pos[k-1]);
+}
+
+void add_position_noise()
+{
+    int k;
+    for (k=0; k<SIZE; k++)
+        pos[k] += rand()/(float)RAND_MAX * NOISE;
+}
+
+void finite_difference()
+{
+    int k;
+    fdvel[0] = 0;
+    for (k=1; k<SIZE; k++)
+        fdvel[k] = (pos[k]-pos[k-1])/T;
+}
+
+float do_foaw_sample(float *posbuf, int size, int *k,
+                     float current_pos, int best)
+{
+    int i, j, l, bad;
+    float b, ykj;
+    float velocity = 0;
+    float noise_max = NOISE;
+
+    /* circular buffer */
+    *k = (*k+1)%size;
+    posbuf[*k] = current_pos;
+
+    for (i=1; i<size; i++)
+    {
+        if (best)
+        {
+            // best-fit-FOAW
+            b = 0;
+            for (l=0; l<(i+1); l++)
+                b +=  i*posbuf[(*k-l+size)%size]
+                    - 2*posbuf[(*k-l+size)%size]*l;
+            b = b / (T*i*(i+1)*(i+2)/6);
+        }
+        else
+            // end-fit-FOAW
+            b = (posbuf[*k]-posbuf[(*k-i+size)%size]) / (i*T);
+        bad = 0;
+        for (j=1; j<i; j++)
+        {
+            ykj = posbuf[*k]-(b*j*T);
+            if (   (ykj < (posbuf[(*k-j+size)%size]-noise_max))
+                || (ykj > (posbuf[(*k-j+size)%size]+noise_max)))
+            {
+                bad = 1;
+                break;
+            }
+        }
+        if (bad) break;
+        velocity = b;
+    }
+
+    return velocity;
+}
+
+void do_foaw(float *pos, int n, int best)
+{
+    float *posbuf = malloc(sizeof(float)*n);
+    int k, i=0;
+    memset(posbuf, 0, sizeof(float)*n);
+    for (k=0; k<SIZE; k++)
+        foawvel[k] = do_foaw_sample(posbuf, n, &i, pos[k], best);
+    free(posbuf);
+}
+
+void subtract(float *result, float *from, float *what)
+{
+    int k;
+    for (k=0; k<SIZE; k++)
+        result[k] = from[k]-what[k];
+}
+
+void median_position(int n)
+{
+    int k, i, j, off=600;
+    float *buf;
+    buf = malloc(sizeof(float)*2*n);
+    for (k=0; k<SIZE; k++)
+    {
+        int size = min(n,k);
+        int a=n, b=n;
+        buf[n] = pos[k];
+        for (i=1; i<size; i++)
+        {
+            if (pos[k-i] > buf[b]) {
+                buf[b+1] = pos[k-i];
+                b++;
+            }
+            else if (pos[k-i] < buf[a]) {
+                buf[a-1] = pos[k-i];
+                a--;
+            }
+            else if (pos[k-i] > buf[b-1]) {
+                buf[b+1] = buf[b];
+                buf[b] = pos[k-i];
+                b++;
+            }
+            else if (pos[k-i] < buf[a+1]) {
+                buf[a-1] = buf[a];
+                buf[a] = pos[k-i];
+                a--;
+            }
+        }
+        mpos[k] = buf[(b-a)/2+a];
+    }
+    free(buf);
+}
+
+void dump(float *data)
+{
+    int k;
+    for (k=0; k<SIZE; k++)
+        printf("%f\n", data[k]);
+}
+
+int main()
+{
+    generate_velocity();
+    integrate_position();
+    add_position_noise();
+    finite_difference();
+    median_position(3);
+    do_foaw(mpos, 15, 1);
+#if 1
+    dump(foawvel);
+#else
+    subtract(dif, foawvel, vel);
+    dump(dif);
+#endif
+    return 0;
+}

velocity.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+
+from pylab import *
+from scipy.signal import lfilter, butter
+
+# Constants
+sr = 1000.0;
+T = 1/sr;
+r = int(sr/100);
+noise_max = 0.1*T;  # This is ||e_k||inf
+
+# Define a velocity curve
+vel = array([0.]*(15*r) + [1.]*(4*r) + [2.]*(25*r) + [0.]*(5*r)
+            + [-1.]*(3*r) + [-1.]*(20*r))
+time = arange(len(vel))/float(sr);
+
+# Integrate it to get position
+pos = lfilter([1], [1,-1], vel)*T;
+
+# Add some noise
+pos = pos + rand(len(pos))*noise_max
+
+# Finite difference
+fdvel = lfilter([1,-1],[1],pos)/T
+
+# Butterworth 100 Hz
+[B,A] = butter(1, 0.1)
+bwvel = lfilter(B,A,fdvel)
+
+# FD skip 3
+dist = 3
+fd3vel = lfilter(array([1]+[0]*(dist-1)+[-1])/float(dist),[1],pos)/T
+
+# Least squared 15 (from Freedom6S API)
+def leastsquared(n=15):
+    dTemp = (n - 1) / 2.0;
+    dTemp2 = 12.0/ (n * ((n*n) - 1));
+    return (dTemp-arange(n))*dTemp2
+
+lsvel = lfilter(leastsquared(15), 1, pos)/T
+
+# First-Order Adaptive Windowing (FOAW)
+def foaw(pos, n=16, best=False):
+    result = zeros(len(pos))
+    for k in range(len(pos)):
+        velocity = 0
+        for i in range(1,min(n,k)):
+            # Calculate slope over interval
+            if (best):
+                # least squared method (best-fit-FOAW)
+                b = ( ( i*sum([pos[k-j] for j in range(i+1)])
+                      - 2*sum([pos[k-j]*j for j in range(i+1)]) )
+                    / (T*i*(i+1)*(i+2)/6) )
+            else:
+                # direct method (end-fit-FOAW)
+                b = (pos[k]-pos[k-i]) / (i*T)
+
+            # Check the linear estimate of each middle point
+            outside = False
+            for j in range(1,i):
+                ykj = pos[k]-(b*j*T)
+
+                # Compare to the measured value within the noise margin
+                # If it's outside noise margin, return last estimate
+                if ykj < (pos[k-j]-noise_max) or ykj > (pos[k-j]+noise_max):
+                    outside = True
+                    break
+            if outside: break
+            velocity = b
+
+        result[k] = velocity
+
+    return result
+
+def median_filter(pos, n=5):
+    result = zeros(len(pos))
+    for k in range(1,len(pos)):
+        result[k] = median(pos[k-min(n,k):k])
+    return result
+
+endfitfoawvel = foaw(pos, n=16, best=False)
+bestfitfoawvel = foaw(pos, n=16, best=True)
+mpos = median_filter(pos, n=3)
+endfitfoawvelm = foaw(mpos, n=16, best=False)
+bestfitfoawvelm = foaw(mpos, n=16, best=True)
+
+# Plotting, velocity curves and derivatives
+def plotcurves(curves, titles, vel_yrange=None, dif_yrange=None):
+    for n, v in enumerate(curves):
+        acc = v-vel
+        subplot(len(curves),2,n*2+1)
+        plot(time, v)
+        if (vel_yrange!=None):
+            axis([time[0],time[-1],vel_yrange[0],vel_yrange[1]])
+        title(titles[n]+': velocity')
+        subplot(len(curves),2,n*2+2)
+        plot(time, acc)
+        if (dif_yrange!=None):
+            axis([time[0],time[-1],dif_yrange[0],dif_yrange[1]])
+        title(titles[n]+': ideal difference')
+    show()
+
+curves = [fdvel, fd3vel, bwvel, lsvel]
+titles = ['Simple Finite Difference',
+          'Finite difference 3',
+          'Butterworth %d Hz'%(sr*0.1),
+          'Least Squared']
+
+plotcurves(curves, titles, vel_yrange = [-1.5, 2.5], dif_yrange = [-0.3, 0.3])
+
+curves = [endfitfoawvel,bestfitfoawvel,endfitfoawvelm,bestfitfoawvelm]
+titles = ['end-fit-FOAW','best-fit-FOAW','end-fit-FOAW w/ median','best-fit-FOAW w/ median']
+
+plotcurves(curves, titles, vel_yrange = [-1.5, 2.5], dif_yrange = [-0.3, 0.3])
+