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

これは単純だと思いましたが、私の素朴なアプローチは非常にうるさい結果をもたらしました。私はこのサンプルの時間と位置を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つのアプローチは、問題を最小二乗平滑化としてキャストすることです。アイデアは、多項式を移動ウィンドウに局所的に適合させ、多項式の導関数を評価することです。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()

以下は、さまざまなパラメーターの（提供したデータを使用した）プロットの例です。

ウィンドウサイズが大きくなるにつれて、区分的に一定の加速度の性質がどのように明白にならないかに注意してください。ただし、高次多項式を使用することである程度回復できます。もちろん、他のオプションには、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リンク）

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

おそらく、この質問と回答と同じことを行う必要があります。

編集：2次元への参照を削除。コードは実際には1つだけを使用します（もう1つは時間変数です）。

ただし、時間サンプルは等間隔に表示されません。それはもっと問題です。