与えられた位置測定、速度と加速度を推定する方法


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になるまで、加速度はゼロで速度は一定です。最後の加速は-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
これはカルマンフィルターの適切なアプリケーションです。カルマン・フィルタ上のWikipediaの記事は非常にあなたのような例があります。位置と速度を推定するだけですが、その例を理解していれば、それを加速度にも拡張するのは簡単です。
Jason R

1
Scipyではこれが役に立つかもしれません< docs.scipy.org/doc/scipy-0.16.1/reference/generated/… >
Mike

回答:


12

1つのアプローチは、問題を最小二乗平滑化としてキャストすることです。アイデアは、多項式を移動ウィンドウに局所的に適合させ、多項式の導関数を評価することです。Savitzky-Golayフィルタリングに関するこの回答には、非一様サンプリングでの動作に関する理論的な背景があります。

この場合、コードはおそらくテクニックの利点/制限に関してより明るいでしょう。次のnumpyスクリプトは、2つのパラメーターに基づいて特定の位置信号の速度と加速度を計算します。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次微分フィルターを2回適用する(おそらく次数が異なる)ものがあります。このタイプのSavitzky-Golayフィルタリングでは、ウィンドウの中間点を使用するため、ウィンドウサイズが大きくなるにつれて、平滑化されたデータの端がどんどん切り捨てられます。その問題を解決するにはさまざまな方法がありますが、より良い方法の1つを次のペーパーで説明します。

PA Gorry、畳み込み(Savitzky–Golay)法による一般的な最小二乗平滑化と微分、Anal。Chem。62(1990)570–573。(グーグル

同じ著者による別の論文では、サンプルコードの単純な方法よりも、不均一なデータを平滑化するより効率的な方法について説明しています。

PA Gorry、一般的な最小二乗平滑化および畳み込み法による非等間隔データの微分、Anal。Chem。63(1991)534–536。(グーグル

最後に、この領域で読む価値のあるもう1つの論文は、PerssonとStrangによるものです。

PO Persson、G。Strang、Savitzky–GolayおよびLegendre Filtersによる平滑化、Comm。コンプ 金融13(2003)301–316。(pdfリンク

これには、より多くの背景理論が含まれており、ウィンドウサイズを選択するためのエラー分析に集中しています。


素敵な分析!+1
ピーターK.

私はこの答えに完全に感謝します!
lgwest 2013年

@Iqwest確かに、それが役に立てば幸いです!
データガイスト

データが等間隔である場合、たとえばdt = 0.1の場合、対応するフィルター関数は何ですか。
lgwest

その後、フィルター係数は一定になるため、sg_filterを1回呼び出すだけで済みます(そして、フィルターにaccelの導関数k--2の階乗を掛けます)。この回答の最初の部分をご覧ください。
データガイスト

弊社のサイトを使用することにより、あなたは弊社のクッキーポリシーおよびプライバシーポリシーを読み、理解したものとみなされます。
Licensed under cc by-sa 3.0 with attribution required.