การวัดตำแหน่งที่กำหนด, วิธีการประมาณความเร็วและการเร่งความเร็ว

นี่เป็นเรื่องง่ายที่ฉันคิด แต่วิธีการไร้เดียงสาของฉันนำไปสู่ผลลัพธ์ที่มีเสียงดังมาก ฉันมีเวลาตัวอย่างและตำแหน่งในไฟล์ชื่อ 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

และต้องการประเมินความเร็วและความแม่นยำ ฉันรู้ว่า accelerstion คงที่ในกรณีนี้ประมาณ 55 องศา / วินาที ^ 2 จนกว่าความเร็วจะอยู่ที่ประมาณ 100 องศา / วินาทีจากนั้น ACC จะเป็นศูนย์และคงที่ความเร็ว ในตอนท้าย accelerstion คือ -55 องศา / วินาที ^ 2 นี่คือโค้ดสคิแลปที่ให้การประมาณที่มีเสียงดังและใช้ไม่ได้โดยเฉพาะการเร่งความเร็ว

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 ซึ่งให้ผลลัพธ์ที่มีประโยชน์

ยินดีต้อนรับคำแนะนำทั้งหมด!

วิธีการหนึ่งก็คือการทำให้ปัญหาเป็นไปอย่างราบรื่นกำลังสองน้อยที่สุด แนวคิดคือให้พอดีกับพหุนามกับหน้าต่างที่เคลื่อนที่ได้จากนั้นประเมินอนุพันธ์ของพหุนาม คำตอบเกี่ยวกับตัวกรอง Savitzky-Golayนี้มีพื้นฐานทางทฤษฎีบางประการเกี่ยวกับวิธีการทำงานสำหรับการสุ่มตัวอย่างแบบ nonuniform

ในกรณีนี้โค้ดน่าจะให้ความสว่างมากกว่าประโยชน์ / ข้อ จำกัด ของเทคนิค สคริปต์numpyต่อไปนี้จะคำนวณความเร็วและความเร่งของสัญญาณตำแหน่งที่กำหนดตามสองพารามิเตอร์: 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()

นี่คือตัวอย่างแปลง (ใช้ข้อมูลที่คุณให้ไว้) สำหรับพารามิเตอร์ต่างๆ

สังเกตว่าธรรมชาติของความเร่งคงที่ทีละน้อยจะชัดเจนน้อยลงเมื่อขนาดหน้าต่างเพิ่มขึ้น แต่สามารถกู้คืนได้ในระดับหนึ่งโดยใช้พหุนามคำสั่งที่สูงกว่า แน่นอนตัวเลือกอื่น ๆ ที่เกี่ยวข้องกับการใช้ตัวกรองอนุพันธ์ตัวแรกสองครั้ง (อาจเป็นคำสั่งที่แตกต่างกัน) อีกสิ่งที่ควรชัดเจนคือการกรอง Savitzky-Golay ประเภทนี้เนื่องจากใช้จุดกึ่งกลางของหน้าต่างตัดปลายของข้อมูลที่ราบเรียบมากขึ้นเมื่อขนาดหน้าต่างเพิ่มขึ้น มีหลายวิธีในการแก้ปัญหาดังกล่าว แต่หนึ่งในวิธีที่ดีกว่าได้อธิบายไว้ในเอกสารต่อไปนี้:

PA Gorry, การทำให้ราบเรียบและความแตกต่างโดยวิธีกำลังสองน้อยที่สุดอย่างทั่วไปโดยวิธีการโน้มน้าว (Savitzky – Golay), Anal Chem 62 (1990) 570–573 ( google )

กระดาษอื่นโดยผู้เขียนคนเดียวกันอธิบายวิธีที่มีประสิทธิภาพมากขึ้นในการทำให้ข้อมูลที่ไม่ใช่รูปแบบเรียบราบรื่นกว่าวิธีที่ตรงไปตรงมาในรหัสตัวอย่าง:

PA Gorry, การปรับให้เรียบแบบสี่เหลี่ยมน้อยที่สุดโดยทั่วไปและการแยกความแตกต่างของข้อมูลที่เว้นระยะแบบไม่สม่ำเสมอโดยวิธี Anal Chem 63 (1991) 534–536 ( google )

ในที่สุดกระดาษอีกหนึ่งค่าที่ควรอ่านในบริเวณนี้คือโดย Persson และStrang :

PO Persson, G. Strang, ปรับให้เรียบโดย Savitzky – Golay และ Legendre Filter, Comm คอมพ์ การเงิน 13 (2546) 301–316 ( ลิงก์ pdf )

มันมีทฤษฎีพื้นหลังมากขึ้นและมุ่งเน้นการวิเคราะห์ข้อผิดพลาดในการเลือกขนาดหน้าต่าง

คุณน่าจะทำแบบเดียวกันกับคำถามและคำตอบนี้, .

แก้ไข: ลบการอ้างอิงถึงสองมิติ; รหัสใช้เพียงหนึ่งจริง ๆ (อีกเป็นตัวแปรเวลา)

อย่างไรก็ตามตัวอย่างเวลาของคุณไม่ได้เว้นระยะเท่ากัน นั่นเป็นปัญหามากกว่า