Compte tenu des mesures de position, comment estimer la vitesse et l'accélération

C'est simple, je pensais, mais mon approche naïve a conduit à un résultat très bruyant. J'ai ces exemples d'heures et de positions dans un fichier nommé 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

et veulent estimer la vitesse et l'accélération. Je sais que l'accélerstion est constante, dans ce cas environ 55 degrés / sec ^ 2 jusqu'à ce que la vitesse soit d'environ 100 degrés / sec, puis l'acc est nul et la constante de vitesse. À la fin, l'accélerstion est de -55 deg / sec ^ 2. Voici le code scilab qui donne des estimations très bruyantes et inutilisables notamment de l'accélération.

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

Je pensais plutôt utiliser un filtre kalman pour obtenir de meilleures estimations. Est-ce approprié ici? Je ne sais pas comment formuler les équations du filer, pas très expérimenté avec les filtres kalman. Je pense que le vecteur d'état est la vitesse et l'accélération et que le signal est la position. Ou existe-t-il une méthode plus simple que KF, qui donne des résultats utiles.

Toutes les suggestions sont les bienvenues!

Une approche consisterait à présenter le problème comme un lissage des moindres carrés. L'idée est d'adapter localement un polynôme à une fenêtre mobile, puis d'évaluer la dérivée du polynôme. Cette réponse sur le filtrage Savitzky-Golay a une base théorique sur la façon dont il fonctionne pour l'échantillonnage non uniforme.

Dans ce cas, le code est probablement plus éclairant quant aux avantages / limites de la technique. Le script numpy suivant calculera la vitesse et l'accélération d'un signal de position donné sur la base de deux paramètres: 1) la taille de la fenêtre de lissage, et 2) l'ordre de l'approximation polynomiale 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()

Voici quelques exemples de tracés (en utilisant les données que vous avez fournies) pour divers paramètres.

Notez comment la nature constante par morceaux de l'accélération devient moins évidente à mesure que la taille de la fenêtre augmente, mais peut être récupérée dans une certaine mesure en utilisant des polynômes d'ordre supérieur. Bien sûr, d'autres options impliquent d'appliquer deux fois un premier filtre dérivé (éventuellement d'ordres différents). Une autre chose qui devrait être évidente est de savoir comment ce type de filtrage Savitzky-Golay, car il utilise le milieu de la fenêtre, tronque de plus en plus les extrémités des données lissées à mesure que la taille de la fenêtre augmente. Il existe différentes façons de résoudre ce problème, mais l'une des meilleures est décrite dans le document suivant:

PA Gorry, Lissage et différenciation des moindres carrés généraux par la méthode de convolution (Savitzky-Golay), Anal. Chem. 62 (1990) 570-573. ( google )

Un autre article du même auteur décrit un moyen plus efficace de lisser les données non uniformes que la méthode simple dans l'exemple de code:

PA Gorry, Lissage des moindres carrés généraux et différenciation des données à espacement non uniforme par la méthode de convolution, Anal. Chem. 63 (1991) 534-536. ( google )

Enfin, un autre article intéressant à lire dans ce domaine est de Persson et Strang :

PO Persson, G. Strang, Lissage par Savitzky – Golay et Legendre Filters, Comm. Comp. Finance 13 (2003) 301–316. ( lien pdf )

Il contient beaucoup plus de théorie de fond et se concentre sur l'analyse des erreurs pour choisir une taille de fenêtre.

Vous devriez probablement faire la même chose que dans cette question et la réponse, .

Modifier: suppression de la référence aux deux dimensions; le code n'en utilise en fait qu'un (l'autre est la variable de temps).

Cependant, vos échantillons de temps ne semblent pas être régulièrement espacés. C'est plus un problème.