주어진 위치 측정, 속도 및 가속도 추정 방법

이것은 내가 생각한 단순하지만 내 순진한 접근 방식은 시끄러운 결과를 가져 왔습니다. 이 샘플 시간과 위치는 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

속도와 가속도를 추정하려고합니다. 속도가 약 100도 / 초가 될 때까지 가속이 일정하다는 것을 알고 있습니다. 마지막에 가속도는 -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 필터링에 대한이 답변 에는 비 균일 샘플링에 대한 작동 원리에 대한 이론적 배경이 있습니다.

이 경우, 기술의 이점 / 제한에 대해 코드가 더 밝게 빛날 것입니다. 다음 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()

다음은 다양한 매개 변수에 대한 예제 플롯 (제공 한 데이터 사용)입니다.

창 크기가 커짐에 따라 가속의 부분적으로 일정한 특성이 덜 명확 해지지 만 고차 다항식을 사용하여 어느 정도 복구 할 수 있습니다. 물론, 다른 옵션에는 1 차 미분 필터를 두 번 (다른 순서로) 적용하는 것이 포함됩니다. 또 다른 분명한 점은이 유형의 Savitzky-Golay 필터링은 창의 중간 점을 사용하여 창의 크기가 증가함에 따라 스무딩 된 데이터의 끝이 점점 더 잘리는 방식입니다. 이 문제를 해결하는 방법에는 여러 가지가 있지만 더 나은 방법 중 하나가 다음 백서에 설명되어 있습니다.

PA Gorry, 컨볼 루션 (Savitzky–Golay) 방법 인 Anal에 의한 일반 최소 제곱 스무딩 및 미분. 화학 62 (1990) 570–573. ( 구글 )

같은 저자의 또 다른 논문은 예제 코드의 간단한 방법보다 비 균일 데이터를 부드럽게하는 더 효율적인 방법을 설명합니다.

PA Gorry, 컨볼 루션 방법 인 Anal에 의해 비 균일 간격 데이터의 일반 최소 제곱 스무딩 및 미분. 화학 63 (1991) 534-536. ( 구글 )

마지막으로,이 영역에서 읽을 가치가있는 논문이 Persson과 Strang에 의해 작성되었습니다 .

PO Persson, G. Strang, Savitzky–Golay 및 Legendre 필터에 의한 스무딩, Comm. Comp. 재정 13 (2003) 301–316. ( pdf 링크 )

여기에는 훨씬 더 많은 배경 이론이 포함되어 있으며 창 크기를 선택하기위한 오류 분석에 중점을 둡니다.

아마도이 질문과 답변에서와 똑같이해야합니다 .

편집 : 2 차원에 대한 참조를 제거했습니다. 코드는 실제로 하나만 사용합니다 (다른 하나는 시간 변수 임).

그러나 시간 샘플의 간격이 균등하지 않은 것 같습니다. 그것은 더 큰 문제입니다.