Medidas de posición dadas, cómo estimar la velocidad y la aceleración

Esto es simple, pensé, pero mi enfoque ingenuo condujo a un resultado muy ruidoso. Tengo estos tiempos y posiciones de muestra en un archivo llamado t_angle.txt:

0.768 -166.099892
0.837 -165.994148
0.898 -165.670052
0.958 -165.138245
1.025 -164.381218
1.084 -163.405838
1.144 -162.232704
1.213 -160.824051
1.268 -159.224854
1.337 -157.383270
1.398 -155.357666
1.458 -153.082809
1.524 -150.589943
1.584 -147.923012
1.644 -144.996872
1.713 -141.904221
1.768 -138.544807
1.837 -135.025749
1.896 -131.233063
1.957 -127.222366
2.024 -123.062325
2.084 -118.618355
2.144 -114.031906
2.212 -109.155006
2.271 -104.059753
2.332 -98.832321
2.399 -93.303795
2.459 -87.649956
2.520 -81.688499
2.588 -75.608597
2.643 -69.308281
2.706 -63.008308
2.774 -56.808586
2.833 -50.508270
2.894 -44.308548
2.962 -38.008575
3.021 -31.808510
3.082 -25.508537
3.151 -19.208565
3.210 -13.008499
3.269 -6.708527
3.337 -0.508461
3.397 5.791168
3.457 12.091141
3.525 18.291206
3.584 24.591179
3.645 30.791245
3.713 37.091217
3.768 43.291283
3.836 49.591255
3.896 55.891228
3.957 62.091293
4.026 68.391266
4.085 74.591331
4.146 80.891304
4.213 87.082100
4.268 92.961502
4.337 98.719368
4.397 104.172363
4.458 109.496956
4.518 114.523888
4.586 119.415550
4.647 124.088860
4.707 128.474464
4.775 132.714500
4.834 136.674385
4.894 140.481148
4.962 144.014626
5.017 147.388458
5.086 150.543938
5.146 153.436089
5.207 156.158638
5.276 158.624725
5.335 160.914001
5.394 162.984924
5.463 164.809685
5.519 166.447678

y desea estimar la velocidad y la aceleración. Sé que la aceleración es constante, en este caso aproximadamente 55 grados / seg ^ 2 hasta que la velocidad sea de aproximadamente 100 grados / seg, entonces el acc es cero y la velocidad constante. Al final, la aceleración es -55 grados / seg ^ 2. Aquí hay un código scilab que proporciona estimaciones muy ruidosas e inutilizables, especialmente de la aceleración.

clf()
clear
M=fscanfMat('t_angle.txt');
t=M(:,1);
len=length(t);
x=M(:,2);
dt=diff(t);
dx=diff(x);
v=dx./dt;
dv=diff(v);
a=dv./dt(1:len-2);
subplot(311), title("position"),
plot(t,x,'b');
subplot(312), title("velocity"),
plot(t(1:len-1),v,'g');
subplot(313), title("acceleration"),
plot(t(1:len-2),a,'r');

Estaba pensando en usar un filtro Kalman para obtener mejores estimaciones. ¿Es apropiado aquí? No sé cómo formular las ecuaciones del archivador, no tengo mucha experiencia con los filtros Kalman. Creo que el vector de estado es la velocidad y la aceleración y la señal interna es la posición. ¿O hay un método más simple que KF, que da resultados útiles?

Todas las sugerencias son bienvenidas!

Un enfoque sería considerar el problema como suavizado de mínimos cuadrados. La idea es ajustar localmente un polinomio con una ventana en movimiento, luego evaluar la derivada del polinomio. Esta respuesta sobre el filtrado de Savitzky-Golay tiene algunos antecedentes teóricos sobre cómo funciona para el muestreo no uniforme.

En este caso, el código es probablemente más esclarecedor en cuanto a los beneficios / limitaciones de la técnica. La siguiente secuencia de comandos numpy calculará la velocidad y la aceleración de una señal de posición dada en función de dos parámetros: 1) el tamaño de la ventana de suavizado, y 2) el orden de la aproximación polinómica local.

# Example Usage:
# python sg.py position.dat 7 2

import math
import sys

import numpy as np
import numpy.linalg
import pylab as py

