融合滤波对应代码的理解

GPS数据和IMU数据的表示

• GPSData

    Eigen::Vector3d position_lla = Eigen::Vector3d::Zero();//LLA
    Eigen::Vector3d velocity = Eigen::Vector3d::Zero();//NED
    Eigen::Vector3d position_ned = Eigen::Vector3d::Zero();

    Eigen::Vector3d true_velocity = Eigen::Vector3d::Zero();
    Eigen::Vector3d true_position_lla = Eigen::Vector3d::Zero();
    double time = 0.0;

• IMUData

    double time = 0.0;
    Eigen::Vector3d linear_accel = Eigen::Vector3d::Zero();
    Eigen::Vector3d angle_velocity = Eigen::Vector3d::Zero();

    Eigen::Vector3d true_linear_accel = Eigen::Vector3d::Zero();
    Eigen::Vector3d true_angle_velocity = Eigen::Vector3d::Zero();

初始化过程

    init_velocity_ = curr_gps_data.true_velocity;
    velocity_ = init_velocity_;

    Eigen::Quaterniond Q = Eigen::AngleAxisd(90 * kDegree2Radian, Eigen::Vector3d::UnitZ()) *
                           Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitY()) *
                           Eigen::AngleAxisd(180 * kDegree2Radian, Eigen::Vector3d::UnitX());
    init_pose_.block<3,3>(0,0) = Q.toRotationMatrix();
    pose_ = init_pose_;

    imu_data_buff_.clear();
    imu_data_buff_.push_back(curr_imu_data);

    curr_gps_data_ = curr_gps_data;

卡尔曼滤波的预测过程

//输入一个imu的数据就进行预测在imu数据调用的过程中会调用UpdateErrorState
//里面原本有一帧，再传入一帧imu就是了
bool Predict(const IMUData &curr_imu_data);

 1. bool UpdateOdomEstimation();//更新里程计的信息
 	a. ComputeAngularDelta 计算相对转动了多少
 	b. ComputeEarthTranform 计算地球转动带来的影响
 	c. ComputeOrientation 将之前的旋转矩阵和现在的旋转矩阵求出来，计算出旋转的部分
 	d. ComputeVelocity 将上一帧的速度和当前帧的速度计算出来
 	e. ComputePosition 计算出pose中的位移的部分
 2. 计算该帧和最前面的帧之间的时间差
 	a. double delta_t = curr_imu_data.time - imu_data_buff_.front().time
 3. 将body系下的坐标转到全局的坐标
 	a. Eigen::Vector3d curr_accel = pose_.block<3, 3>(0, 0)
                                 * curr_imu_data.linear_accel
 4. UpdateErrorState 根据时间间隔t和全局下的加速度进行预测更新 对应的(1)(2)步骤

bool ESKF::UpdateErrorState(double t, const Eigen::Vector3d &accel) {
    Eigen::Matrix3d F_23 = BuildSkewMatrix(accel);
    F_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = F_23; // [3,6]
    F_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3,3>(0,0); // [3,12]
    F_.block<3,3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -pose_.block<3,3>(0,0); // [6,9] Cbn
    B_.block<3,3>(INDEX_STATE_VEL, 3) = pose_.block<3,3>(0,0); //Cbn
    B_.block<3,3>(INDEX_STATE_ORI, 0) = -pose_.block<3,3>(0,0); //Cbn

    TypeMatrixF Fk = TypeMatrixF::Identity() + F_ * t;
    TypeMatrixB Bk = B_ * t;

    Ft_ = F_ * t;

    X_ = Fk * X_;	//卡尔曼更新步骤(1),误差也没给只能当作是准的
    P_ = Fk * P_ * Fk.transpose() + Bk * Q_ * Bk.transpose(); //卡尔曼更新步骤(2)，方差项可以更新根据Bk
}
5. imu_data_buff_.pop_front(); 上一帧的用完去掉

里程计的更新部分

UpdateOdomEstimation();
1.计算两帧imu之间转动的角度，有前后两帧的角速度，速度一半乘以时间就可以了
Eigen::Vector3d angular_delta;
ComputeAngularDelta(angular_delta);
2.计算地球转动带来的影响,计算两帧之间地球转动的角度
Eigen::Matrix3d R_nm_nm_1;
ComputeEarthTranform(R_nm_nm_1);
3.更新pose中的旋转向量的部分，一个是转动的角度，一个是地球的角度
last_R，上帧的R，curr_R imu更新的R
Eigen::Matrix3d curr_R, last_R;
ComputeOrientation(angular_delta, R_nm_nm_1, curr_R, last_R);
4.计算出当前帧的速度和上一帧的速度
Eigen::Vector3d curr_vel, last_vel;
ComputeVelocity(curr_vel, last_vel, curr_R, last_R);
5，计算当前的位置
ComputePosition(curr_vel, last_vel);

    std::deque<IMUData> imu_data_buff_;
