Các phép đo vị trí, cách ước tính vận tốc và gia tốc

Điều này đơn giản tôi nghĩ, nhưng cách tiếp cận ngây thơ của tôi đã dẫn đến một kết quả rất ồn ào. Tôi có thời gian và vị trí mẫu này trong một tệp có tên 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

và muốn ước tính vận tốc và gia tốc. Tôi biết rằng gia tốc là không đổi, trong trường hợp này khoảng 55 độ / giây ^ 2 cho đến khi vận tốc khoảng 100 độ / giây, thì acc là 0 và vận tốc không đổi. Ở cuối gia tốc là -55 độ / giây ^ 2. Dưới đây là mã scilab cung cấp các ước tính rất ồn ào và không sử dụng được, đặc biệt là khả năng tăng tốc.

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

Tôi đã nghĩ đến việc sử dụng bộ lọc kalman thay vào đó, để có được ước tính tốt hơn. Ở đây có thích hợp không? Không biết cách xây dựng các phương trình filer, không có nhiều kinh nghiệm với các bộ lọc kalman. Tôi nghĩ rằng vectơ trạng thái là vận tốc và gia tốc và tín hiệu là vị trí. Hoặc có một phương pháp đơn giản hơn KF, cho kết quả hữu ích.

Chào mừng tất cả các đề xuất!

Một cách tiếp cận sẽ là giải quyết vấn đề nhỏ nhất - làm mịn hình vuông. Ý tưởng là để phù hợp cục bộ một đa thức với một cửa sổ chuyển động, sau đó đánh giá đạo hàm của đa thức. Câu trả lời này về lọc Savitzky-Golay có một số nền tảng lý thuyết về cách thức hoạt động của nó để lấy mẫu không hình thành.

Trong trường hợp này, mã có thể được chiếu sáng nhiều hơn về lợi ích / hạn chế của kỹ thuật. Kịch bản numpy sau đây sẽ tính toán vận tốc và gia tốc của tín hiệu vị trí đã cho dựa trên hai tham số: 1) kích thước của cửa sổ làm mịn và 2) thứ tự của xấp xỉ đa thức cục bộ.

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

Dưới đây là một số sơ đồ ví dụ (sử dụng dữ liệu bạn cung cấp) cho các tham số khác nhau.

Lưu ý làm thế nào tính chất không đổi của piecewise của gia tốc trở nên ít rõ ràng hơn khi kích thước cửa sổ tăng lên, nhưng có thể được phục hồi ở một mức độ nào đó bằng cách sử dụng đa thức bậc cao. Tất nhiên, các tùy chọn khác liên quan đến việc áp dụng bộ lọc phái sinh đầu tiên hai lần (có thể của các đơn đặt hàng khác nhau). Một điều rõ ràng nữa là cách lọc loại Savitzky-Golay này, vì nó sử dụng điểm giữa của cửa sổ, cắt ngắn phần cuối của dữ liệu được làm mịn ngày càng nhiều khi kích thước cửa sổ tăng lên. Có nhiều cách khác nhau để giải quyết vấn đề đó, nhưng một trong những cách tốt hơn được mô tả trong bài viết sau:

PA Gdvisor, Làm mịn và phân biệt bình phương nhỏ nhất bằng phương pháp tích chập (Savitzky xô Golay), Hậu môn. Hóa. 62 (1990) 570 hào573. ( google )

Một bài báo khác của cùng tác giả mô tả một cách hiệu quả hơn để làm mịn dữ liệu không đồng nhất so với phương pháp đơn giản trong mã ví dụ:

PA Gdvisor, Làm mịn bình phương nhỏ nhất và phân biệt dữ liệu khoảng cách không đồng nhất bằng phương pháp tích chập, Hậu môn. Hóa. 63 (1991) 534 bóng536. ( google )

Cuối cùng, một bài báo đáng đọc hơn trong lĩnh vực này là của Persson và Strang :

PO Persson, G. Strang, Làm mịn bằng bộ lọc Golay và Legendre của Savitzky, Comm. Comp. Tài chính 13 (2003) 301 Từ316. ( liên kết pdf )

Nó chứa nhiều lý thuyết nền tảng hơn, và tập trung vào phân tích lỗi để chọn kích thước cửa sổ.

Bạn có lẽ chỉ nên làm giống như trong câu hỏi và câu trả lời này .

Chỉnh sửa: xóa tham chiếu đến hai chiều; mã thực sự chỉ sử dụng một (cái còn lại là biến thời gian).

Tuy nhiên, các mẫu thời gian của bạn dường như không cách đều nhau. Đó là nhiều hơn một vấn đề.