Dati misurazioni di posizione, come stimare la velocità e l'accelerazione

Questo è semplice, ho pensato, ma il mio approccio ingenuo ha portato a un risultato molto rumoroso. Ho questi tempi e posizioni di esempio in un file chiamato 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

e voglio stimare la velocità e l'accelerazione. So che l'accelerazione è costante, in questo caso circa 55 gradi / sec ^ 2 fino a quando la velocità è di circa 100 gradi / sec, quindi l'acc è zero e la velocità costante. Alla fine l'accelerazione è di -55 gradi / sec ^ 2. Ecco il codice scilab che fornisce stime molto rumorose e inutilizzabili in particolare dell'accelerazione.

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');

Stavo pensando di usare un filtro kalman per ottenere stime migliori. È appropriato qui? Non so come formulare le equazioni del filer, non molto esperte con i filtri kalman. Penso che il vettore di stato sia velocità e accelerazione e in-signal sia posizione. Oppure esiste un metodo più semplice di KF, che fornisce risultati utili.

Tutti i suggerimenti sono benvenuti!

Un approccio sarebbe quello di lanciare il problema come livellamento dei minimi quadrati. L'idea è di adattare localmente un polinomio a una finestra mobile, quindi valutare la derivata del polinomio. Questa risposta sul filtro Savitzky-Golay ha delle basi teoriche su come funziona per il campionamento non uniforme.

In questo caso, il codice è probabilmente più illuminante per quanto riguarda i vantaggi / i limiti della tecnica. Il seguente script numpy calcolerà la velocità e l'accelerazione di un dato segnale di posizione in base a due parametri: 1) la dimensione della finestra di smoothing e 2) l'ordine dell'approssimazione polinomiale locale.

# 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()

Ecco alcuni grafici di esempio (utilizzando i dati forniti) per vari parametri.

Nota come la natura costante a tratti dell'accelerazione diventa meno evidente all'aumentare della dimensione della finestra, ma può essere recuperata in una certa misura usando i polinomi di ordine superiore. Naturalmente, altre opzioni prevedono l'applicazione di un primo filtro derivato due volte (possibilmente di ordini diversi). Un'altra cosa che dovrebbe essere ovvia è come questo tipo di filtro Savitzky-Golay, poiché utilizza il punto medio della finestra, tronca sempre più le estremità dei dati levigati all'aumentare delle dimensioni della finestra. Esistono vari modi per risolvere quel problema, ma uno dei migliori è descritto nel seguente documento:

PA Gorry, livellamento e differenziazione dei minimi quadrati generali con il metodo della convoluzione (Savitzky – Golay), Anal. Chem. 62 (1990) 570-573. ( google )

Un altro documento dello stesso autore descrive un modo più efficiente per lisciare i dati non uniformi rispetto al metodo semplice nel codice di esempio:

PA Gorry, Livellamento generale dei minimi quadrati e differenziazione dei dati distribuiti in modo non uniforme mediante il metodo di convoluzione, Anal. Chem. 63 (1991) 534-536. ( google )

Infine, un altro documento che vale la pena leggere in quest'area è di Persson e Strang :

PO Persson, G. Strang, Smoothing di Savitzky – Golay e Legendre Filters, Comm. Comp. Finance 13 (2003) 301–316. ( link pdf )

Contiene molta più teoria di base e si concentra sull'analisi degli errori per la scelta della dimensione di una finestra.

Si dovrebbe probabilmente solo fare lo stesso come in questa domanda e risposta, .

Modifica: rimosso il riferimento a due dimensioni; il codice effettivamente ne utilizza solo uno (l'altro è la variabile del tempo).

Tuttavia, i campioni di tempo non sembrano essere distribuiti uniformemente. Questo è più un problema.