Suite à l'implémentation d'ESKF par Joan Sola dans l'article précédent, j'ai voulu tirer des conclusions à partir d'un exemple et également implémenter l'algorithme EKF. J'ai donc étudié l'algorithme de filtrage basé sur les informations de navigation dans l'algorithme "Multi-Sensor Fusion Positioning" de Deep Blue Academy, et a également révisé l'algorithme et mis en œuvre le code. Ci-joint deux rendus (ils ressemblent à ceux d'ESKF dans l'article précédent). L'adresse github est : GitHub - Shelfcol/gps_imu_fusion : gps_imu_fusion avec eskf,ekf,ukf,etc

Ce qui suit est une dérivation détaillée de la formule et une partie de ma propre compréhension.

1. Introduction

L'équation principale du filtre de Kalman est la construction de l'équation de prédiction et de l'équation d'observation. Les deux équations sont les suivantes :

$x_k =F(x_{k-1},v_k)+B_{k-1}\omega_k$

$y_k=G(x_k)+C_kn_k$

Certains modèles peuvent facilement dériver l'équation du mouvement, tandis que certains modèles ne peuvent pas construire directement la relation entre les deux états et ne peuvent pas dériver l'équation du mouvement. Par conséquent, l'équation différentielle de la quantité d'état peut également être utilisée pour dériver indirectement l'équation du mouvement. .

2. Filtrage basé sur les informations de navigation

1) Équation différentielle des informations de navigation

La quantité d'état utilisée ici est $X=[P^T \ V^T \ q^T \ \epsilon^T \bigtriangledown^T]_{16 \times 1}$ , où $\epsilon$ représente le biais de la vitesse angulaire et $\bigtriangledown$ le biais de l'accélération. Il convient de noter que l'ordre des quaternions ici est la partie réelle en premier, l'étape imaginaire en dernier. Cet ordre peut également être vu lors de la résolution ultérieure de la matrice F.

L'équation différentielle des informations de navigation est la suivante :

L’équation d’état peut donc s’écrire sous la forme différentielle suivante :

$\point X =f(X,v,w,g^n)$

Il n’est pas linéaire, donc s’il doit être linéarisé, nous pouvons obtenir

$\point X =f(X,v,w,g^n)$

2) Solution matricielle jacobienne

Linéariser les équations différentielles

$\dot X = F_tX+B_tW+\overline f(g^n)$

Dérivation de la matrice jacobienne :

Ici, nous devons expliquer le produit quaternion :

3) Discrétisation des équations différentielles des systèmes continus

Comme ce qui est obtenu est une équation différentielle continue dans le temps, une discrétisation est requise ici. La référence ici est : Robot Dynamics Simulation - Discrétisation du modèle spatial d'états

3-1) Discrétisation précise

3-2) Discrétisation approximative

3-3) Résultats de discrétisation de ce système

3. Description des codes

Le code est déduit strictement selon la formule. Seuls les codes clés pour la prédiction et la mise à jour des équations sont affichés ici.

Prédiction :

// 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;
}

Mise à jour :

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. Résumé

La différence entre EKF et ESKF :

1) ESKF doit calculer le compteur kilométrique à chaque fois sur la base de l'équation du mouvement, et l'équation de prédiction d'EKF remplace le calcul du compteur kilométrique.

2) La quantité d'état d'ESKF est plus compacte et n'a pas de redondance, tandis que EKF représente un quaternion en rotation avec redondance.