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


11

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

모든 제안을 환영합니다! 여기에 이미지 설명을 입력하십시오


1
이것은 칼만 필터의 적절한 응용입니다. 칼만 필터에 대한 위키 백과의 기사는 매우 당신과 같은 예를 가지고있다. 위치와 속도 만 추정하지만이 예를 이해하면 가속도까지 확장하는 것이 간단합니다.
Jason R

1
Scipy에서 이것은 유용 할 것입니다 < docs.scipy.org/doc/scipy-0.16.1/reference/generated/… >
Mike

답변:


12

한 가지 방법은 문제를 최소 제곱 평활 법으로 만드는 것입니다. 아이디어는 다항식을 이동 창에 로컬로 맞추고 다항식의 미분을 평가하는 것입니다. 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()

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

3pt 평활, 2 차 다항식 7pt 평활, 2 차 다항식 11pt 다듬기, 2 차 다항식 11pt 다듬기, 4 차 다항식 11pt 평활화, 10 차 다항식

창 크기가 커짐에 따라 가속의 부분적으로 일정한 특성이 덜 명확 해지지 만 고차 다항식을 사용하여 어느 정도 복구 할 수 있습니다. 물론, 다른 옵션에는 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 링크 )

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


좋은 분석! +1
Peter K.

이 답변에 전적으로 감사드립니다!
lgwest 2016 년

@ Iqwest 물론, 그것이 도움이되기를 바랍니다!
datageist

데이터의 간격이 균일하면 (예 : dt = 0.1) 해당 필터 기능은 무엇입니까?
lgwest

그러면 필터 계수가 일정하므로 sg_filter를 한 번만 호출하면됩니다 (가속을 위해 미분 k-2의 계승에 필터를 곱하면됩니다). 이 답변 의 첫 번째 부분을 참조하십시오 .
datageist

당사 사이트를 사용함과 동시에 당사의 쿠키 정책개인정보 보호정책을 읽고 이해하였음을 인정하는 것으로 간주합니다.
Licensed under cc by-sa 3.0 with attribution required.