自動運転のための知覚融合 - カルマンおよび拡張カルマン フィルター (Lidar&Radar)

著者 | ウィリアム編集者 | オートボット

元のリンク: https://zhuanlan.zhihu.com/p/138684962

「自動運転ハート」公開アカウントに注目するには下のカードをクリックしてください

ADAS ジャンボ乾物、手に入れられます

クリックして入ってください→オートパイロットの核心【SLAM】技術交流グループ

カルマン フィルターと拡張カルマン フィルター

カルマン フィルターは、自動運転の分野で最も一般的に使用されるデータ最適推定アルゴリズムです。多くの場合、人々の第一印象はその複雑な線形代数表現です。この記事では、初心者がカルマン フィルタリングに慣れ、マスターできるように、カルマン フィルタリングとその変形拡張カルマン フィルタリングの数学的原理とコード実装を紹介します。実際のプロジェクトでは、ライダーとミリ波レーダーの計測データを融合し、対象物の位置や速度を正確に追跡します。

Github: https://github.com/williamhyin/CarND-Extended-Kalman-Filter

電子メール: [email protected]

カルマンフィルター

1. まず第一に、カルマン フィルタリングとは何なのかを理解する必要があります。

Wikipedia の公式説明によると、カルマン フィルター (カルマン フィルター) は、時間の経過とともに観察される一連の測定値 (統計ノイズやその他の誤差を含む) を使用して未知の変数の推定値を生成します。多くの場合、各変数の同時確率分布を推定することで、より正確に推定できます。したがって、時間範囲にわたって、結果は単一の測定のみに基づく結果よりも正確になります。

少し複雑に聞こえますが、カルマン フィルターは、一連の不完全でノイズの多い測定から動的システムの状態を推定できる最適な推定アルゴリズムです。

2. 次に、カルマン フィルタリングがなぜ必要なのかを理解する必要があります。

カルマン フィルタリングを使用する基本的な理由は、センサーからの測定結果が 100% 信頼できるわけではないためです。エラーがない場合、またはエラーが予想されるしきい値をはるかに下回っている場合は、カルマン フィルタリングはまったく必要ありません。測定データを次の状態に更新します。

自動運転の分野では、カルマン フィルターは、LIDAR とミリ波レーダーのデータを機能レベルで融合して車の位置と速度を推定するために広く使用されています。距離、速度、角度などのさまざまなセンサーによって抽出された測定結果は、信号のドリフトやノイズの影響により不正確になることがよくあります。カルマン フィルターを使用すると、さまざまなセンサーからの測定値を、位置を予測する数学的モデルと組み合わせる方法が得られます。特定のセンサーまたはモデルをどの程度信頼するかに基づいて、測定値と推定値の間の信頼性の重みを更新し、正確な位置の最良の推定値をもたらします。

3. カルマンフィルタリングの手順は何ですか?

カルマン フィルターの操作は、予測更新の2 つのフェーズで構成されます。予測フェーズでは、フィルターは前の状態の推定結果を使用して現在の状態を推定します。更新フェーズでは、フィルターは現在の状態の観測値を使用して予測フェーズで取得した推定値を最適化し、現在の状態でより正確な新しい推定値を取得します。

センサーから新しいデータを取得するたびにこれら 2 つのステップを繰り返し、位置状態推定値と誤差共分散行列Pの精度を常に更新します。

26ebb912e2247a6a3ec1cab9444e0ac9.png

測定値の更新がベイズ規則に基づいていることは注目に値します前の瞬間の状態ベクトルの事前確率P(x)測定値の尤度 P ( z|x ) がある場合、尤度は現在の位置で取得できる測定値を指します。

01cea9b23f620de827eea5ca8b5f1a5a.png

P ( z ) が定数の場合、ターゲット位置の確率分布は実際には事後確率 P(x|z) を求めていることがわかります。

fdfb377ccd78e3ab7b634a94e8efc6f6.png

