Medições de posição dadas, como estimar velocidade e aceleração

Isso é simples, pensei, mas minha abordagem ingênua levou a um resultado muito barulhento. Eu tenho este exemplo de tempos e posições em um arquivo chamado 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 deseja estimar velocidade e aceleração. Eu sei que a aceleração é constante, neste caso cerca de 55 graus / s ^ 2 até que a velocidade esteja em torno de 100 graus / s, então o acc é zero e a velocidade é constante. No final, a aceleração é de -55 graus / s ^ 2. Aqui está o código scilab que fornece estimativas muito barulhentas e inutilizáveis, especialmente da aceleração.

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

Eu estava pensando em usar um filtro kalman para obter melhores estimativas. É apropriado aqui? Não sei como formular as equações do filtro, pouco experiente com os filtros kalman. Eu acho que o vetor de estado é velocidade e aceleração e sinal é posição. Ou existe um método mais simples que o KF, que fornece resultados úteis.

Todas as sugestões são bem-vindas!

Uma abordagem seria considerar o problema como suavização de mínimos quadrados. A idéia é ajustar localmente um polinômio a uma janela em movimento e avaliar a derivada do polinômio. Esta resposta sobre a filtragem de Savitzky-Golay tem alguns antecedentes teóricos sobre como funciona para amostragem não uniforme.

Nesse caso, o código é provavelmente mais esclarecedor quanto aos benefícios / limitações da técnica. O seguinte script numpy calculará a velocidade e a aceleração de um determinado sinal de posição com base em dois parâmetros: 1) o tamanho da janela de suavização e 2) a ordem da aproximação polinomial 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()

Aqui estão alguns exemplos de gráficos (usando os dados que você forneceu) para vários parâmetros.

Observe como a natureza constante por partes da aceleração se torna menos óbvia à medida que o tamanho da janela aumenta, mas pode ser recuperada até certo ponto usando polinômios de ordem superior. Obviamente, outras opções envolvem a aplicação de um primeiro filtro derivado duas vezes (possivelmente de ordens diferentes). Outra coisa que deve ser óbvia é como esse tipo de filtragem Savitzky-Golay, já que usa o ponto médio da janela, trunca as extremidades dos dados suavizados cada vez mais à medida que o tamanho da janela aumenta. Existem várias maneiras de resolver esse problema, mas uma das melhores é descrita no documento a seguir:

PA Gorry, suavização e diferenciação geral dos mínimos quadrados pelo método de convolução (Savitzky – Golay), Anal. Chem. 62 (1990) 570-573. ( google )

Outro artigo do mesmo autor descreve uma maneira mais eficiente de suavizar dados não uniformes do que o método direto no código de exemplo:

PA Gorry, suavização geral dos mínimos quadrados e diferenciação de dados espaçados de maneira não uniforme pelo método de convolução, Anal. Chem. 63 (1991) 534-536. ( google )

Finalmente, mais um artigo que vale a pena ler nesta área é de Persson e Strang :

PO Persson, G. Strang, Suavização por Savitzky – Golay e Legendre Filters, Comm. Comp. Finanças 13 (2003) 301–316. ( link em pdf )

Ele contém muito mais teoria de background e se concentra na análise de erros para escolher o tamanho da janela.

Você provavelmente deve fazer o mesmo que nesta pergunta e resposta ,.

Editar: referência removida para duas dimensões; o código realmente usa apenas um (o outro é a variável de tempo).

No entanto, suas amostras de tempo não parecem estar uniformemente espaçadas. Isso é mais um problema.