カルマンフィルター-実装、パラメーター、チューニング

10

まず、カルマンフィルターを作成するのはこれが初めてです。

私は以前、次の質問を投稿しました。この投稿の背景を説明しているStackOverflowで、ノイズと速度値からの変動を除外します。これは私がフィルタリングしようとしている値の典型的なサンプルです。ここでは、必ずしも減少する必要はありません。しかし、変化率は通常、このようなものです

X ------- Y
16 --- 233.75
24 --- 234.01
26 --- 234.33
32 --- 234.12
36 --- 233.85
39 --- 233.42
47 --- 233.69
52 --- 233.68
55 --- 233.76
60 --- 232.97
66 --- 233.31
72 --- 233.99

このチュートリアルに従って、カルマンフィルターを実装しました：カルマンフィルター（ダミー用）。

私の実装は次のようになります（疑似コード）。

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

私は事前の推定値0から始めます。これは問題なく動作するようです。しかし、私が気づくのは、この更新が実行されるたびにkalmanGainが減少することです。つまり、フィルターを実行している時間が長くなるほど（？）、新しい値が信頼できるようになります。それは欲しくない。

私は移動平均（単純で指数加重）を使用するだけからこれを使用するようになりました。現在、私はそれほど良い結果を得ることができません。

私の質問は、これが適切な実装であり、以前のエラー分散と標準偏差が、投稿したサンプル値に従って適切に見えるかどうかです。私のパラメーターは、実際にランダムに選択されて、良い結果が得られるかどうかを確認しています。いくつかの異なる範囲を試しましたが、結果は良くありません。私ができる変更について何か提案があれば、本当にありがたいです。明らかな欠落があるとすいません。初めての投稿もこちら。

filters algorithms kalman-filters

— オレM
ソース

5

カルマンフィルターは、入力信号が線形動的システムの状態のノイズの多い観測で構成されている場合に役立ちます。システム状態の一連の観察を前提として、カルマンフィルターは、基礎となるシステムの状態のより良いより良い推定を再帰的に提供することを目的としています。正常に適用するには、状態を推定しているシステムのダイナミクスのモデルが必要です。Wikipediaで詳細に説明されているように、このモデルは、以前の状態、システムへの入力、およびプロセスノイズと呼ばれるガウス分布された確率的コンポーネントを前提として、システムの基礎となる状態が1つのタイムステップでどのように変化すると予想されるかを記述します。

そうは言っても、そのような基礎となるモデルがあるかどうかはあなたの質問からは明らかではありません。リンクされた投稿は、センサーからの速度値を消費していることを示していました。これらは、システムの状態の直接観測（状態はその速度）またはモデルの間接観測（状態がその位置など）としてモデル化できます。ただし、カルマンフレームワークを使用するには、時間の経過とともにその状態がどのように変化すると予想されるかについてのモデルを選択する必要があります。この追加情報は、最適な推定値を生成するために使用されます。カルマンフィルターは、適用される信号を単に「クリーンアップ」する魔法のブラックボックスではありません。

そうは言っても、カルマンフィルターが入力の観測値が次第に無視されるようになるまで、自身の出力にますます自信を持つようになるという現象は実際に起こります。これは、プロセスノイズ共分散行列の値を手動で増やすことで軽減できます。次に、定性的には、システムの状態遷移のモデルにはより大きな確率的要素が含まれるため、現在の状態が与えられた場合に推定器が次の状態を正確に予測する能力が低下します。これにより、システム状態の現在の推定値への依存が減少し、後続の観測への依存が増加し、「入力を無視する」効果が防止されます。

— ジェイソンR
ソース

+1：特に最後の段落。KF設計のノイズ共分散は、いじくる「つまみ」と考えてください。

— Peter K.

4

私がそれを正しく理解していれば、何かが動いていて、速度を観察でき、この速度はうるさいです。測定から、2種類の変動を観察します。\

ノイズによる変動
オブジェクトが本当に速度を変更しているための変化（例：回転）

カルマンゲインがゼロになる理由は、オブジェクトの速度が一定であると暗黙的に想定していて、この真の速度を推定することだけが必要なためです。