予測の更新は、合計確率式に基づいて行われます

f8e4fa7468c18ccdc6bbd07178f34f35.png

次の例では、確率密度関数を利用してカルマン フィルターがどのように機能するかを説明します。

最初のタイム ステップではk-1、ターゲットの位置は平均位置x_k-1を持つ。次に、運動モデルに基づいて予測し、時刻kでのx_k。予測自体には大きな不確実性があるため、予測位置の結果には大きな分散が生じます。同時にセンサーから測定値 y を取得すると、測定値の不確実性が少なくなるため、測定された位置結果の分散が小さくなります。カルマン フィルターは、予測と測定の 2 つの確率関数を乗算して最適な位置を推定します。数学的導出を通じて、乗算の結果が厳密な分布とより小さい分散を備えたガウス関数であることがわかり、これは最終的な位置推定精度が向上することを意味することに注目する価値があります。

6e10986091e976e7f7485f4ebb4e2a94.png

4. カルマン フィルタリングと隠れマルコフ モデルの関係は何ですか?

カルマン フィルタリングは、線形代数と隠れマルコフ モデル (隠れマルコフ モデル) に基づいて構築されています。その基本的な力学系はマルコフ連鎖で表すことができます。再帰的推定です。つまり、直前の状態の推定値と現在の状態の観測値がわかっていれば、現在の状態の推定値を計算できるため、記録する必要はありません。観測または推定の履歴情報。

d09fb9889cbb443e9cbc776998988729.png

カルマン ベクトルと行列

カルマンフィルターの予測ステップと測定ステップで相互に渡される値は次の 2 つです。

b9f86d32519786bd6125af97af0fffb7.png

以下はWikipediaにあるカルマンフィルターの各ベクトルと行列の数学的説明です、一見すると非常に複雑ですが、実際は慣れると式を設定するだけで済むことが分かります。各ベクトルと行列の意味については、次の章で詳しく説明します。以下、Xk-1|k-1をx、Xk|kをx'と表し、他の変数の同様の表現も同様の意味を有する

6f115c3e7a9e9267ab25f7517c045eb9.png

予測する

オブジェクトの現在の位置と速度がわかっていると仮定して、それを x 変数に保持します。さて、一秒が経過しました。1 秒前の物体の位置と速度、および等加速度運動の加速度 a がわかっているため、1 秒後の物体の位置を予測できます。オブジェクトが同じ速度で移動すると仮定すると、次のようになります。

86d24c3dc63b42bdc66cfef715a64b07.png

方程式により予測値が計算されます。

しかし、オブジェクトがまったく同じ速度を維持していない可能性があります。おそらく物体の方向が変わり、加速または減速したのでしょう。したがって、1 秒後の位置を予測すると、不確実性が高まります。

93b74d5e9cd6ecc33ca4c386c51a1365.png

不確実性の増加を表します。

各変数の意味を個別に説明します。

  • xは平均状態ベクトルで、時刻 t-1 における追跡対象の包括的な予測と測定によって得られた推定値が含まれます。推定値には 4 つの値が含まれます。すべてデカルト座標に基づきます(px, py, vx, vy)x位置と速度が平均 x のガウス分布で表されるため、「平均状態ベクトル」と呼ばれます。

  • Fは次の状態値を予測する状態遷移行列であり、これには現実的な線形運動モデルが必要です。

d9a7e5571bf0d23d21663a11328f6766.png

上記の線速度モデルによれば、次の状態遷移方程式が得られます。

5bbcfa31657fa8f5a6493033b81178f1.png

ここで F は、x‘時刻 t-1 における推定状態 x の下で、時刻 t における理論的に予測された状態に基づく状態変換行列であり、制御行列および制御変数とB呼ばれます。u等加速度運動に従うため、方程式の後にノイズ項を追加する必要がありますw

6f7aa41b8d423bb5840b98d16c2211c4.png