1.  计算两帧imu之间转动的角度，有前后两帧的角速度，速度一半乘以时间就可以了
bool ESKF::ComputeAngularDelta(Eigen::Vector3d &angular_delta) 
    IMUData curr_imu_data = imu_data_buff_.at(1);//两帧的imu结果
    IMUData last_imu_data = imu_data_buff_.at(0);
    double delta_t = curr_imu_data.time - last_imu_data.time;
    
    Eigen::Vector3d curr_angular_vel = curr_imu_data.angle_velocity;
    Eigen::Vector3d last_angular_vel = last_imu_data.angle_velocity;

    Eigen::Vector3d curr_unbias_angular_vel = curr_angular_vel;
    Eigen::Vector3d last_unbias_angular_vel = last_angular_vel;

    angular_delta = 0.5 * (curr_unbias_angular_vel + last_unbias_angular_vel) * delta_t; //通过两帧的角速度计算出偏转角

2.计算地球转动带来的影响,计算两帧之间地球转动的角度
w =[0.0, w * cos(L),w * sin(L)];
bool ESKF::ComputeEarthTranform(Eigen::Matrix3d &R_nm_nm_1)
	IMUData curr_imu_data = imu_data_buff_.at(1);
    IMUData last_imu_data = imu_data_buff_.at(0);

    double delta_t = curr_imu_data.time - last_imu_data.time;

    constexpr double rm = 6353346.18315;
    constexpr double rn = 6384140.52699;
    Eigen::Vector3d w_en_n(-velocity_[1] / (rm + curr_gps_data_.position_lla[2]),
                           velocity_[0] / (rn + curr_gps_data_.position_lla[2]),
                           velocity_[0] / (rn + curr_gps_data_.position_lla[2])
                           * std::tan(curr_gps_data_.position_lla[0] * kDegree2Radian));

    Eigen::Vector3d w_in_n = w_en_n + w_; //当前位置的一个地球转动角速度
    auto angular = delta_t * w_in_n; //计算出时间delta_t内转动的角度

    Eigen::AngleAxisd angle_axisd(angular.norm(), angular.normalized()); //角度模长以及归一化向量

    R_nm_nm_1 = angle_axisd.toRotationMatrix().transpose();
3.  更新pose中的旋转向量的部分，一个是转动的角度，一个是地球的角度
	Eigen::Matrix3d curr_R, last_R;
    ComputeOrientation(angular_delta, R_nm_nm_1, curr_R, last_R);
