Gegebene Positionsmessungen, wie man Geschwindigkeit und Beschleunigung schätzt

Das ist einfach, dachte ich, aber mein naiver Ansatz führte zu einem sehr lauten Ergebnis. Ich habe diese Beispielzeiten und -positionen in einer Datei mit dem Namen 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

und wollen Geschwindigkeit und Beschleunigung schätzen. Ich weiß, dass die Beschleunigung konstant ist, in diesem Fall ungefähr 55 Grad / s ^ 2, bis die Geschwindigkeit ungefähr 100 Grad / s beträgt, dann ist die Beschleunigung Null und die Geschwindigkeit konstant. Am Ende beträgt die Beschleunigung -55 ° / s ^ 2. Hier ist Scilab-Code, der sehr verrauschte und unbrauchbare Schätzungen insbesondere der Beschleunigung liefert.

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

Ich dachte daran, stattdessen einen Kalman-Filter zu verwenden, um bessere Schätzungen zu erhalten. Ist es hier angebracht? Ich weiß nicht, wie ich die Filer-Gleichungen formulieren soll, die mit Kalman-Filtern nicht sehr erfahren sind. Ich denke, der Zustandsvektor ist Geschwindigkeit und Beschleunigung und das In-Signal ist Position. Oder gibt es eine einfachere Methode als KF, die nützliche Ergebnisse liefert?

Alle Vorschläge willkommen!

Ein Ansatz wäre, das Problem als Glättung der kleinsten Quadrate zu betrachten. Die Idee ist, ein Polynom lokal mit einem sich bewegenden Fenster anzupassen und dann die Ableitung des Polynoms zu bewerten. Diese Antwort zur Savitzky-Golay-Filterung enthält einige theoretische Hintergrundinformationen zur Funktionsweise ungleichmäßiger Stichproben.

In diesem Fall ist der Code hinsichtlich der Vor- und Nachteile der Technik wahrscheinlich aufschlussreicher. Das folgende Numpy- Skript berechnet die Geschwindigkeit und Beschleunigung eines bestimmten Positionssignals basierend auf zwei Parametern: 1) der Größe des Glättungsfensters und 2) der Reihenfolge der lokalen Polynomnäherung.

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

Hier sind einige Beispieldiagramme (unter Verwendung der von Ihnen angegebenen Daten) für verschiedene Parameter.

Es ist zu beachten, dass die stückweise konstante Natur der Beschleunigung mit zunehmender Fenstergröße weniger offensichtlich wird, aber bis zu einem gewissen Grad durch Verwendung von Polynomen höherer Ordnung wiederhergestellt werden kann. Bei anderen Optionen wird natürlich zweimal ein Filter der ersten Ableitung angewendet (möglicherweise in unterschiedlicher Reihenfolge). Eine andere Sache, die offensichtlich sein sollte, ist, wie diese Art der Savitzky-Golay-Filterung, da sie den Mittelpunkt des Fensters verwendet, die Enden der geglätteten Daten mit zunehmender Fenstergröße immer mehr abschneidet. Es gibt verschiedene Möglichkeiten, dieses Problem zu lösen, aber eine der besseren wird im folgenden Dokument beschrieben:

PA Gorry, Allgemeine Glättung und Differenzierung der kleinsten Quadrate nach der Faltungsmethode (Savitzky-Golay), Anal. Chem. 62 (1990) 570–573. ( google )

Ein anderes Papier desselben Autors beschreibt eine effizientere Methode zum Glätten ungleichmäßiger Daten als die einfache Methode im Beispielcode:

PA Gorry, Allgemeine Glättung der kleinsten Quadrate und Differenzierung von Daten mit ungleichmäßigem Abstand nach der Faltungsmethode, Anal. Chem. 63 (1991) 534–536. ( google )

Ein weiteres lesenswertes Papier in diesem Bereich ist von Persson und Strang :

PO Persson, G. Strang, Glättung durch Savitzky-Golay- und Legendre-Filter, Comm. Comp. Finance 13 (2003) 301–316. ( pdf link )

Es enthält viel mehr Hintergrundtheorie und konzentriert sich auf die Fehleranalyse bei der Auswahl einer Fenstergröße.

Sie sollten wahrscheinlich genau das Gleiche tun wie in dieser Frage und Antwort .

Bearbeiten: Verweis auf zweidimensionale entfernt; Der Code verwendet tatsächlich nur einen (der andere ist die Zeitvariable).

Ihre Zeitmuster scheinen jedoch nicht gleichmäßig verteilt zu sein. Das ist eher ein Problem.