ただし、位置更新に対する加速度の影響を表すため、本プロジェクトの表現では省略しましたが、本プロジェクトでは等速モデルを使用しており、追尾対象物の正確な加速度を計測したり知ることができないBuため、のBu加速度を設定します。Bu=0効果はwノイズ項に追加され、vで表されます。

この方程式が実際には であることがわかりますx'= F * x +noiseこのノイズはプロセス ノイズと呼ばれ、モーション ノイズとも呼ばれます。プロセス ノイズは、オブジェクトの位置を予測する際のオブジェクトの位置の不確実性を表します。一定速度モデルを使用するため、状態予測コード内のこのノイズを完全に無視し、このノイズの影響をプロセス共分散行列に入れて、状態予測のQ不確実性を記述することができます。つまり、一定速度モデルの場合です。コードx'= F * x

  • Qはプロセス共分散行列で、プロセス ノイズに関連付けられた共分散行列です。

プロセス共分散行列の計算はもう少し複雑なので、以下で推測します。

上で、状態ベクトル x は位置と速度のみを追跡すると述べましたが、加速度は不明であるため、加速度をランダム ノイズとしてモデル化します。

このランダム ノイズは、上で導出された式の最後の項によって分析的に表現できます。vプロセス ノイズは、ランダムな加速ノイズ ベクトルとして理解できます。

a55caf32772a965f11f689574b7c838d.png

このベクトルは、平均 0 と共分散 Q を持つ 2 値の独立ガウス分布、つまり ν∼N (0,Q) で表すことができます。

v分解し続けることができる

f1b85b39131320686b2ac25bfac1bd36.png

delta_t は 2 つのカルマン フィルター ステップ間の時間差から計算されますが、加速度はax, ay平均 0 と標準偏差 のsigma_ax 和 sigma_ayランダム ベクトルです。

プロセス ノイズ ベクトルの期待値にノイズ行列の転置を乗算した値Qとして定義します。v

3c3134f7fd8ea6bdc7241bc168405e6a.png

G確率変数が含まれていないため、G期待値の計算を削除します。

f36a9a83bb779c3afdc162b369acbc18.png

ax,ayそれらは相互に相関していないため、これはさらに単純化sigma_axy = 0できることを意味します。Qv

f9850ab39ee95d43daa042e8fde3a245.png

Qこれを行列の計算式に当てはめると、次のようになります。

c23921d85c19b168efb25af708d0cee0.png

ご覧のとおり、Q時間の経過とともに位置と速度についてますます確実性が薄れるため、行列には​​多数の時間 delta_t エントリが含まれています。増分 t が増加するにつれて、より多くの不確実性が状態共分散行列 P に追加されます。

sigma_ax,sigma_ayこのプロジェクトで提供する実際の運動モデルに従って推定する必要があることに注意してくださいsigma_ax=sigma_ay =3

  • Pは状態共分散行列で、オブジェクトの位置と速度の不確実性に関する情報が含まれており、一般に分散が含まれていると考えられます。しかし、なぜそれが共分散行列と呼ばれるのでしょうか?

例を通して意味を理解しますP

状態ベクトルの要素はn1 つなので、p 行列は ですn x n = 4 x 4Py位置の大きさが大きくなると、Vx速度の大きさも大きくなり、共分散が生じることPyを意味します。Vx状態要素が互いに影響を及ぼさない場合、それらの共分散はゼロであると言えます。下図に示すように、ターゲットが水平または垂直に移動し続け、相互作用せず、共分散は 0 になりますが、ターゲットが斜めに移動すると、 と はPx相互影響し、共分散は 0 ではありません。 。PyPxPy

c537ba074914875c7fd761d3cf849969.png

