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

11

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! insira a descrição da imagem aqui

lgwest
fonte
1
Esta é uma aplicação apropriada de um filtro Kalman. O artigo da Wikipedia sobre filtros Kalman tem um exemplo muito parecido com o seu. Ele apenas estima posição e velocidade, mas se você entender esse exemplo, é fácil estendê-lo também para a aceleração.
Jason R
1
No Scipy isso pode ser útil < docs.scipy.org/doc/scipy-0.16.1/reference/generated/… >
Mike

Respostas:

12

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.

Alisamento 3pt, polinômio de segundo grau 7pt de alisamento, polinômio de segundo grau Alisamento 11pt, polinômio de segundo grau Alisamento 11pt, polinômio de 4º grau Alisamento 11pt, polinômio de 10º grau

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.

datagrama
fonte
Boa análise! +1
Peter K.
Agradeço totalmente esta resposta!
lgwest
@Iqwest Coisa certa, espero que ajude!
datageist
Se os dados estiverem espaçados uniformemente, por exemplo, dt = 0,1, qual é o filtro correspondente então?
lgwest
Então os coeficientes do filtro serão constantes, então você pode chamar sg_filter apenas uma vez (e multiplicar o filtro pelo fatorial da derivada k-2 para aceleração). Veja a primeira parte desta resposta .
datageist
2

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.

Peter K.
fonte