既知の位置と加速度から速度を推定する

システムモデルのモデリング、つまり状態ベクトルと入力ベクトルの取得に行き詰まっています。私の推測では、位置と速度は状態ベクトルであり、加速度は入力ベクトルです。私の2番目の推測は、3つの量はすべて状態ベクトルにあり、入力ベクトルにはないということです。

だから...私の場合、状態ベクトルと入力ベクトルは何ですか？

追加情報：

位置センサーと加速度センサーから測定値を取得します。すべてが1Dで発生しています。たとえば、直線上です。これらの読み取り値をマージして（ノイズを除去して）、各タイムステップの速度の推定値を取得したいと考えています。

これらの方程式はシステムを記述します。彼らが正しくモデル化されているかどうかはわかりません。私が正しく理解していれば、加速度は一定であると予測しても安全です（実際には変化しますが）-プロセスの共分散行列がこの仮定を修正するためです（正しいですか）。ここに画像の説明を入力してください

使用するサンプルデータもいくつかあります（入力値はここでは簡単にするためにノイズを加えていません）。

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737

追加2：

より良いコミュニケーションのために、私は新しい回答を作成していますが、最初の回答へのコメントとして扱われるべきです。ジェイソン、あなたはすでにものすごく手助けしてくれました。あなたの時間に本当に感謝しています。ただし、これでもまだ問題があります。カルマンフィルターの結果は期待どおりではありません。時間がありましたら、以下をご覧ください。ありがとうございます。私はすでにあなたに1杯または2杯のビール（または必要に応じてコーヒー）を借りています-ペイパルがある場合はprimoz [at] codehunter.euで私に連絡してください:)

Jasonが最初の回答で提案したモデルを実装しました。ジャークを4番目の状態変数として追加しました。何時間もレビューした後、私はここに戻って助けを求めることにしました。KFから得られる値は期待どおりではありません。以下の表は、アルゴリズムの最初の10回の反復からのデータを表しています。ジャークが時間ステップごとに増加しているため、他の推定が間違っていることに注意してください。1秒後、実際の加速度と推定値の差は1m /s²を超えます（表の最後の行を参照）。

           real           measured                   estimated                    real 
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity 
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287

私のマトリックスはここにあります：

ジャークの各タイムステップでこの追加の原因は何ですか？私のマトリックスのいずれかが間違っていますか？

同じことが最初の解決策（3つの状態モデルのみ）にも当てはまります-加速は必要に応じて変化していません。

最終編集：

ようやくそれを機能させることができました。実装エラーや間違ったP＆Qマトリックスがあったかどうかはわかりません。

kalman-filters

— プリモシュ・クラリ
ソース

有用な回答を得るには、より多くの情報を提供する必要があります。システムについて説明してください。どのような方程式をまとめたかを示してください。

— ジム・クレイ

ご回答ありがとうございます。追加情報を提供しました。

— プリモジュKralj

誰かが私を助けてください-私は私の心を失っているように感じます。

— プリモジュKralj

明らかな問題はありません。実装エラーがある可能性があります。

— Jason R

@Phonon、難しい感情はありません。私も:)ジェットコースターにしてきたような気がします

— プリモジュKralj

回答:

この場合、一定の加速を仮定しません。システムの加速度を測定する手段がない場合は、通常、そうしますが、それはあなたのケースでは観察可能であると言います。このシステムをモデル化する最も明白な方法は、状態ベクトルを使用することです。

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$

ここで、は位置、は速度、は時刻でのシステムの加速度です。システムの位置と加速度を測定できると言うので、測定ベクトルは次のようになります。 $x_k$ $v_k$ $a_k$ $k$ $\mathbf{z_k}$

z_{k} = H x_{k} + v_{k}

$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$

と

