レーザースキャン+既知のマップを備えた拡張カルマンフィルター

私は現在、レーザースキャナーを備えたポイントロボットに拡張カルマンフィルターを実装する必要がある学校のプロジェクトに取り組んでいます。ロボットは、回転半径0度で回転し、前進することができます。すべての動作は区分的に線形です（駆動、回転、駆動）。

私たちが使用しているシミュレーターは、加速をサポートしていません。すべての動きは瞬時です。

また、ローカライズする必要のある既知のマップ（PNG画像）もあります。レーザースキャンをシミュレートするために、画像のレイトレースを行うことができます。

私のパートナーと私は、使用する必要があるモーションモデルとセンサーモデルについて少し混乱しています。

これまでのところ、状態をベクトルとしてモデル化しています。$(x,y,\theta)$

次のように更新式を使用しています

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

初期化を忘れてPゼロになり、修正が行われなかったことに気づくまでは、すべてが機能していると考えていました。まだシステムにノイズを導入していないため、伝搬は非常に正確でした。

モーションモデルでは、Fに次の行列を使用しています。

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

その更新式のヤコビアンとして。これは正しいです？

$x$$y$$\theta$

Pを初期化する方法があるもう1つの問題。1、10、100を試したところ、マップが50x50だけの場合、ロボットはすべて（-90、-70）の位置にロボットを配置しません。

私たちのプロジェクトのコードはここにあります：https : //github.com/en4bz/kalman/blob/master/src/kalman.cpp

アドバイスは大歓迎です。

編集：

この時点で、基本的な動きのノイズでフィルターを安定させることができましたが、実際の動きはありません。ロボットが動き始めるとすぐに、フィルターは非常に速く発散し、マップを終了します。

• レーザースキャンと既知のマップに基づくローカリゼーションにEKFを使用することは悪い考えであり、EKFの主な仮定の1つが無効であるため機能しません。測定モデルは区別できません。

モンテカルロローカリゼーション（粒子フィルター）を検討することをお勧めします。これにより、測定モデルの問題が解決するだけでなく、グローバルなローカリゼーション（マップ内の初期ポーズは完全に不明）も可能になります。

編集：別の重要なポイントについて言及するのを忘れて申し訳ありません：

• $z_t = h(x_t) + N(0, Q_t)$$h(x_t)$ 部分とガウスノイズを追加します。

まず最初に、使用しているローカリゼーションの種類については何も言及していません。既知または未知の対応がありますか？

Matlabでヤコビアンを取得するには、次のようにします。

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

結果

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

拡張カルマンフィルターでは、システムが線形で、ノイズがガウスである必要があります。ここでのシステムは、運動モデルと観測モデルが線形でなければならないことを意味します。レーザーセンサーは、ロボットのフレームからランドマークに向かう範囲と角度を提供するため、測定モデルは線形ではありません。についてP、それが大きいと仮定した場合、EKF推定量は最初は貧弱であり、以前の状態ベクトルが利用できないため、測定に依存します。ただし、ロボットが動き始めて感知し始めると、EKFはモーションモデルと測定モデルを使用してロボットのポーズを推定するため、改善されます。ロボットがランドマークを感知していない場合、不確実性が大幅に増加します。また、角度にも注意する必要があります。C ++では、cos and sin、角度はラジアンでなければなりません。この問題については、コードには何もありません。ラジアンでの計算中に角度のノイズを度数で想定している場合、ラジアンでの計算は1度のノイズと見なされるため、奇妙な結果になる可能性があります。