Pの対角線には要素自体の分散が含まれており、値が大きいほど代表状態要素の不確実性が大きくなります。対角線の周りには 2 つの状態要素間の共分散があり、状態要素の値が互いにどのように「共変動」するかを示しています。このような共分散行列 (以下のR, S, Q) はすべて対称行列です。Pyつまり、Vxと の共分散はVxPyの共分散でもあります。以下の表に示すように:

12362f398d7a8e0b3367f3eb208dc491.png

P 行列の各コンポーネントの意味を理解した後、適切なパラメータを選択して P 行列を初期化できます。フィルターの正確な初期位置がわかっている場合は、エントリがゼロの初期共分散行列を与えることができます。好き

b24e8685be2786b3abe3165f36ba2853.png

ただし、このような状況は比較的まれであり、一般に、最初の予測は既存の測定結果に基づいており、多くの場合、ノイズが含まれています。

この場合、 Radar または Lidar の測定ノイズ分散を使用して行列を初期化してみることができますP

初期の位置と速度が正確に分からない場合、共分散行列は、対角要素が B である行列として初期化できます。B は適切で比較的大きな数になります。このプロジェクトのように、初期状態の位置の合計はわかっているだけでPxPy速度のVx合計はわかっていませんがVy、この場合、Vx-variance と-variance にVy大きな初期値を設定できます。

初期化コードは次のとおりです。

is_initialized_ = false;
previous_timestamp_ = 0;
// initializing matrices
R_laser_ = MatrixXd(2, 2);
R_radar_ = MatrixXd(3, 3);
H_laser_ = MatrixXd(2, 4);
Hj_ = MatrixXd(3, 4);
//measurement covariance matrix - laser
R_laser_ << 0.0225, 0,
0, 0.0225;
//measurement covariance matrix - radar
R_radar_ << 0.09, 0, 0,
0, 0.0009, 0,
0, 0, 0.09;
// Initialize P
ekf_.P_ = MatrixXd(4, 4);
ekf_.P_ << 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1000, 0,
0, 0, 0, 1000;
H_laser_ << 1, 0, 0, 0,
0, 1, 0, 0;

予測更新コードは次のとおりです。

x_ = F_ * x_;
MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;

測定値の更新

2 つの測定センサーからデータを取得します。

  • レーザー測定

  • レーダー測定

  1. レーザー測定

このプロジェクトでは、レーザー測定によって物体の現在位置が取得されますが、物体の速度は得られません。レーザーはレーダー センサーよりも空間分解能が高く、レーザー センサーは速度も計算できますが、精度はレーダーほど高くありません。

更新 x と p を測定するための式を見てみましょう。

c8bedab13ccc351d5481f0c552f4278e.png

各変数の意味を個別に説明します。

  • 測定ベクトルz

LIDAR の場合、Z ベクトルには位置 x と位置 y での測定値が含まれます。

7bff51f0aea248e06f72879c2e68e259.png
  • 変換行列 H

H行列は、理論的に推定されたターゲットの状態ベクトルをx’センサーの計測空間に投影する行列です。LIDAR の場合、LIDAR センサーは位置のみを測定するため、状態変数から速度情報を破棄します。つまり、状態ベクトルにはx[px,py,vx,vy] に関する情報が含まれますが、zベクトルには [px,py] のみが含まれます。変換 H を乗算すると、センサーの実際の測定値zと理論的に推定された状態ベクトルx‘の間の残差が計算されます。

ライダーの場合:

1b610f6d941e91689fa9def1a496f43f.png
  • 測定ノイズ共分散行列 R

Rマトリックスはセンサー測定の不確実性を表します。R マトリックスは正方形で、マトリックスの各辺に同じ数の測定パラメータがあります。

LIDAR の場合、2D 測定ベクトルがあります。各位置コンポーネントpx,pyはランダム ノイズの影響を受けます。したがって、ノイズ ベクトルzωと同じ次元になります。測定ノイズ ω は、ゼロ平均と共分散 R の分布、つまり ω~N(0,R) です。したがって、R行列は平均がゼロの 2 x 2 共分散行列であり、ノイズ ベクトル ω とその転置の積の期待値です。