「ねえ、一定の速度で動いている物体があり、この一定の速度を推定したい」

$x_k$ $k$ $y_k$

{バツ}_{k} = {バツ}_{k - 1}

$x_k = x_{k-1}$

y_{k} = {バツ}_{k} + q_{k}

$y_k = x_k + q_k$

しかし、オブジェクトはそのように動いていません。スピードが変化しているので、いつ、どのように変化するかわかりません。

代わりに言う必要があるのは：

「ねえ、私は速度で動いている物体を持っていますが、それがどのように速度を変えているのかわかりません」

これを行うには多くの方法があります。最も簡単な方法は、状態に不確実性を追加することです。

{バツ}_{k} = {バツ}_{k - 1} + \underset{あなたは不確実性を追加します}{\underset{⏟}{v_{k - 1}}}

$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$

y_{k} = {バツ}_{k} + q_{k}

$y_k = x_k + q_k$

q_{k}

$q_k$

v_{k}

$v_k$

カルマンフィルターの方程式は次のようになります。

{\hat{y}}_{k | k - 1} = {\hat{バツ}}_{k | k - 1}

$\hat y_{k|k-1} = \hat x_{k|k-1}$

K_{k} = \frac{P_{k | k - 1}}{P_{k | k - 1} + Q^{o}}

$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$

{\hat{バツ}}_{k | k} = {\hat{バツ}}_{k | k - 1} + K_{k} （ y_{k} - {\hat{y}}_{k | k - 1} ）

$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$

P_{k | k} = P_{k | k - 1} - K_{k} P_{k | k - 1}

$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$

P_{k + 1 | k} = P_{k | k} + Q^{s}

$P_{k+1|k} =P_{k|k} + Q^s$

0.05 $Q^o$ $Q^s$

あなたのコードでは、わずかな変更は次のようになります：

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance $Q^s$

このstateVariance値は、何でもかまいません。これは、速度が実際にどの程度変化するかについてのあなたの自信に基づいています。速度がほぼ一定であると思われる場合は、これを小さい値に設定してください。

この方法では、カルマンゲインはゼロになりません。

— ssk08
ソース

3

カルマンフィルターを使用するには動的システムが必要です。

私は提案します

y = \sum_{i = 0}^{n} a_{i} x^{i}

$y = \sum\limits_{i =0}^n a_i\, x^i$

a [k + 1] = a [k] + w

$a[k+1] = a[k] + w$

c o v (w) = Q

$cov(w) = Q$

z = \sum_{i = 0}^{n} a_{i} x^{i} = y

$z = \sum\limits_{i =0}^n a_i\, x^i = y$

$x$ $a$

— アイアオ
ソース

1

PIDコントローラーなど、古典的な制御理論からのアイデアを使用できると思います。

信号Yは、コントローラーu（t）のセットポイントにすることができます。プロセスプラントは1であり、y（t）はフィルター処理された出力です。あなたがしなければならないすべてはあなたが望むものを得るためにパラメータ（調整）P、IとDを設定することです。

出力y（t）は入力u（t）を "追跡"しようとしますが、パラメーターはこの追跡がどのようになるかを制御します。

微分ゲインDは、応答を急速なエラー変化に敏感にします。あなたの場合、Dは小さいほうがいいと思います。u（t）が突然変化した場合、y（t）が変化しないようにする必要があります。

積分ゲイン 'I'は、蓄積されたエラーに対して応答を敏感にします。そこに高い値を入れるべきです。u（t）がレベルを変更してそこに保持すると、エラーが発生し、y（t）に同じようにさせます。

Pゲインは微調整できます。とにかく、パラメーターで遊んでみて、何が得られるか見てみましょう。

ただし、複雑な調整方法はいくつかありますが、必要になるとは思いません。

幸運を。

— ダニエル・R・ピパ
ソース

実際には、より良いアプローチがあります。この投稿を参照してください。

— ダニエルR.ピパ

この投稿を

— ダニエルR.ピパ2012

1

ウィキペディアのページのように表記法に従っているカルマンフィルターのシンプルでクリーンな実装を次に示します。https://github.com/zziz/kalman-filter

— fvzaur
ソース