Измерения заданной позиции, как оценить скорость и ускорение

Это просто, подумал я, но мой наивный подход привел к очень шумному результату. У меня есть этот пример времени и позиции в файле с именем 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

и хочу оценить скорость и ускорение. Я знаю, что ускорение является постоянным, в этом случае около 55 градусов / сек ^ 2, пока скорость не станет около 100 градусов / сек, тогда акк равна нулю, а скорость постоянна. В конце ускорение -55 град / с ^ 2. Вот код scilab, который дает очень шумные и непригодные оценки особенно ускорения.

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

Я думал об использовании фильтра Калмана, чтобы получить более точные оценки. Это уместно здесь? Не знаю, как сформулировать уравнения фильтра, не очень опытный с фильтрами Калмана. Я думаю, что вектор состояния - это скорость и ускорение, а сигнал - это позиция. Или есть более простой метод, чем KF, который дает полезные результаты.

Все предложения приветствуются!

Один из подходов состоит в том, чтобы поставить задачу как сглаживание методом наименьших квадратов. Идея состоит в том, чтобы локально подогнать многочлен с движущимся окном, а затем оценить производную многочлена. Этот ответ о фильтрации Савицкого-Голея имеет теоретическое обоснование того, как он работает для неравномерной выборки.

В этом случае код, вероятно, более осветителен в отношении преимуществ / ограничений метода. Следующий скриптовый код вычислит скорость и ускорение заданного сигнала положения на основе двух параметров: 1) размера окна сглаживания и 2) порядка аппроксимации локального полинома.

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

Вот несколько примеров графиков (с использованием предоставленных вами данных) для различных параметров.

Обратите внимание, что кусочно-постоянный характер ускорения становится менее очевидным при увеличении размера окна, но может быть восстановлен до некоторой степени с помощью полиномов высшего порядка. Конечно, другие варианты включают применение первого производного фильтра дважды (возможно, разных порядков). Еще одна вещь, которая должна быть очевидна, это то, как этот тип фильтрации Савицкого-Голея, поскольку он использует среднюю точку окна, все больше и больше обрезает концы сглаженных данных при увеличении размера окна. Существуют различные способы решения этой проблемы, но один из лучших описан в следующей статье:

Горри П. А. Общие методы сглаживания и дифференцирования наименьших квадратов методом свертки (Савицкого – Голея), Anal. Химреагент 62 (1990) 570–573. ( Google )

Другая статья того же автора описывает более эффективный способ сглаживания неоднородных данных, чем простой метод в примере кода:

Горри П. А. Общее сглаживание наименьших квадратов и дифференцирование неравномерно распределенных данных методом свертки // Анал. Химреагент 63 (1991) 534–536. ( Google )

Наконец, еще одна статья, которую стоит прочитать в этой области, - это Перссон и Странг :

П. О. Перссон, Г. Странг, Сглаживание фильтрами Савицкого – Голея и Лежандра, Comm. Комп. Финансы 13 (2003) 301–316. ( pdf ссылка )

Он содержит гораздо больше теории фона и концентрируется на анализе ошибок при выборе размера окна.

Вы, вероятно, должны сделать то же самое, что и в этом вопросе и ответе .

Редактировать: удалена ссылка на два измерения; код на самом деле использует только один (другой является переменной времени).

Тем не менее, ваши временные выборки не отображаются равномерно. Это больше проблема.