Podane pomiary pozycji, jak oszacować prędkość i przyspieszenie

Pomyślałem, że to proste, ale moje naiwne podejście doprowadziło do bardzo głośnego wyniku. Mam przykładowe czasy i pozycje w pliku o nazwie 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

i chcą oszacować prędkość i przyspieszenie. Wiem, że przyspieszenie jest stałe, w tym przypadku około 55 stopni / sek. ^ 2, aż prędkość osiągnie około 100 stopni / sek., Wtedy acc będzie równy zero i stała prędkości. Na końcu przyspieszenie wynosi -55 ° / s ^ 2. Oto kod scilab, który podaje bardzo głośne i bezużyteczne oszacowania, zwłaszcza przyspieszenia.

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

Zastanawiałem się nad użyciem filtru Kalmana, aby uzyskać lepsze oszacowania. Czy to jest właściwe tutaj? Nie wiem, jak sformułować równania filtru, niezbyt doświadczone z filtrami Kalmana. Myślę, że wektorem stanu jest prędkość i przyspieszenie, a w sygnale jest pozycja. Czy istnieje metoda prostsza niż KF, która daje użyteczne wyniki.

Wszystkie sugestie mile widziane!

Jednym z podejść byłoby zastosowanie problemu wygładzania metodą najmniejszych kwadratów. Chodzi o to, aby lokalnie dopasować wielomian do ruchomego okna, a następnie ocenić pochodną wielomianu. Ta odpowiedź na temat filtrowania Savitzky'ego-Golaya ma pewne teoretyczne podstawy działania niejednorodnego próbkowania.

W tym przypadku kod jest prawdopodobnie bardziej pouczający, jeśli chodzi o zalety / ograniczenia techniki. Poniższy skrypt numpy obliczy prędkość i przyspieszenie danego sygnału pozycji na podstawie dwóch parametrów: 1) wielkości okna wygładzania i 2) rzędu lokalnego przybliżenia wielomianowego.

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

Oto kilka przykładowych wykresów (wykorzystujących podane dane) dla różnych parametrów.

Zwróć uwagę, w jaki sposób cząstkowa stała natura przyspieszenia staje się mniej oczywista wraz ze wzrostem wielkości okna, ale można ją w pewnym stopniu odzyskać, stosując wielomian wyższego rzędu. Oczywiście inne opcje obejmują dwukrotne zastosowanie pierwszego filtra pochodnej (być może z różnych rzędów). Kolejną rzeczą, która powinna być oczywista, jest sposób, w jaki ten typ filtrowania Savitzky'ego-Golaya, ponieważ wykorzystuje punkt środkowy okna, coraz bardziej przycina końce wygładzonych danych wraz ze wzrostem rozmiaru okna. Istnieją różne sposoby rozwiązania tego problemu, ale jeden z lepszych opisano w następującym artykule:

PA Gorry, Ogólne wygładzanie i różnicowanie metodą najmniejszych kwadratów metodą splotu (Savitzky – Golay), Anal. Chem. 62 (1990) 570–573. ( google )

Inny artykuł tego samego autora opisuje bardziej wydajny sposób wygładzania niejednolitych danych niż prosta metoda w przykładowym kodzie:

PA Gorry, Ogólne wygładzanie najmniejszych kwadratów i różnicowanie nierównomiernie rozmieszczonych danych metodą splotu, Anal. Chem. 63 (1991) 534–536. ( google )

Na koniec jeszcze jeden artykuł warty przeczytania w tym zakresie to Persson i Strang :

PO Persson, G. Strang, Wygładzanie: Savitzky – Golay i Legendre Filters, Comm. Komp. Finanse 13 (2003) 301–316. ( link pdf )

Zawiera znacznie więcej teorii tła i koncentruje się na analizie błędów przy wyborze rozmiaru okna.

Powinieneś po prostu zrobić to samo jak w tym pytaniu i odpowiedzi .

Edycja: usunięto odniesienie do dwuwymiarowych; kod faktycznie używa tylko jednego (drugi to zmienna czasowa).

Jednak próbki czasu nie wydają się być równomiernie rozmieszczone. To bardziej problem.