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

11

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! ingrese la descripción de la imagen aquí

lgwest
fuente
1
Esta es una aplicación apropiada de un filtro Kalman. El artículo de Wikipedia sobre los filtros de Kalman tiene un ejemplo muy parecido al suyo. Solo estima la posición y la velocidad, pero si comprende ese ejemplo, también es sencillo extenderlo a la aceleración.
Jason R
1
En Scipy, esto podría ser útil < docs.scipy.org/doc/scipy-0.16.1/reference/generated/… >
Mike

Respuestas:

12

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.

Alisado 3pt, polinomio de segundo grado Alisado de 7 puntos, polinomio de segundo grado Alisado de 11 puntos, polinomio de segundo grado Alisado de 11 puntos, polinomio de cuarto grado Alisado de 11 puntos, polinomio de décimo grado

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.

Datageist
fuente
Buen análisis! +1
Peter K.
¡Aprecio totalmente esta respuesta!
lgwest
@Iqwest Claro, ¡espero que ayude!
Datageist
Si los datos están espaciados uniformemente, por ejemplo, dt = 0.1, ¿cuáles son las funciones de filtro correspondientes?
lgwest
Entonces los coeficientes del filtro serán constantes, por lo que puede llamar a sg_filter una vez (y multiplicar el filtro por el factorial de la derivada k - 2 para la aceleración). Vea la primera parte de esta respuesta .
Datageist