def sg_filter(x, m, k=0):
    """
    x = Vector of sample times
    m = Order of the smoothing polynomial
    k = Which derivative
    """
    mid = len(x) / 2        
    a = x - x[mid]
    expa = lambda x: map(lambda i: i**x, a)    
    A = np.r_[map(expa, range(0,m+1))].transpose()
    Ai = np.linalg.pinv(A)

    return Ai[k]

def smooth(x, y, size=5, order=2, deriv=0):

    if deriv > order:
        raise Exception, "deriv must be <= order"

    n = len(x)
    m = size

    result = np.zeros(n)

    for i in xrange(m, n-m):
        start, end = i - m, i + m + 1
        f = sg_filter(x[start:end], order, deriv)
        result[i] = np.dot(f, y[start:end])

    if deriv > 1:
        result *= math.factorial(deriv)

    return result

def plot(t, plots):
    n = len(plots)

    for i in range(0,n):
        label, data = plots[i]

        plt = py.subplot(n, 1, i+1)
        plt.tick_params(labelsize=8)
        py.grid()
        py.xlim([t[0], t[-1]])
        py.ylabel(label)

        py.plot(t, data, 'k-')

    py.xlabel("Time")

def create_figure(size, order):
    fig = py.figure(figsize=(8,6))
    nth = 'th'
    if order < 4:
        nth = ['st','nd','rd','th'][order-1]

    title = "%s point smoothing" % size
    title += ", %d%s degree polynomial" % (order, nth)

    fig.text(.5, .92, title,
             horizontalalignment='center')

def load(name):
    f = open(name)    
    dat = [map(float, x.split(' ')) for x in f]
    f.close()

    xs = [x[0] for x in dat]
    ys = [x[1] for x in dat]

    return np.array(xs), np.array(ys)

def plot_results(data, size, order):
    t, pos = load(data)
    params = (t, pos, size, order)

    plots = [
        ["Position",     pos],
        ["Velocity",     smooth(*params, deriv=1)],
        ["Acceleration", smooth(*params, deriv=2)]
    ]

    create_figure(size, order)
    plot(t, plots)

if __name__ == '__main__':
    data = sys.argv[1]
    size = int(sys.argv[2])
    order = int(sys.argv[3])

    plot_results(data, size, order)
    py.show()

Aquí hay algunos gráficos de ejemplo (usando los datos que proporcionó) para varios parámetros.

Observe cómo la naturaleza constante por partes de la aceleración se vuelve menos obvia a medida que aumenta el tamaño de la ventana, pero puede recuperarse en cierta medida utilizando polinomios de orden superior. Por supuesto, otras opciones implican aplicar un primer filtro derivado dos veces (posiblemente de diferentes órdenes). Otra cosa que debería ser obvia es cómo este tipo de filtrado Savitzky-Golay, ya que utiliza el punto medio de la ventana, trunca los extremos de los datos suavizados cada vez más a medida que aumenta el tamaño de la ventana. Hay varias formas de resolver ese problema, pero una de las mejores se describe en el siguiente documento:

PA Gorry, suavizado y diferenciación de mínimos cuadrados generales por el método de convolución (Savitzky-Golay), Anal. Chem 62 (1990) 570–573. ( google )

Otro artículo del mismo autor describe una forma más eficiente de suavizar datos no uniformes que el método directo en el código de ejemplo:

PA Gorry, suavizado general de mínimos cuadrados y diferenciación de datos espaciados no uniformemente por el método de convolución, Anal. Chem 63 (1991) 534-536. ( google )

Finalmente, un artículo más que vale la pena leer en esta área es por Persson y Strang :

PO Persson, G. Strang, Suavizado por Savitzky – Golay y Legendre Filters, Comm. Comp. Finanzas 13 (2003) 301–316. ( enlace pdf )

Contiene mucha más teoría de fondo y se concentra en el análisis de errores para elegir un tamaño de ventana.

Probablemente debería simplemente hacer lo mismo que en esta pregunta y respuesta, .

Editar: se eliminó la referencia a dos dimensiones; el código en realidad solo usa uno (el otro es la variable de tiempo).

Sin embargo, sus muestras de tiempo no parecen estar espaciadas uniformemente. Eso es más un problema.