カルマンフィルタリングを簡単に理解する

重要な注意事項: この記事はオンライン情報から編集されたものであり、ブロガーが関連する知識ポイントを学習するプロセスを記録しているだけであり、削除されていません。

1. 参考資料

私が理解するカルマン フィルター、
カルマン フィルター、わかりやすいチュートリアル
カルマン フィルターの簡単な分析 カルマン
フィルターの仕組み、写真で
見る カルマン フィルターについて語る
Python コード例による拡張カルマン フィルター (EKF)
カルマンカルマンフィルター
アルゴリズム
のカルマン フィルター
移動ロボット技術の SLAM 派生 (7.5) -- 状態推定のためのカルマン フィルターと拡張カルマン フィルター

ビデオ素材

諦めから極めよう！カルマンフィルタリングの理論から実践まで～
[カルマンフィルタ]_カルマンフィルタ_ネットワーク全体における最も詳細な数学的導出.
カルマンフィルタリングをできるだけ簡単かつ詳細に説明するにはどうすればよいですか?

2. 関連紹介

確率分布のディリクレ分布 (事前、事後、尤度)

1. 確率分布

レーダー測距を例にとると、レーダー測定により、ミサイルが7.2mの位置で0.8の確率、7.2mの位置で0.1の確率、6.9mの位置で0.1の確率があることがわかります。を確率分布といいます。多数の値とそれらの値が発生する確率から構成されるデータは、確率分布と呼ばれます。

2. ノイズエラー

携帯電話のコンパスソフトを使用している場合は、動いていなくてもコンパスが一定の範囲内で変動し、体重計を使用している場合は、動いていませんが、体重計のデータが変化し続けます。上記の現象はノイズエラーです。LiDAR、ミリ波レーダー、温度センサーなどは使用中にノイズ誤差が発生します。

• 時刻 t にレーダーによって測定されたミサイルと目標の間の距離の確率分布は、$z_{}=N(7、0.1{\_}^2)$、0.1 はレーダーノイズ誤差です。
• 時刻 t-1 におけるミサイルの位置の確率分布は${バツ}_{t-}=N(10,0.2{\_}^2)$、0.2 は、最後の瞬間のミサイル位置のノイズ誤差です。

カルマン フィルタリングは、レーダー測定値と直前のミサイル状態データを融合することでノイズ誤差を低減するために使用されます。主に次の 2 つのステップに分かれます。

1. 最後の 1 秒のミサイルの位置に基づいて推定値現時点でのミサイルの位置を大まかに推定するには、ミサイルの速度 (均一運動を想定)推定値;
   $$時間tにおけるxのおおよその推定値={バツ}_{t-}-v_{t-}=N(10,0.2{\_}^2)-N(4,0.7{\_}^2)=N(6,0.2{\_}^2+0.7{\_}^2)$$

