- モーションモデルは、以前に計算されたオブジェクトの座標と速度を、UDPを介して既知の形式のタイムスタンプでほぼ定期的に送信します。
- ナビゲーション信号シミュレータは、TCP接続を確立し、それを介して状態ベクトルを受信できます。これには、座標と速度に加えて、加速度とジャークが含まれます。
- 最大10 ^ 4 m / sの速度では、外乱加速度は0.001 m / s2を超えません。
- 座標は独立と見なすことができます。
- 航法信号シミュレータは未来の与えられた瞬間の状態ベクトル予報を受け取るべきです。
加速度とジャークを計算する必要があるため、対応する次数の多項式を予測に使用する必要があるという考えに至りましたが、多項式の係数を決定する問題は未解決のままでした。
数学的には正しくないが、直感的で通常動作する自家製アルゴリズムからスプライン近似とフィルタリングまで、さまざまなオプションを検討し、カルマンフィルタリングに落ち着きました。 私の選択は、一方ではその数学的な正確さと、他方では実際にカルマンを試してみたいという長年の願望によるものでした。
この狭いターゲットのタスクを解決するためのツールとして、Pythonが選択されました。
「kalman filtering python」を検索すると、 filterpyとpykalmanの標準的な実装があることがわかりました 。さらに、広く普及しているnumpyパッケージを使用して、Kalmanフィルタリングを簡単に実装できます。 このスクリプトは、インターネットに接続されていないWindowsマシンで動作するはずだったので、最後のオプションにとどまることにしました。
カルマンフィルタリングに関するscipy cookbookとWikipediaの記事に基づいて、事前定義された入力データのセットで必要な機能を実行するトライアルプログラムを実装しました。
Pythonの3番目のバージョンのソースを以下に示します。 指定は、ウィキペディアの記事と同じです。
デバッグ中、次のレーキを踏んだ：状態ベクトルPを推定するための共分散行列の初期値をゼロに設定し、加速度とジャークの初期値が0になっている（それらは不明である、覚えている？） このエラーを修正した後、元のデータと予測されたデータが見事に一致しました。
標準のPythonプロファイラーを使用して、アルゴリズムの速度、まず第一に、偏差ベクトルSの共分散行列の逆数を推定しました。
試用プログラムのソースコード：
import numpy as np import pylab import profile # n_iter = 5000 # dim = 5 # dim_o = 2 # def calcF( t ): res = np.identity( dim ) _t = 1.0 for i in range( 1, dim ): _t *= t / i for j in range( 0, dim-i ): res[ j ][ i+j ] = _t return res # def calcFG( t ): F = np.identity( dim ) G = np.zeros( ( dim, 1 ) ) _t = 1.0 for i in range( 0, dim ): for j in range( 0, dim-i ): F[ j ][ i+j ] = _t if i <= dim_o: G[ dim_o - i ] = _t _t *= t / ( i+1 ) return F, G # xtruth = np.zeros( ( dim, 1 ) ) xtruth[0][0] = 15.3 xtruth[1][0] = 8.7 xtruth[2][0] = -0.3 xtruth[3][0] = 0.3 xtruth[4][0] = -1.0 # H = np.zeros( ( dim_o, dim ) ) for i in range( dim_o ): H[i][i] = 1.0 H_t = H.transpose() # R = 1e-10 * np.identity( dim_o ) # , t = 0.1 * np.arange( n_iter ) + np.random.normal( 0.0, 0.02, size=( n_iter, ) ) # D = 13.3 * 0.05 / 7000 * 2 / 60.0 # , x = np.zeros( ( dim, 1 ) ) # dx = np.zeros( ( dim_o, n_iter ) ) # P = 10.0 * np.identity( dim ) # z = np.zeros( ( n_iter, dim_o, 1 ) ) for i in range( 0, n_iter ): z[i] = H.dot( calcF( t[ i ] ) ).dot( xtruth ) # F D^2*G*G^T F = np.zeros( ( n_iter, dim, dim ) ) GGt = np.zeros( ( n_iter, dim, dim ) ) for i in range( 1, n_iter ): dt = t[ i ] - t[ i-1 ] F[i], G = calcFG( dt ) GGt[i] = D*D * G.dot( G.transpose() ) # def calc(): global t, x, P, D, z, H, R, dx for i in range( 1, n_iter ): xpred = F[i].dot( x ) Ppred = F[i].dot( P ).dot( F[i].transpose() ) + GGt[i] y = z[i] - H.dot( xpred ) S = H.dot( Ppred ).dot( H_t ) + R K = Ppred.dot( H_t ).dot( np.linalg.inv( S ) ) x = xpred + K.dot( y ) P = ( np.identity( dim ) - K.dot( H ) ).dot( Ppred ) # dx[0][i] = y[0][0] / x[0][0] dx[1][i] = y[1][0] / x[1][0] profile.run( 'calc()' ) # pylab.figure() pylab.plot( t, dx[0], label='x' ) pylab.plot( t, dx[1], label='v' ) pylab.legend() pylab.show()
まとめ
数学を恐れないでください、時にはそれはあなたの人生を大いに単純化することができます！