b37d6c88e132fd54b83526b5ddc48a43.png

非対角線 0 は、ノイズが互いに相関していないことを示していることに注意してください。

通常、ランダム ノイズ測定行列のパラメータはセンサー メーカーによって提供されますが、このプロジェクトではレーダー センサーとレーザー センサー用のR行列が提供されています

  • 測定残差共分散行列 S

理論上の推定値と測定値の間の誤差は、共分散行列 S によって表すこともできます。

2e4bde79e86a5667813a0fc6155c86e0.png
  • 測定残差ベクトル y

y ベクトルは、実際の測定値と予測推定値の間の残差です。

  • カルマンゲイン K

カルマン フィルターの考え方は、予測推定値と実際の測定値にそれぞれ重みを与え、予測推定値と実際の測定値の重み付き線形結合を通じて推定値を取得することです。

3febd1e9593b440ab12a1b94df95b63e.png

カルマン ゲイン kは、予測推定値と現在の観測値の重み情報をx‘含むベクトル y に与えられる重みであり、状態ベクトルと状態共分散行列を更新し続けるために使用されます一般に、重みが低いほど、より高い不確実性が割り当てられます。つまり、センサーの測定値が非常に不確実な場合 (R が P' より大きい場合)、カルマン フィルターは状態推定値とみなすものにより大きな重みを与えます状態推定が不確実な場合 (P' が R より大きい場合)、カルマン フィルターはセンサー測定値により多くの重みを与えますzxPx′z

LIDAR 更新コードは次のとおりです。

void KalmanFilter::Update(const VectorXd &z) {
      /**
       * TODO: update the state by using Kalman Filter equations
       */
      VectorXd z_pred = H_ * x_;
      VectorXd y = z - z_pred;;
      MatrixXd Ht = H_.transpose();
      MatrixXd S = H_ * P_ * Ht + R_;
      MatrixXd Si = S.inverse();
      MatrixXd K = P_ * Ht * Si;
  
      x_ = x_ + K * y;
      int x_size = x_.size();
      MatrixXd I = MatrixXd::Identity(x_size, x_size);
      P_ = (I - K * H_) * P_;
  }
  1. レーダー測定

レーザーセンサーは正確な歩行者の位置データを取得できますが、実際に歩行者の速度を直接観測することはできないため、現時点ではレーダーが必要です。ドップラー効果に基づいて、レーダーは移動する物体の半径方向速度の直接測定値を取得できます。半径方向速度とは、物体がセンサーに向かって、またはセンサーから遠ざかる速度を指します。しかし、レーダーが提供する測定結果は、距離、角度、距離率を含む極座標値であり、X 状態ベクトルが位置するデカルト座標に変換する必要がありますが、この変換は非線形であり、従来の方法では実現できません。この部分は拡張カルマン フィルタリングで詳しく説明されています。

拡張カルマンフィルター

上では、レーザー測定値をカルマン フィルターに統合しましたが、レーダー測定値は同じように統合されません。

その理由は、レーダーによる世界の見方が異なるためです。

e21a17a0ee8bfe3d7a6739d8ec4bde61.png
  • 距離 ρ は原点からターゲットまでの半径方向の距離であり、 として定義できますρ=sqrt(px2+py2)

  • 方向角度 φ 線と x 方向の間の角度φ=atan(py/px)φ は x 軸上で反時計回りに正の方向に参照されるため、上の図では実際には負の値になることに注意してください。

  • 距離変化率 ρ は、光線 L に沿った速度 v の投影であり、動径速度とも呼ばれます。

レーザー測定の行列Hとレーダー測定の方程式は、h(x)実際には同じ仕事をしています。どちらも更新ステップで方程式を解く必要があります*y* = z - Hx'

しかし、レーダーの場合、状態ベクトルを極座標にマッピングするH行列が存在しないため、マッピングを手動で計算してデカルト座標を極座標に変換する必要があります。x

