前回の Joan Sola 氏による ESKF の実装に続き、一例から推測し、EKF アルゴリズムも実装したいと思い、「マルチセンサー融合測位」アルゴリズムにおけるナビゲーション情報に基づくフィルタリング アルゴリズムを検討しました。 Deep Blue Academy のアルゴリズムも見直して、コードを実装します。2 つのレンダリングを添付します(前の記事の ESKF のものと似ています)。github アドレスは次のとおりです: GitHub - Shelfcol/gps_imu_fusion: gps_imu_fusion with eskf,ekf,ukf,etc
以下は、公式の詳細な導出と私自身の理解の一部です。
1 はじめに
カルマン フィルターの主要な方程式は、予測方程式と観測方程式の構築です。2 つの式は次のとおりです。
簡単に運動方程式を導出できるモデルもあれば、2つの状態の関係を直接構築することができず運動方程式を導出できないモデルもあるため、状態量の微分方程式を利用して間接的に運動方程式を導出することもできます。 。
2.ナビゲーション情報に基づくフィルタリング
1) 航行情報の微分方程式
ここで使用する状態量は であり、 は
角速度の偏り、
は加速度の偏りです。ここでのクォータニオンの順序は、最初に実数部、最後に虚数ステップであることに注意してください。この順序は、後で F 行列を解くときにも確認できます。
ナビゲーション情報の微分方程式は次のとおりです。
したがって、状態方程式は次の微分形式で書くことができます。
これは非線形なので、線形化する必要がある場合は、次のようになります。
2) ヤコビ行列解
微分方程式を線形化する
ヤコビ行列の導出:
ここでクォータニオン積について説明する必要があります。
3) 連続系の微分方程式の離散化
得られるのは連続時間微分方程式なので、ここで離散化が必要となります。ここでの参考資料:ロボット ダイナミクス シミュレーション - 状態空間モデルの離散化
3-1) 正確な離散化
3-2) 近似離散化
3-3) 本システムの離散化結果
3. コードの説明
コードは厳密に公式に従って推定されており、方程式の予測と更新に重要なコードのみを掲載しています。
予測:
// IMU数据预测
bool EKF::Predict(const IMUData &curr_imu_data) {
imu_data_buff_.push_back(curr_imu_data);
double delta_t = curr_imu_data.time - imu_data_buff_.front().time; // dt
Eigen::Vector3d curr_accel = curr_imu_data.linear_accel; // 局部坐标系下加速度
Eigen::Vector3d curr_angle_velocity = curr_imu_data.angle_velocity; // 局部坐标系下的角速度
Eigen::Quaterniond curr_ori = Eigen::Quaterniond(pose_.block<3, 3>(0, 0));
UpdateEkfState(delta_t, curr_accel,curr_angle_velocity,curr_ori);
UpdateState(); //! 每次都需要更新位姿
imu_data_buff_.pop_front();
return true;
}
bool EKF::UpdateEkfState(const double t, const Eigen::Vector3d &accel, const Eigen::Vector3d& curr_angle_velocity,
const Eigen::Quaterniond& curr_ori ) {
F_.setZero(); // 初始化为零矩阵
F_.block<3,3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity();
double q0 = curr_ori.w();
double q1 = curr_ori.x();
double q2 = curr_ori.y();
double q3 = curr_ori.z();
double FVq0 = 2 * Eigen::Vector3d(q0,-q3,q2).transpose()*accel;
double FVq1 = 2 * Eigen::Vector3d(q1,q2,q3).transpose()*accel;
double FVq2 = 2 * Eigen::Vector3d(-q2,q1,q0).transpose()*accel;
double FVq3 = 2 * Eigen::Vector3d(-q3,-q0,q1).transpose()*accel;
Eigen::Matrix<double,3,4> FVq = (Eigen::Matrix<double,3,4>()<< FVq0,FVq1,FVq2,FVq3,
-FVq3,-FVq2,FVq1,FVq0,
FVq2,-FVq3,-FVq0,FVq1).finished();
F_.block<3,4>(INDEX_STATE_VEL, INDEX_STATE_ORI) = FVq;
F_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3,3>(0,0);
Eigen::Vector3d w = curr_angle_velocity;
Eigen::Matrix<double,4,4> Fqq = 0.5* (Eigen::Matrix<double,4,4>()<<0,-w.x(),-w.y(),-w.z(),
w.x(),0,w.z(),-w.y(),
w.y(),-w.z(),0,w.x(),
w.z(),w.y(),-w.x(),0).finished();
F_.block<4,4>(INDEX_STATE_ORI,INDEX_STATE_ORI) = Fqq;
Eigen::Matrix<double,4,3> Fqkesi = 0.5 * (Eigen::Matrix<double,4,3>()<<-q1,-q2,-q3,
q0,-q3,q2,
q3,q0,-q1,
-q2,q1,q0).finished();
F_.block<4,3>(INDEX_STATE_ORI,INDEX_STATE_GYRO_BIAS) = Fqkesi;
B_.setZero();
B_.block<3,3>(INDEX_STATE_VEL, 3) = pose_.block<3,3>(0,0);
B_.block<4,3>(INDEX_STATE_ORI, 0) = Fqkesi;
TypeMatrixF Fk = TypeMatrixF::Identity() + F_ * t;
TypeMatrixB Bk = B_ * t;
X_ = Fk * X_ + Bk * W_+ gt_*t ;
P_ = Fk * P_ * Fk.transpose() + Bk * Q_ * Bk.transpose();
return true;
}
更新:
bool EKF::Correct(const GPSData &curr_gps_data) {
curr_gps_data_ = curr_gps_data;
C_.setIdentity(); // 单位矩阵
G_.setZero();
G_.block<3,3>(INDEX_MEASUREMENT_POSI, INDEX_MEASUREMENT_POSI) = Eigen::Matrix3d::Identity();
Y_ = curr_gps_data.position_ned; //Y_measure
K_ = P_ * G_.transpose() * (G_ * P_ * G_.transpose() + C_ * R_ * C_.transpose()).inverse(); // kalman增益
P_ = (TypeMatrixP::Identity() - K_ * G_) * P_;
// P_ = (TypeMatrixP::Identity() - K_ * G_) * P_ * (TypeMatrixP::Identity() - K_ * G_).transpose()+K_*C_*K_.transpose();
X_ = X_ + K_ * (Y_ - G_ * X_);
UpdateState();
return true;
}4. まとめ
EKF と ESKF の違い:
1) ESKF は毎回運動方程式に基づいてオドメーターを計算する必要があり、EKF の予測式がオドメーターの計算を置き換えます。
2) ESKF の状態量はよりコンパクトで冗長性がありませんが、EKF は冗長性のある回転四元数を表します。
5. 参考文献
ロボットダイナミクスシミュレーション - 状態空間モデルの離散化 - Gu Yueju
エラー状態カルマンフィルターの四元数運動学でGPS+IMU融合を実現、(ソースコード付き)_qq_38650944のブログ - CSDNブログ
マルチセンサーフュージョンポジショニング 第 4 章