ロボットの位置決めでは、KF または EKF が一般的に使用されるセンサー フュージョン アルゴリズムです。EKF の使用方法については、以前に「データ フュージョンを実現するためのカルマン フィルター (カルマン フィルター) を理解する方法」でまとめました。
わかりやすく、理解しやすいです。拡張カルマンフィルター EKF は複数のソースに使用されます。
ほとんどがMATLABベースで書かれているので、時間があるのでC++ベースの計算方法を簡単に書いてbflと比較してみます。
簡単に言うと、EKF は予測と更新の 2 つのプロセスに分かれています。予測部分では、一般に IMU や Odom データなどの比較的データ頻度の高いセンサーが使用されます。効果を比較するために、bfl の例でシミュレートされたセンサー データが使用されます。ここで、まず bfl の例を簡単に分析します。
for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
{
auto start = std::chrono::steady_clock::now();
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);
// DO ONE MEASUREMEN
ColumnVector measurement = mobile_robot.Measure();
// UPDATE FILTER
// filter.Update(&sys_model,input,&meas_model,measurement);
filter.Update(&sys_model,input);
filter.Update(&meas_model,measurement);
measurement = mobile_robot.Measure();
x = filter.PostGet()->ExpectedValueGet();
covarLabelMeas = filter.PostGet()->CovarianceGet();
cout << " ExpectedValueGet = " << x <<" measurement = " << measurement << endl;
} // estimation loop
このうちmobile_robot.Move(input)、シミュレーションによりセンサーデータを取得し、前方への距離をシミュレーションします。そしてmobile_robot.Measure()その瞬間の観測値を求めることです。
次に、EKF で定義されたモデルを直接呼び出すことができます ( はfilter.Update(&sys_model,input)予測値の更新、 はfilter.Update(&meas_model,measurement)観測値の更新です。違いはモデルの確率分布であり、その関数は bfl ライブラリでオーバーロードされます)。
更新後、filter.PostGet()->ExpectedValueGetを通じて EKF で最適な状態を取得できます。
以下は、主に事前分布、システム モデル、観測値を含むモデルの共分散分布の概要です。簡単に言うと、初期位置が正確かどうか、予測プロセスでどの程度の誤差が生じるか、センサーのハードウェアの問題によって観測データに誤差が生じるかどうか、ということです。
先験的な共分散:
ColumnVector prior_Mu(2);
prior_Mu(1) = PRIOR_MU_X;
prior_Mu(2) = PRIOR_MU_Y;
SymmetricMatrix prior_Cov(2);
prior_Cov(1,1) = PRIOR_COV_X;
prior_Cov(1,2) = 0.0;
prior_Cov(2,1) = 0.0;
prior_Cov(2,2) = PRIOR_COV_Y;
Gaussian prior(prior_Mu,prior_Cov);
システムノイズ:
// create gaussian
ColumnVector sysNoise_Mu(2);
sysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
sysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;
SymmetricMatrix sysNoise_Cov(2);
sysNoise_Cov = 0.0;
sysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
sysNoise_Cov(1,2) = 0.0;
sysNoise_Cov(2,1) = 0.0;
sysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
// create the model
LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
観測ノイズ:
// Construct the measurement noise (a scalar in this case)
ColumnVector measNoise_Mu(1);
measNoise_Mu(1) = MU_MEAS_NOISE;
SymmetricMatrix measNoise_Cov(1);
measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
// create the model
LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
最後のステップは観測行列の構築ですが、簡単に理解すると、観測データとカルマン フィルタリングに必要なデータの間の変換関係がわかります。
// create matrix H for linear measurement model
Matrix H(1,2);
double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
H = 0.0;
H(1,1) = wall_ct * RICO_WALL;
H(1,2) = 0 - wall_ct;
例えば観測値は壁からの距離ですが、KFで保持される状態量は座標値(x,y)なので、距離を座標値に変換する必要がありますが、ここで定義する運動モデルはしたがって、これらの変換は運動学モデルの「Did it」と同等です。この例のモーション モデルは次のとおりです。
// Create the matrices A and B for the linear system model
Matrix A(2,2);
A(1,1) = 1.0;
A(1,2) = 0.0;
A(2,1) = 0.0;
A(2,2) = 1.0;
Matrix B(2,2);
B(1,1) = cos(0.8); // 0.69670670934717
B(1,2) = 0.0;
B(2,1) = sin(0.8); //0.71735609089952
B(2,2) = 0.0;
上記は、bfl ライブラリで行われる融合プロセスです。それでは、式に従ってKFフィルターを自分で実装してみましょう。
まず、運動方程式は元から変更されておらず、次の方程式が予測の更新に使用されます:
xk = A k xk − 1 + B k uk + wk x_k = A_k x_{k−1} +B_k u_k +w_kバツｋ=あｋ×k − 1+Bｋあなたｋ+wｋ
状態の更新に加えて、現在の状態の信頼性である状態の共分散も更新する必要があります。状態遷移方程式とも呼ばれます。ここで、Q k Q_kQｋそれはシステムのノイズです。
P k = A k ∗ P k − 1 ∗ AT + Q k P_{k} = A_k*P_{k-1}*A^T + Q_kPｋ=あｋ∗Pk − 1∗あT+Qｋ
以上で予測部分は完了です。次は融合プロセスともいえる更新プロセスです。2 つのガウス分布が融合されます。最初のステップはカルマン ゲインの計算です。どちらをより信じるかという問題です。 : K = P k ∗
HH ∗ P k ∗ HT + R k K = {P_k*H \over H*P_k*H^T + R_k}K=H∗Pｋ∗HT+RｋPｋ∗H
ここでP k P_kPｋは KF で現在維持されている状態の信頼性、R k R_kRｋは観測値の信頼性、そして計算されたKKKは重みに相当します。
重みを使用すると、誰をより信頼すべきかがわかります。観測値をより信頼すると、KF で維持される状態は観測値に依存するようになります。
xk = xk − 1 + K (測定値 − H ∗ xk ) x_k = x_{k-1} +K(測定値 -H*x_k)バツｋ=バツk − 1+K (測定_ _ _ _ _ _ _ _ _ _−H∗バツｋ） HH
もここで見ることができますH観測行列を使用すると、観測値と状態値が一致します。
観測値が更新されるため、対応する信頼水準も更新する必要があります。
P k = ( I − K ∗ H ) ∗ P k − 1 P_k = (I - K*H)*P_{k-1}Pｋ=(私−K∗ひ）∗Pk − 1
その後、更新プロセスが完了し、その後は継続的な反復プロセスが続きます。最終的に計算された効果はまったく同じです~
cout << "MAIN: Starting estimation" << endl;
ColumnVector X_k(2); // 状态量 （x，y）
X_k= prior_Mu; // 先验
Matrix P(2,2); // 先验协防差
P = prior_Cov;
Matrix Identity(2,2); // 用于数据格式转换
Identity(1,1) = 1;
Identity(1,2) = 0.0;
Identity(2,1) = 0.0;
Identity(2,2) = 1;
for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
{
auto start = std::chrono::steady_clock::now();
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);
// DO ONE MEASUREMEN
ColumnVector measurement = mobile_robot.Measure();
cout << " \n "<< endl;
// 预测
// 1.更新状态量
X_k = A*X_k + B*input + sysNoise_Mu;
// 2.协防差传播
P = A*(P*A.transpose()) + sysNoise_Cov*Identity;
// 观测值更新
Matrix K,R;
R = measNoise_Cov;
// 1.计算增益矩阵
K = P*H.transpose() *( H*P*H.transpose() + R).inverse();
// 2.更新状态量
ColumnVector X_k_1(2);
X_k = X_k + K *(measurement - H*X_k);
// 3.协防差传播
P = (Identity - K*H)*P;
cout << " K:" << K <<endl;
cout << " Z - Hx:" << measurement - H*X_k <<endl;
cout << " K(Z - Hx):" << K *(measurement - H*X_k) <<endl;
cout << " x + K(Z - Hx):" << X_k <<endl;
} // estimation loop
具体的なコードは上記の通りで、操舵角を考慮したC++ベースの非線形カルマンフィルターは後日更新される予定です。