以下は、h(x)予測された位置と速度を距離、角度、距離レートの極座標にマッピングする方法を指定する関数です。

a6ab62e7ea44ad8480062188f0c64d28.png

次の図は、デカルト座標を極座標に変換する方法を示しています。

33fe140396e513c267d68dc431074d4a.png

したがって、レーダーの場合、y = z - Hx '方程式は次のようになりますy = z - h(x')

非線形測定関数ができたので、予測された状態をh(x)新しい測定値で更新したい場合、カルマン フィルター方程式を引き続き使用できますか?zx*

答えはノーで、その理由は次のとおりです。予測された状態はxすでにガウス分布で表されており、その分布を上記の非線形関数にマッピングするとh(x)、結果の関数はガウス分布になりません。

3638df426e0a06a8415ecbb4ceaca6f5.png

カルマン フィルターはガウス関数でのみ機能することに注意することが重要です。

このとき、非線形処理関数:拡張カルマンフィルターを導入する必要があります。

h(x)関数の線形化は拡張カルマン フィルター (EKF) の鍵です。

EKF は、現在の推定値の平均値付近の分布を線形化します。つまり、元のガウス分布の平均位置で、h(x) に接する線形関数を使用して測定値関数を近似し、カルマン関数で近似します。フィルター アルゴリズム この線形化は、状態の予測と更新に使用されます。

1ef34d47fb11fb1f7589d994ea8efac6.png

上の図に示すように、非線形関数の分布はガウス分布ではありません。

下図では、非線形関数を近似的に線形化した分布がガウス分布になります。

1647e8999e9d7f0bb7a04f6b114077cc.png

EKF は、関数を線形化するために「一次テイラー展開」と呼ばれる方法を使用します。

e609933edc2c5f01f59f59f6123f9826.png

に対応する導関数はxヤコビ行列と呼ばれ、すべての偏導関数が含まれます。ヤコビアン行列の導出は少し複雑なので、ここで結果を直接与えます。興味のある友人は、h(x) の偏導関数を 1 つずつ見つけることができます。

477e01921e6ece38c0d21aeda83cf431.png

数学的な証明は複雑に見えるかもしれませんが、カルマン フィルター方程式と拡張カルマン フィルター方程式は非常に似ています。

bd380e41da631d2f3aa6d34d1099e306.png

主な違いは、予測と更新が非線形モデル変数に対してのみ個別に実行される場合です。

  • P' を計算するとき、F 行列は Fj に置き換えられます。

  • S、K、P を計算するとき、カルマン フィルターの H 行列はヤコビ行列 Hj に置き換えられます。

  • x' を計算するには、F 行列の代わりに予測更新関数 f を使用します。

  • y を計算するには、H 行列の代わりに h 関数を使用します。

非線形モデルの推定と最適化のみを行う場合は、予測と測定更新の両方のステップで非線形モデル方程式を使用する必要があります。つまり、予測では F 行列の代わりにそのヤコビ行列 Fj を使用する必要もあります。たとえば、位置と速度の最適な推定を完了するためにのみレーダーを使用します。

ただし、このプロジェクトでは、レーザー センサーとレーダー センサーを融合することが目的であるため、レーザー レーダーは線形モデルを使用するため、2 つのセンサーのそれぞれの予測に f 関数または Fj 関数を使用せず、依然として従来のカルマン フィルター方程式と F 行列。更新を測定するステップでは、異なるセンサーに応じてカルマンフィルター方程式または拡張カルマンフィルター方程式が選択されます。

レーダー更新コードは次のとおりです。