bool ESKF::ComputeOrientation(const Eigen::Vector3d &angular_delta,
                              const Eigen::Matrix3d R_nm_nm_1,
                              Eigen::Matrix3d &curr_R,
                              Eigen::Matrix3d &last_R) {
    Eigen::AngleAxisd angle_axisd(angular_delta.norm(), angular_delta.normalized());
    last_R = pose_.block<3, 3>(0, 0);
    curr_R = R_nm_nm_1 * pose_.block<3, 3>(0, 0) * angle_axisd.toRotationMatrix();
    pose_.block<3, 3>(0, 0) = curr_R;

4，计算出当前帧的速度和上一帧的速度
    Eigen::Vector3d curr_vel, last_vel;
    ComputeVelocity(curr_vel, last_vel, curr_R, last_R);
    bool ESKF::ComputeVelocity(Eigen::Vector3d &curr_vel, 		Eigen::Vector3d& last_vel,
                                             const Eigen::Matrix3d &curr_R,
                                             const Eigen::Matrix3d last_R) {
    IMUData curr_imu_data = imu_data_buff_.at(1);
    IMUData last_imu_data = imu_data_buff_.at(0);
    double delta_t = curr_imu_data.time - last_imu_data.time;

    Eigen::Vector3d curr_accel = curr_imu_data.linear_accel;
    Eigen::Vector3d curr_unbias_accel = GetUnbiasAccel(curr_R * curr_accel); //就是把地球的重力加速度加上，转化到了导航坐标系

    Eigen::Vector3d last_accel = last_imu_data.linear_accel;
    Eigen::Vector3d last_unbias_accel = GetUnbiasAccel(last_R * last_accel);

    last_vel = velocity_;

    velocity_ += delta_t * 0.5 * (curr_unbias_accel + last_unbias_accel);
    curr_vel = velocity_;
5.计算里程计的位置
    ComputePosition(curr_vel, last_vel);
	bool ESKF::ComputePosition(const Eigen::Vector3d& curr_vel, const Eigen::Vector3d& last_vel){
    double delta_t = imu_data_buff_.at(1).time - imu_data_buff_.at(0).time;

    pose_.block<3,1>(0,3) += 0.5 * delta_t * (curr_vel + last_vel);   

卡尔曼滤波的更新部分

//过来一帧的GPS信号就对位姿进行一下的更新
bool ESKF::Correct(const GPSData &curr_gps_data)

 1. 更新gps的参数
 	a. curr_gps_data_ = curr_gps_data
 
 2. 使用预先积分得到的结果减去gps绝对的结果得到一个误差
 	a.Y_ = pose_.block<3,1>(0,3) - curr_gps_data.position_ned
 
 3. 进行卡尔曼滤波的经典后三个步骤
 	a.K_ = P_ * G_.transpose() * (G_ * P_ * G_.transpose() + C_ * R_ * C_.transpose()).inverse(); //计算卡尔曼增益,卡尔曼更新步骤(3)
 	b.P_ = (TypeMatrixP::Identity() - K_ * G_) * P_; //观测更新方差矩阵，卡尔曼更新步骤(4)
 	c.X_ = X_ + K_ * (Y_ - G_ * X_); //对状态进行更新，卡尔曼更新步骤(5)
 4. 利用后验的结果对已有的状态进行误差消除
    EliminateError();
 5. 全部状态归0
    ResetState();

void ESKF::EliminateError() {
    pose_.block<3,1>(0,3) = pose_.block<3,1>(0,3) - X_.block<3,1>(INDEX_STATE_POSI, 0);  //位姿[0 3]减去第一个
    velocity_ = velocity_ - X_.block<3,1>(INDEX_STATE_VEL, 0); //速度[3 6]减去第二个
    Eigen::Matrix3d C_nn = Sophus::SO3d::exp(X_.block<3,1>(INDEX_STATE_ORI, 0)).matrix(); //先求解出旋转矩阵，再乘起来
    pose_.block<3,3>(0,0) = C_nn * pose_.block<3,3>(0,0);  //旋转乘上第三个

    gyro_bias_ = gyro_bias_ - X_.block<3,1>(INDEX_STATE_GYRO_BIAS, 0); //加速度和角速度误差处理一下
    accel_bias_ = accel_bias_ - X_.block<3,1>(INDEX_STATE_ACC_BIAS, 0);
}
void ESKF::ResetState() {
    X_.setZero();
}
  

卡尔曼滤波的所有过程

if (curr_imu_data_.time < curr_gps_data_.time) {
  eskf_ptr_->Predict(curr_imu_data_);
  imu_data_buff_.pop_front();
} else {
  eskf_ptr_->Predict(curr_imu_data_);
  imu_data_buff_.pop_front();
  eskf_ptr_->Correct(curr_gps_data_);
  gps_data_buff_.pop_front();
}

系统可观性的分析

• 相当于每一帧的观测数据都会保存一个的FGY的内容

Eigen::Matrix<double, 15, 15> F;
Eigen::Matrix<double, 3, 15> G;
Eigen::Matrix<double, 3, 1> Y;
eskf_ptr_->GetFGY(F, G, Y);
observability_analysis.SaveFG(F, G, Y, curr_gps_data_.time);

1.获取当前的一个FGY的值，赋值给F，G，Y
    F = Ft_;	// Ft_ = F_ * t;
    G = G_;		// G_.block<3,3>(0,0) = Eigen::Matrix3d::Identity(); [3 * 15] 是一个确定的矩阵
    Y = Y_;   // Y_ = pose_.block<3,1>(0,3) - curr_gps_data.position_ned;
FG fg;
fg.time = time;
fg.F = F - Eigen::Matrix<double, 15, 15>::Identity();
fg.G = G;
fg.Y.push_back(Y);
FGs_.push_back(fg);

• 最后进行可观性的分析
• 举个例子就是保存了100个的FGY的数据

observability_analysis.ComputeSOM();
observability_analysis.ComputeObservability();
1.