2. レーダーで測定されたミサイル位置測定そしてミサイルの位置の大まかな推定。推定値、ミサイル位置の正確な推定値を取得するために線形に重み付けされ、この値はと呼ばれます。最良の見積もり。

   これら 2 つの確率分布を単純に乗算して、正確な推定値の確率分布を取得します。
   $${バツ}_{}=時間t\text{における}x\text{のおおよその推定値}\times z_{}=N(6,0.2{\_}^2+0.7{\_}^2)\times N(7、0.1{\_}^2)$$

   $$\begin{array}{rl}& N(6,0.2{\_}^2+0.7{\_}^2)\times N(7、0.1{\_}^2)=N(6,0.53^2)\times N(7、0.1{\_}^2)\\ & =N(\frac{6\ast 0.1{\_}^2+7\ast 0.53^{}}{0.53^2+0.1{\_}^2}、\frac{0.1{\_}^2\ast 0.53^{}}{0.53^2+0.1{\_}^2}\end{array}$$

   $$\begin{array}{rl}& {バツ}_{}=N(\frac{6\ast 0.1{\_}^2+7\ast 0.53^{}}{0.53^2+0.1{\_}^2}、\frac{0.1{\_}^2\ast 0.53^{}}{{\left(0.53^2+0.1{\_}^2\right)}^2}）=N((1-\frac{0.53^{}}{0.53^2+0.1{\_}^2}）\ast 6+\frac{0.53^{}}{0.53^2+0.1{\_}^2}\ast 7、\\ & \frac{0.53^2+0.1{\_}^{}}{{\left(0.53^2+0.1{\_}^2\right)}^2}\end{array}$$

   ここで、$\frac{0.53^{}}{0.53^2+0.1{\_}^2}$呼ばれたカルマンゲイン。

3. ガウス分布

ガウス分布は次のようにも知られています。正規分布，记的$\text{N}(\mu ,p^2)$、$メートル、\sigma$は正規分布のパラメータであり、それぞれガウス分布の期待値と分散です。これら 2 つのパラメータが決定されると、確率$ｐ（ｘ）$も決定される。特に、$メートル=0、p^2=1$の場合、X の分布は次のようになります。標準正規分布。ガウス分布は平均位置で最も高い確率を持ちます。

確率変数のガウス分布の確率密度関数は次のとおりです。
$$N(x,メートル、s）=\frac{}{p\sqrt{午後2時}}{e}^{-\frac{(x-\mu {)}^{}}{2P{\_}^2}}$$

3.1 平均と分散

正規分布では、各確率変数は 分散は変数の不確実性の程度を表します。分散は信頼性として機能し、分散が大きいほど信頼性が低くなります。

平均$\mu$は数学的期待値$メートル=E\left(X\right)$，方差$D\left(X\right)=p^2=そして、\{[x-E\left(x\right){]}^2\}\; は$、確率変数の分散の尺度です。分散の平方根は標準偏差であり、これは$s$。

3.2 共分散

共分散行列の概要 (共分散行列)

共分散は、2 つの変数間の相関の程度を表します。数式は次のとおりです:
$$\begin{array}{rl}Cov\_\_\_(X,や）& =E\left[(X-E\left[X\right])\left(Y-E\left[Y\right]\right)\right]\\ & =E\left[XY\right]-2E\_\_\left[Y\right]E\left[X\right]+E\left[X\right]E\left[Y\right]\\ & =E\left[XY\right]-E\left[X\right]E[Y\end{array}$$

3.3 ガウス分布の乗算式

$$N（m_{}、p_1^{}）\ast N（m_{}、p_2^{}）=N(\frac{p_1^{}{メートル}_{}+p_2^{}{メートル}_{}}{p_1^{}+p_2^{}}、\frac{p_1^{}{p}_2^{}}{p_1^{}+p_2^{}}）$$

4. ベイズの公式

$$\text{事後確率}p(\theta |x)=\frac{\text{事前確率}p\left(\theta \right)\ast \text{尤度}p(x|\theta }{\text{合計確率}P\left(x\right)}$$

そのうち、確率の合計$P\left(x\right)$はθ \thetaに関連します$\theta \; は$独立した定数であるため、事後確率は事前確率と尤度の積に比例します。

5. 事前確率と事後確率

事前確率は、何かが起こる可能性に関する事前の客観的な評価であり、事後確率は、起こった何かが特定の理由によって引き起こされる確率です。

5.1 事前確率（事前）

事前確率$p\left(\theta \right)$ : イベントは発生していません。確率変数$\theta$が発生する確率。

5.2 事後確率（事後）

事後確率$p(\theta |x)$ : イベント$x$が発生した場合、確率変数$\theta$が発生する確率。

5.3 可能性

尤度確率$p(x|\theta )$ : 尤度関数は、特定の観測値が与えられた場合にモデル パラメーターが妥当であるかどうかを記述するパラメーターの関数です。あれは、「原因」から「結果」の可能性へ。

5.4 例

3 つのランダム イベント A、B、C があり、考えられる結果は D です。

• A: 雨が降りました。
• B: 渋滞。
• C: 遅く起きる。
• D: 遅れました。

A、B、および C の事象確率$P\left(A\right)、P\left(B\right)、P\left(C\right)$は先験的確率、事前確率は後の結果とは独立しています。

遅れたイベントが発生し、どのイベント (A、B、C) がそれを引き起こした確率が$P(A|D)、P(B|D)、P(C|D)$と呼ばれる事後確率。

イベント (A、B、C) が発生すると、D が遅れたという記述は$P(D|A)、P(D|B)、P(D|C)$と呼ばれる尤度確率。雨、交通渋滞、遅起きが遅刻の原因であると考えるのは、3 つの出来事のうちどれがより合理的ですか? 最も合理的な答えは、私たちがよく言うことです最大の可能性。

6. 実測値と予測値

予測値 (別名)評価、は数学モデルの予測値 (Prediction) です。

観察、としても知られています。実価はセンサー測定 (Measurement) です。例: LiDAR はロボットと障害物間の距離 7m を直接測定します。

7. フィルタリングとフィルター（フィルター）

filtering is weighting(フィルタリングとは重み付けのことです)。フィルタリングの機能は、さまざまな信号成分にさまざまな重みを与えることです。最も簡単な方法loss pass filterは、低周波信号に直接重み 1 を与え、高周波部分に重み 0 を与えることです。ウィーナー フィルタリングなどのより複雑なフィルタリングの場合は、信号の統計的知識に基づいて重みを設計する必要があります。

統計的信号処理の観点からは、ノイズリダクションは一種のフィルタリングとみなすことができます。ノイズリダクションの目的は、信号そのものを強調し、ノイズの影響を抑制することです。この観点から見ると、ノイズ リダクションとは、信号の重みを高くし、ノイズの重みを低くすることです。ウィナーフィルタは代表的なノイズ低減フィルタです。

7.1 ロボティクス分野におけるフィルタリングアルゴリズム

ロボット工学の分野におけるフィルタリング アルゴリズムとは、ノイズのあるロボット位置データからロボットの実際の位置を推定することを指します。一般的に使用されるフィルタリング アルゴリズムは、ベイジアン フィルタリングとカルマン フィルタリングです。

動作モデル（数理物理モデル）に基づいて、ロボットの姿勢や姿勢を大まかに推定することができます。推定値次に、フィルタリング アルゴリズムを使用して、この大まかな推定値と実際のセンサー (GPS、ジャイロスコープなど) を組み合わせます。測定Fusion はロボットの正確な位置と姿勢を取得できます推定値。

7.2 ローパスフィルター

低周波を通過させやすいフィルターをローパスフィルターと呼びます。

8.モバイルモデル

直前のロボットの位置情報と制御コマンドが与えられると、コマンド実行後のロボットの予想位置が一連の式によって計算されます。ここでの一連の計算式はモバイルモデルです。フィルタリング アルゴリズムを実装する前に、まずロボットの移動モデルをモデル化する、つまりロボットの移動モデルを確立する必要があります。一般的に使用されるロボットの動作モデルは次の 2 つです。走行距離ベースの運動モデル、速度ベースのモビリティモデル。

走行距離ベースのロボットモビリティモデル

あなたが普段乗っているタクシーには走行距離計が付いており、この走行距離計はタイヤがどれだけ回転したかを計測することができます。直感的に理解すると、走行距離は特定の指標の変化量であり、その変化とは距離や角度の変化です。

オドメトリベースの運動モデルが解決すべき問題は、直前の瞬間のロボットの位置と向きの角度がわかれば、直前の瞬間と現在の瞬間の間のロボットの予想される移動距離と角度がわかることです。変化があった場合には、現時点でのロボットの位置と姿勢角度を求めます。たとえば、直前のロボットの位置座標は (1, 0) であり、この期間の変化は (0.1, 0.2) であることがわかっています。モバイル モデルは、現時点でのロボットの位置推定値を計算します。( 1, 0) + (0.1, 0.2 ) + (確率変数、確率変数)。

9. 制御モデル

制御コマンドとロボットの位置情報を入力すると、制御モデルはコマンド実行後にロボットがどこまで進むと予想されるか、機体の向きがどの程度変化するかをロボットに伝えます。

10. 状態推定

10.1 問題の導入

簡単な例を使用して、状態推定問題を説明します。

一次元空間において、座標軸の正の方向に沿って座標$y_{}$看板。私たちは立ち止まり、距離計を使って看板までの距離を$d$ . つまり、現在の座標は$\times$って何ですか

既知の看板の座標は$y_{}$、標識までの距離$d$、座標$バツ=y_{}-d$。

しかし、測定器には誤差があり、計算結果は正確ではないため、次のように考えることができます。私たちは動いていないので、複数回測定して平均を取ることができます。これまでのところ、状態推定問題を完全に構築しました。

• 1次元空間で自分の座標を推定して$\times$；
• 答えを教えてください: 看板の座標$y_{}$、参考として使用できます。
• 測定器を使用して、自分から標識までの距離$d$、これを使用して自分の座標を逆計算します$\times$。

座標xxを取得するために無限の測定を行うのに無限の時間を費やすことはできないため、これを「推定」と呼びます。$x$の真の値。代わりに、測定の数が十分に大きい場合、結果は十分に正確である、つまり、何らかの方法で真の値に近づいていると考えられます。

10.2 状態推定に関する概念

上記の例は、離散時間、線形、時間不変のガウスシステムの状態推定とは何かを説明できます。具体的には、次のように説明できます。

• 離散時間: 測定方法が時間的に連続的ではないことを表します。

• Linear : センサー (距離計) の数学的モデルのタイプを説明します。基準物体の観測に関する方程式であるため、観測方程式・観測モデル（観測モデル）とも呼ばれます。yyを参照してください$y$と距離測定結果$d$、独自の座標を計算します$x$，つまり$バツ=f(y,d)=y-d$、モデルは線形関数。

• 時間不変: センサーの数学的モデルのマッピング関係は時間とともに変化しません。時不変とは、推定される状態を指すものではありません$x\; は$時間とともに変化せず、状態との$f:\times 、y\to dは$時間が経っても変化しません。簡単に言うと、これは観測方程式が時間とともに変化しないこと。例: 観測方程式$f(x,y）=y-x$は f ( x , y ) = y − 2 xf(x,y)=y−2x にはなりません。$f(x,y）=y-2x$およびその他のマッピング関係。

• ガウス分布: 測定ノイズの種類を表します。測定$n\; は$ガウス分布に従います。

  距離計の誤差、つまり測定ノイズがあることを考慮し、測定値$d$は実際には、真の値と測定ノイズを組み合わせた結果です。したがって、理論結果に基づいて$n$。式は次のとおりです。
  $$d=\underset{\text{理論上の結果}=d_{本当です\_}}{\underbrace{f(x,y}}+理想$$
  的な状況は、測定ノイズ n がガウス分布であることです。

10.3 大数の法則に基づく測定

ガウス分布に従う測定ノイズは確率変数であり、繰り返し測定すると、得られる結果は同じではなく、真の値を中心に変動します。ただし、測定回数$K$が十分に大きい場合、そのようなノイズ項の累積は平均値に近づく傾向があります。ノイズの平均が 0 の場合、正確な結果を近似的に取得できます。
$$\begin{array}{rl}{d}_{私は}& =\frac{}{K}\sum d_{}\\ & =d_{本当です\_}+\underset{\approx 0}{\underbrace{\frac{}{K}\underset{\approx 0}{}{n}_{}}}\\ & \approx d_{本当です\_}\end{array}$$

3. カルマンフィルタリングの巨視的理解

カルマン フィルター シリーズ 1——カルマン フィルター

カルマン フィルター アルゴリズムは再帰的予測フィルター アルゴリズムであり、データのフィルター処理と次の瞬間の予測を行うアルゴリズムです。

1. カルマンフィルターの導入

身近な小さな例でカルマン フィルタリングを紹介しましょう: コインと定規があります。その目的は、コインのサイズをできるだけ正確に取得することです。合計 k 回の測定が測定されます。各測定結果は記録されます。 as Z 1 $Z_{}、Z_{}、\dots Z_{}$。コインのサイズはどうやって推定するのでしょうか? 平均を取るのが自然な考え方ですが、計算プロセスは次のとおりです。

簡単に理解すると、ある測定の推定結果が前回の推定結果に関連しているということになります。これは、カルマン フィルターの再帰的な考え方を具体化したものです。以下のように表現：

現在の推定値 = 前回の推定値 + 係数 * (現在の測定値 - 前回の推定値)

$${\widehat{バツ}}_{}={\widehat{バツ}}_{k-}+\frac{}{k}(Z_{}-{\widehat{バツ}}_{k-}）$$

パラメータの説明

• ${\widehat{バツ}}_{}$k番目の推定値を表します
• $Z_{}$k 番目の測定値を表します。
• ${\widehat{バツ}}_{k-}$k-1番目の推定値を表します。

上の式からわかるように、k 番目の推定値 = k-1 番目の推定値 + $\frac{}{k}$(k番目の測定値 - k-1番目の推定値)。測定回数とともに k が増加すると、$\frac{}{k}$は 0 になる傾向があり、上記の式は次のようになります: ${\widehat{バツ}}_{}={\widehat{バツ}}_{k-}$つまり、k が増加するにつれて、k 番目の推定値は (k-1) 番目の推定値のみに関連し、k 番目の測定値とは関係がありません。測定結果は重要ではありません。実はこれも非常にわかりやすいのですが、何回も測定すると、実は前回の推定値が真の値に非常に近くなるので、もう一度測定する意味が薄れてしまうのですが、逆にkが非常に大きいと、小さい、つまり k =1 (k は測定回数を表し、k=0 は無意味です) の場合、上記の式は次のようになります: ${\widehat{バツ}}_{}=Z_{}$つまり、このときの推定値は測定値によって完全に決まることになります。

2. カルマン係数

上の式の$\frac{}{k}$K k \text{K}_kに置き換えられます${\text{K}}_{}$意味は、${\text{K}}_{}$あれはカルマン係数。

${\text{K}}_{}$計算式は下図のとおりです。

この式を巨視的に理解すると、次のようになります。

• 当$e_{}\gg e_{そして}$のとき、つまり推定誤差が測定誤差よりはるかに大きいとき、${\text{K}}_{}$1, ${\widehat{バツ}}_{}$Z k Z_{k}の傾向があります$Z_{}$、測定値を信じていることを示します。
• 当$e_{}\ll e_{そして}$のとき、つまり推定誤差が測定誤差よりはるかに小さいとき、${\text{K}}_{}$0, ${\widehat{バツ}}_{}$X ^ k − 1 \hat{\mathrm{X}}_{k-1}の傾向があります${\widehat{バツ}}_{k-}$、推定値の信頼性を示します。

3. カルマンフィルターデータ融合

この章では、簡単な例を使用して、カルマン フィルター データ フュージョンの役割を説明します。

対象物を同時に測定する電子秤と通常の秤があり、電子秤で測定した重量結果は Z1=30、標準偏差は σ 1 = 2 {\sigma _1} = 2 となります$p_{}=2$ ; 通常の秤で測定した重量結果は Z2=32、標準偏差は$p_{}=4$。

正規分布を満たすと仮定すると、2 つの秤で測定した物体の重量の分布図は次のように描くことができます。

上図Z1は電子秤の測定結果の分布を示し、Z2は通常の秤の測定結果の分布を示しています。計算を行う前に、まず実際の値を推測できると思います。上記から、通常のスケールの標準偏差が 4 であるのに対し、電子スケールの標準偏差はわずか 2 であることがわかります。これは、電子スケールの測定結果がより安定していることを意味し、測定結果が安定しない可能性があります。最終的に、フュージョン結果は電子スケール測定に偏っている可能性があり、結果は 30 に近くなります。もちろん、これらは私の推測にすぎません。次に、計算して、最適な推定値が推測と一致するかどうかを確認してみましょう。

最初の測定結果 Z1 を推定値として、2 番目の測定結果 Z2 を測定値として、推定値$\hat{Z}$の式は次のとおりです。
$$\hat{Z}=Z_{}+k\left(Z_{}-Z_{}\right)$$ Z ^ \hat{\mathrm{Z}}
を作るには$\hat{Z}$最適には、 Z ^ \hat{\mathrm{Z}}を作成する必要があります。$\stackrel{\sigma \; Z\; ^}{Z}$ \sigma_{\hat{Z}}の標準偏差$p_Z$最小値、つまり分散$v年\left(\hat{Z}\right)$は最小値であり、導出式は次のとおりです。

不定詞の場合: ${p_Z}^2={\left(1-k\right)}^2{p_{}}^2+k^{2P}{{\_}_{}}^2$ 、 σ Z ^ 2 {\sigma_{\hat{Z}}}^2 を作るために${p_Z}^2$が最小なので、k を見つける必要があります。上式をk = 0として導出し、極点を求めると次の図のようになります。

これまでのところ、決勝戦があります推定値そして標準偏差、この結果は、以前に推測したように 30 に近いことがわかり、この結果の標準偏差は前の 2 つの測定の標準偏差よりも小さいことがわかります。次の図に示すように、3 つの結果の分布図を描きます。

after_merge は融合後の結果を表しており、上図からわかるように、融合後の結果の分布は高く、薄くなり、効果が向上しています。

4. カルマンフィルタリングに対する一般的な理解

カルマン フィルタリングを簡単な言葉でできるだけ詳しく説明するにはどうすればよいでしょうか?

ブタを飼っているとします。

1週間前の豚の体重は46±0.5kgでした。ここで ±0.5 を使用していることに注意してください。これは、実際には 1 週間前のこの豚の体重についてあまり確信が持てないことを意味します。つまり、体重 46kg には 0.5kg の誤差があります。

さて、もう一週間飲みました。そこで、1週間でどのくらいの重さになるのか知りたいのですが、おおよその誤差はどのくらいですか?

1週間後の体重を求めるには、長年の養豚経験から得た豚の体重計算式に基づいて近似値を求める方法と、直接体重を測定する方法があります。もちろん、どちらの方法にも一定のエラーはあります。実験式で得られた重さは48kgで誤差2kg、直接計量で得られた重さは49kgで誤差1kgとします。

しかし、私は乙女座なので、経験式で求めた値にしても、直接秤量して得た値にしても、精度が足りないと感じています。1週間前の豚の体重、経験式で推定した値、直接計量で得た値を組み合わせて総合的に判断して豚の実際の体重に最も近い値を求める方法があればいいのですがそして誤差が最小になります。これがカルマン フィルタリングのタスクです。

次に、養豚モデルを数式に抽象化してみましょう。

上の図の左側では、前週の豚の体重は、時間 k-1 での状態値として抽象化でき、時間 k-1 での最適推定値に誤差項を加えたもので表されます。右側については当てはまります。ここで、
$$P_{}=E\left[e_{}{e}_k^{}\right]$$
この項は推定値の共分散を表します。ここで注意すべき点は 2 つあります。

1. 上の図のすべての変数は太字で示されており、それらがベクトルまたは行列であることを示しています。
2. 状態値を表すために数値の代わりに (列) ベクトルが使用される理由は、豚の体重は値で表すことができますが、実際のアプリケーション (ミサイルの場合など) では多くの状態を数値で表すことができないためです。空間内の位置には 3 つの座標 (x、y、z) があります。

上図の右側は時刻 k での状態値を表しており、この値は渡すことができます。予測モジュール（つまり、経験式に基づいて豚の体重を推定する）エラー訂正モジュール（つまり、豚の体重を直接計って）推定します。同様に、予測モジュールと誤り訂正モジュールにも対応するモジュールがあります。エラーそして誤差共分散行列。カルマンフィルタリングで行う必要があるのは、ベイズ推定の関連理論によると、予測モジュールと誤差補正モジュールの共分散を同時に考慮し、誤差が小さい項目にはより大きな重みを割り当て、誤差が大きい項目にはより小さな重みを割り当て、予測誤差を最小限に抑える。

具体的な実装プロセスは次のとおりです。

4. カルマンフィルターの導入

1 はじめに

カルマン フィルタリングは一連の再帰的な数式で記述され、プロセスの状態を推定し、推定された平均二乗誤差を最小限に抑えるための効率的で計算可能な方法を提供します。カルマン フィルターは広く使用されており強力であり、信号の過去の状態、現在の状態、および将来の状態を推定できます。

カルマン フィルター (KF) は、最適化推定アルゴリズム（最適推定アルゴリズム）は、ドローン、自動運転、衛星ナビゲーション、コンピュータビジョン、信号処理、およびロボットナビゲーション制御システム、誘導システム、センサーデータ融合システム、軍事レーダーシステム、ミサイル追跡システムなどの他の分野で一般的に使用されています。実際の機能は主に次のとおりです。センサー測定に基づいて予測を更新し、より正確な推定を実現します。

2. 適用可能なシナリオ

カルマン フィルターは確率変数の推定に適しています動的システムの最適な状態。観測されたシステム状態パラメータにノイズが含まれており、観測値が不正確な場合でも、カルマン フィルタリングは状態の真の値の最適な推定を完了できます。

カルマン フィルター アルゴリズムの本質は、2 つの正規分布の融合が依然として正規分布であるという特性を使用して反復することです。

3. ステータスオブザーバー

状態オブザーバー: 直接測定できないが間接的に測定できるいくつかの状態を最適化および推定するために使用されます。

シナリオ: ロケット噴射装置の内部温度の観察

現在、地球から火星に向けてロケットが飛行していますが、そのロケットは液体水素を燃料として使用していますが、過度の温度上昇はロケットの噴射装置の機械部品の損傷を引き起こすため、噴射装置の内部温度を監視することが非常に重要です。ただし、センサーはインジェクター内部への設置をサポートしていません。そうしないと溶けて消えてしまいます。しかし、インジェクターの外側に温度センサーを配置して、内部温度を間接的に測定することはできます。ただし、間接的な測定値と実際の値の間には誤差があるため、状態オブザーバーを使用して次のことができます。間接的な測定に基づいて真の値を推定する。

4. 最良の状態推定器

最適状態推定器: エラーの影響を受けたセンサー測定値から最適なシステム状態を推定します。

カルマン フィルター自体は最適状態推定器であり、前述の状態オブザーバーと似ていますが、KF が確率システム向けに設計されている点が異なります。状態オブザーバーと最良状態推定器 (KF) の予測値式は次のとおりです。
$$\text{国家監視員}{\widehat{バツ}}_{k+}=あ{\widehat{バツ}}_{}+ぶう{\_}_{}+K（{と}_{}-C{\widehat{バツ}}_{}）\text{決定論的システム}$$

$$\text{カルマンフィルター}{\widehat{バツ}}_{}=あ{\widehat{バツ}}_{k-}+ぶう{\_}_{}+K_{}(y_{}-C(A{\widehat{バツ}}_{k-}+ぶう{\_}_{}))\text{確率システム}$$

シナリオ: トンネル内の車両の位置を推定する

車がトンネルに入ると、車の GPS 受信機が衛星信号を受信することが困難になります。高層ビルに囲まれた道路を走行している場合、マルチパス効果（Multipath Effect、つまり信号の歪み）により測位ノイズが大きくなります。これは単純にセンサーの測定値が誤差の影響を受けるケースと考えることができますが、車両の真の位置を推定したい場合は、カルマン フィルターを使用することをお勧めします。

KFの目的は、GPSの計測値（Measurement）と数理モデルの予測値（Prediction）を組み合わせて、最適な自車位置の推定状態（自車位置）を見つけることです。

5. カルマンゲイン

例えば、ライダーで計測したロボットと障害物との距離は7m、精度は90%、速度から推定するとロボットと障害物との距離は6m、精度は80%となります。最終的な距離推定結果は次のようになります: result = ( 1
$$結果\_\_=(1-\frac{}{0.8+0.9}）\ast 6+\frac{}{0.8+0.9}\ast 7=6.52\text{メートル}.$$
このうち、$\frac{}{0.8+0.9}$呼ばれたカルマンゲイン、速度から計算された推定値に対するこのセンサー データの相対値を表します。信頼性。

6. その他のカルマンフィルター

カルマン フィルターは線形ですが、実際には、多くのデータ モデルは非線形です。現時点では、拡張カルマン フィルターを検討できます。さらに、興味のある友人は、図 12 に示すように、アンセンテッド カルマン フィルターと粒子フィルターについて学ぶこともできます。

5. カルマンフィルターライブラリ

シムドカルマン

シムドカルマン

Python はカルマン フィルタリングを実装します

【わかりやすい】嫦娥を天国に送ったカルマンフィルターのアルゴリズムを理解するには？

ミサイルが水平に移動するものとし、各瞬間のミサイルの速度を dv 、速度測定器の標準偏差を v_std、GPS センサーで測定した各瞬間のミサイル位置position_noise と GPS の分散をpredict_var とする。

# -*- coding: utf-8 -*-

import numpy as np


t = np.linspace(1,100,100) # 在1~100s内采样100次
a = 0.5 # 加速度值
position = (a * t**2)/2

position_noise = position+np.random.normal(0,120,size=(t.shape[0])) # 模拟生成GPS位置测量数据（带噪声）
import matplotlib.pyplot as plt
plt.plot(t,position,label='truth position')
plt.plot(t,position_noise,label='only use measured position')


#---------------卡尔曼滤波----------------
# 初始的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = 120**2 #这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
v_std = 50 # 测量仪器的方差（这个方差在现实生活中是需要我们进行传感器标定才能算出来的，可搜Allan方差标定）
for i in range(1,t.shape[0]):
  
    dv =  (position[i]-position[i-1]) + np.random.normal(0,50) # 模拟从IMU读取出的速度
    position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std**2 # 更新预测数据的方差
    # 下面是Kalman滤波
    position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var)
    # 高斯分布乘法公式
    predict_var = (predict_var * odo_var)/(predict_var + odo_var)
    predicts.append(position_predict)

    
plt.plot(t,predicts,label='kalman filtered position')

plt.legend()
plt.show()