void KalmanFilter::UpdateEKF(const VectorXd &z) {
    /**
     * TODO: update the state by using Extended Kalman Filter equations
     */
    double px = x_(0);
    double py = x_(1);
    double vx = x_(2);
    double vy = x_(3);
    // Coordinate conversion from Cartesian coordinates to Polar coordinates
    double rho = sqrt(px*px + py*py);
    double theta = atan2(py, px);
    double rho_dot = (px*vx + py*vy) / rho;
    VectorXd h = VectorXd(3);
    h << rho, theta, rho_dot;
    VectorXd y = z - h;
    // Nominalization of angle
    while (y(1)>M_PI) y(1)-=2*M_PI;
    while (y(1)<-M_PI) y(1)+=2*M_PI;

    MatrixXd Ht = H_.transpose();
    MatrixXd S = H_ * P_ * Ht + R_;
    MatrixXd Si = S.inverse();
    MatrixXd K =  P_ * Ht * Si;

    x_ = x_ + (K * y);
    int x_size = x_.size();
    MatrixXd I = MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;

}

ここまでで、カルマン フィルタリングと拡張カルマン フィルタリングの原理の説明を完了しましたが、次に、レーザーとレーダー センサーの融合プロセスを見てみましょう。

センサーフュージョンフレームワーク

762ba11075e605792bb854ff37793826.png

レーザーとレーダー センサーの融合の全体的なプロセス フローは次のとおりです。

  • 最初の測定- カルマン フィルターは、自動車に対するターゲットの位置の初期測定値を受け取ります。これらの測定値は、レーダーまたはライダー センサーから取得されます。

  • 状態と共分散行列の初期化- カルマン フィルターは、最初の測定に基づいてターゲットの位置を初期化します。

  • Δt 時間後、車は別のセンサー測定値を受信します。

  • 予測- アルゴリズムは時間 Δt におけるターゲットの位置を予測します。バイクの位置を予測する 1 つの方法は、バイクの速度が一定であり、Δt 時間後に物体が一定のv * Δ t距離を移動すると仮定することです。この拡張カルマン フィルタリング プロジェクトでは、速度が一定であると仮定します。

  • update - フィルターは、予測された位置とセンサーの測定値を比較します。予測された位置と測定された位置が組み合わされて、更新された位置が得られます。カルマン フィルターは、各値の不確実性に応じて、予測された位置または測定された位置により多くの重みを置きます。

  • 現在の観測データがレーダー センサーからのものである場合、レーダーのH合計R行列を使用して拡張カルマン フィルターを設定し、新しいヤコビ行列を計算し、Hj非線形関数を使用して座標系を変更し、測定更新を呼び出す必要があります。

  • 現在の観測が LIDAR からのものである場合、LIDAR のH合計R行列を使用してカルマン フィルターを設定し、measureupdate を呼び出します。

  • Δt 時間後、自動車は別のセンサー測定値を受信します。アルゴリズムは次の予測更新ステップの実行を開始します。

6d1c3fbeb1132fb12c1ede1608897b53.png

カルマンフィルターの性能評価

追跡アルゴリズムを実装した後、アルゴリズムのパフォーマンスを調べて、推定結果が実際の結果とどの程度異なるかを評価する必要があります。

最も一般的なチェック基準は、二乗平均平方根誤差 (二乗平均平方根誤差) と呼ばれます。

01713381315d2ec9c32776aaf150d127.png

推定された状態と真の状態の差は残差と呼ばれます。二乗平均平方根誤差は、これらの残差を二乗して平均することによって取得されます。

このプロジェクトでは、実際の状態の値が与えられています。

二乗平均平方根誤差の評価コードは次のとおりです。

VectorXd Tools::CalculateRMSE(const vector<VectorXd> &estimations,
                              const vector<VectorXd> &ground_truth) {
  /**
   * TODO: Calculate the RMSE here.
   */
    VectorXd rmse(4);
    rmse << 0,0,0,0;
    if (estimations.size() != ground_truth.size()|| estimations.size() == 0) {
     cout << "Invalid estimation or ground_truth data" << endl;
        return rmse;
    }

    for (unsigned int i=0; i < estimations.size(); ++i) {

        VectorXd residual = estimations[i] - ground_truth[i];

        // coefficient-wise multiplication
        residual = residual.array()*residual.array();
        rmse += residual;
    }

    // calculate the mean
    rmse = rmse/estimations.size();

    // calculate the squared root
    rmse = rmse.array().sqrt();

    // return the result
    return rmse;
}