H = [\begin{array}{cc} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$

結果として、測定モデルは次のようになります。

z_{k} = [\begin{matrix} x_{k} \\ a_{k} \end{matrix}] + v_{k}

$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$

ここで、は、測定に固有の（ガウス）ノイズです。ここで、ダイナミクスに影響を与える既知の力をシステムに追加しない限り、入力ベクトルます。カルマンモデルのさまざまな用語の詳細については、この最近の質問を参照してください。これらの用語について、知っておくべきことと知らないことがあるでしょう。 $\mathbf{v_k}$ $\mathbf{u_k} = \mathbf{0}$

編集：加速項の状態遷移行列を処理する方法に関する以下のコメントに関して：それを処理するにはいくつかの異なる方法があります。ゼロジャーク（ジャークは加速度の時間微分）モデルを想定できます。これは、と想定することと同じです。または、4番目の要素を状態ベクトルに追加できます。 $a_{k+1}=a_k$

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \\ \overset{⃛}{x_{k}} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \\ j_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$

ここで、は、システムのジャーク、または時刻での加速度の時間変化率です。ジャークを直接測定していないので、次の測定行列があります。 $j_k$ $k$

H = [\begin{array}{cc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$

そして、状態遷移行列は次のようになります。

F = [\begin{array}{cccc} 1 & d t & \frac{1}{2} d t^{2} & \frac{1}{6} d t^{3} \\ 0 & 1 & d t & \frac{1}{2} d t^{2} \\ 0 & 0 & 1 & d t \\ 0 & 0 & 0 & 1 \end{array}]

$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$

これは「コンスタントジャーク」モデルです（単純な例で見るかもしれない一定速度または一定加速度モデルに似ています）。状態遷移行列は、ジャークがkのすべての値に対して一定であるという暗黙の仮定を行います。これは本当ではないようです。問題のその側面を処理するには、状態遷移方程式のジャークコンポーネントにプロセスノイズ項を導入します。

定性的には、これにより、ジャーク項が時間ステップごとにどのように変化するかがわからないが、一部のガウス分布ではランダムな変化が予想されることを表現できます。これは、アプリケーションに適切に機能する一連のパラメーターが見つかるまで、このような適応フィルターモデルを「調整」するためによく使用されるツールです。

— ジェイソンR
ソース

ありがとう、あなたは本当に私の頭の中で光を照らしました。しかし、私はまだ次のステップで予想される私の加速が何であるかを把握していません（一定でない場合）-これは、遷移行列A（正確には最後の行）を構築するために重要です。あなたは何を提案しますか？

— プリモジュKralj

質問を移行することで全体の混乱があります。あなたの答えの一部がなくなっています。私は改造がこれを修理することを望みます。また、私の元の質問のアディション2も見てください。

— プリモジュKralj

編集したテキストを復元して、私の回答に再適用することができました。

— Jason R

最初の解（3つの状態変数）のプロセス共分散を取得する方法を説明できますか？オンラインでまともなリンクを見つけることはできませんが、実際にはそれを完全に説明しています。

— プリモジュKralj

私はあなたの状態変数がジェイソンの答えに従っていると仮定しています。その場合、問題はマトリックス（プロセスノイズ）であると考えられます。ジャーク更新を除いてどこでもゼロであるため、位置、速度、加速度の値を更新する唯一の項は、方程式を更新します（ジャークの確率的性質が反映される場合を除く）。 $Q$

次のような行列を使用してみてください。 $Q$

Q = [\begin{array}{cccc} 0.00001 & 0 & 0 & 0 \\ 0 & 0.00001 & 0 & 0 \\ 0 & 0 & 0.00001 & 0 \\ 0 & 0 & 0 & 0.001 \end{array}]

$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$

これらの対角要素がゼロの場合、モデルは「位置、速度、または加速度が変化することを期待していません（更新方程式以外による）」と言っています---これは明らかにそうではありません。

— ピーターK
ソース

いい説明ですが、なぜ0.001を選ぶのか、なぜ2.84を選ばないのでしょうか。

— プリモジュKralj

ありがとう！2.84より0.00001を選択する理由：まあ、2.84は分散に対して非常に大きいです。私はジャーク数（0.001）の背後に有効な推論があり、他の数はこれよりかなり小さいはずであると想定しました。ジャーク数を選択する実際の理由がない場合は、すべての対角要素で同じ値を試してください。

— Peter K.

しかし、これは実際にはどういう意味ですか？私の位置センサーに0.00001のばらつき（Qマトリックスの最初の診断要素）があると言った場合、それはたとえば、誤差が+-0.00001メートルであることを意味します（そうではないでしょうか）？精度の高いセンサー（位置）には小さい分散を選択し、精度の低いセンサー（加速度）には大きい分散を選択すると思います。

— プリモジュKralj

測定の分散がであるとすると、カルマンフィルターフレームワークでは、標準偏差がガウス分布であることを意味します。したがって、時間の〜68％、測定誤差の大きさは未満、時間の〜95％、未満などになります。

0.00001

$0.00001$

\sqrt{0.00001}

$\sqrt{0.00001}$

\sqrt{0.00001}

$\sqrt{0.00001}$

2 \sqrt{0.00001}

$2\sqrt{0.00001}$

— ジェイソンR

両方ありがとう、このスレッドが将来の訪問者にも役立つことを願っています。

— プリモジュKralj

問題はQにあると思います。QはKにその値をチャージしています。Qにdelta_tの累乗をプリロードすると、良い結果が得られます。

ウインナーモデルに関する記事を参照してください。それは良いQ行列を持っています。

A00はdelta_tの5乗の倍数である必要がありますA11 delta_tの3乗A22 delta_tの乗数1 A01 delta_tの乗数4の倍数

等々

見る

https://www.google.com/url?sa=t&source=web&rct=j&url=https://pdfs.semanticscholar.org/9c1b/db6b5a25c1231e775c654f731e1ca1fef31a.pdf&ved=2ahUKEwj4vJbQAVJBQBQVAVJVQVQAVJVQVJVQVQAVJVQVJVQVQVJ7VVB7VJBQVQVJzQVz7VJBQJVQVQJvQzzVQVQzVQzvzQVz7VzQVQQQQW

— ラムス
ソース