結論

  • カルマン フィルタリングは計算量が少ないですが、線形問題のみを解決できるため、非線形問題の場合は拡張カルマン フィルタリングなどのアルゴリズムを使用する必要があります。

  • 拡張カルマン フィルタリングは非線形問題を解決できますが、それでもいくつかの欠点があります。

  • ヤコビ行列を解析的に求める必要がある場合、計算は非常に困難になります。

  • ヤコビアン行列を数値解析により求めると、計算コストが非常に大きくなる。

  • 拡張カルマン フィルターは、微分可能なモデルを備えたシステムにのみ適しています。

  • レーダー測定は非線形であるため、レーダー測定の更新

次のブログでは、非線形モデルを処理するためのカルマン フィルターのより強力なバリアントであるアンセンテッド カルマン フィルター(UKF) を紹介します。

実際、無香カルマン フィルターは味のないカルマン フィルターに変換されるべきです。これは、作者が拡張カルマン フィルターでは退屈すぎると考えたため、無香カルマン フィルターを発明したためです:)!

引用:

  • ユダシティ

  • ウィキ

(1)動画講座はこちら!

自動運転の心臓部は、ミリ波レーダービジョンフュージョン、高精度地図、BEV知覚、マルチセンサーキャリブレーション、センサー展開、自動運転協調知覚、セマンティックセグメンテーション、自動運転シミュレーション、L4知覚、意思決定計画、軌道予測を統合します。 、など、各方向の学習ビデオ。ご自身で受講することを歓迎します (コードをスキャンして学習を入力してください)。

c4d4c0600fef861feacff10508fdc737.png

(コードをスキャンして最新のビデオをご覧ください)

動画公式サイト:www.zdjszx.com

(2) 中国初の自動運転学習コミュニティ

1,000 人近くのコミュニケーション コミュニティと 20 以上の自動運転技術スタックの学習ルートが、自動運転の認識 (分類、検出、セグメンテーション、キー ポイント、車線境界線、3D 物体検出、占有、マルチセンサー フュージョン、物体追跡、オプティカル フロー推定、軌道予測)、自動運転位置決めとマッピング(SLAM、高精度マップ)、自動運転計画と制御、フィールド技術ソリューション、AI モデル展開の実装、業界トレンド、求人リリース、スキャンへようこそ以下の QR コード、自動運転の中心となるナレッジ プラネットに参加してください。ここは本物の乾物がある場所です。この分野の大手の人々と、仕事の開始、勉強、仕事、転職に関するさまざまな問題を交換し、論文 + コードを共有します。毎日+ビデオ、交換を楽しみにしています!

da57890774284aef5efa4c5d6b4352ae.jpeg

(3) 自動運転の心臓部】フルスタック技術交流会

The Heart of Autonomous Driving は、物体検出、セマンティック セグメンテーション、パノラマ セグメンテーション、インスタンス セグメンテーション、キー ポイント検出、車線境界線、物体追跡、3D 物体検出、BEV 認識、マルチセンサー フュージョン、 SLAM、光流推定、深さ推定、軌道予測、高精度地図、NeRF、計画制御、モデル展開、自動運転シミュレーションテスト、プロダクトマネージャー、ハードウェア構成、AI求人検索とコミュニケーションなど。

0538359b34ce819e96aff8589aff9c21.jpeg

Autobot Assistant Wechat への招待を追加してグループに参加します

備考:学校/会社+方向+ニックネーム

おすすめ

転載: blog.csdn.net/CV_Autobot/article/details/131356284