惯性传感器导航解算多传感器融合6

惯性传感器导航解算/多传感器融合6

主要目的：利用IMU测量的角速度、加速度,根据上一时刻导航信息,推算出当前时刻导航信息,包括姿态解算、速度解算、位置解算。

1、调用各个函数接口更新 (及格部分)

bool Activity::UpdatePose(void) {
    if (!initialized_) {
        // use the latest measurement for initialization:
        OdomData &odom_data = odom_data_buff_.back();
        IMUData imu_data = imu_data_buff_.back();

        pose_ = odom_data.pose;
        vel_ = odom_data.vel;

        initialized_ = true;

        odom_data_buff_.clear();
        imu_data_buff_.clear();

        // keep the latest IMU measurement for mid-value integration:
        imu_data_buff_.push_back(imu_data);
    } else {
        //
        // TODO: implement your estimation here
        //
        // get deltas:
        Eigen::Vector3d deltas = Eigen::Vector3d::Zero();
        size_t index_curr_ = 1;
        size_t index_prev_ = 0;
        if(Activity::GetAngularDelta(index_curr_, index_prev_, deltas) == false){
            std::cout << "GetAngularDelta(): index error" << std::endl;
            return false;
        }

        // update orientation:
        Eigen::Matrix3d R_curr_ = Eigen::Matrix3d::Identity();
        Eigen::Matrix3d R_prev_ = Eigen::Matrix3d::Identity();
        Activity::UpdateOrientation(deltas, R_curr_, R_prev_);

        // get velocity delta:
        double delta_t_ = 0;
        Eigen::Vector3d velocity_delta_ = Eigen::Vector3d::Zero();
        Activity::GetVelocityDelta(index_curr_, index_prev_, R_curr_, R_prev_, delta_t_, velocity_delta_);

        // update position:
        Activity::UpdatePosition(delta_t_, velocity_delta_);

        // move forward -- 
        // NOTE: this is NOT fixed. you should update your buffer according to the method of your choice:

        imu_data_buff_.pop_front();
    }
    
    return true;
}

2、切换中值法和欧拉法

角速度

    // angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev);
    angular_delta = delta_t*angular_vel_prev;

线速度

    //velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev);
    velocity_delta = delta_t*linear_acc_curr;

3、结果

欧拉

 中值

 

4、去除误差 4.1、角速度误差

inline Eigen::Vector3d Activity::GetUnbiasedAngularVel(const Eigen::Vector3d &angular_vel) {
    return angular_vel - angular_vel_bias_;
}

4.2、 线加速度误差

inline Eigen::Vector3d Activity::GetUnbiasedLinearAcc(
    const Eigen::Vector3d &linear_acc,
    const Eigen::Matrix3d &R
) {
    return R*(linear_acc - linear_acc_bias_) - G_;
}

 

需要从载体坐标系(b)转换到世界坐标系(w)所以需要先乘以旋转矩阵R

5、解算当前信息 5.1、角速度解算

bool Activity::GetAngularDelta(
    const size_t index_curr, const size_t index_prev,
    Eigen::Vector3d &angular_delta
) {
    //
    // TODO: this could be a helper routine for your own implementation
    //
    if (
        index_curr <= index_prev ||
        imu_data_buff_.size() <= index_curr
    ) {
        return false;
    }

    const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
    const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

    double delta_t = imu_data_curr.time - imu_data_prev.time;

    Eigen::Vector3d angular_vel_curr = GetUnbiasedAngularVel(imu_data_curr.angular_velocity);
    Eigen::Vector3d angular_vel_prev = GetUnbiasedAngularVel(imu_data_prev.angular_velocity);

    angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev);

    return true;
}

5.2、速度解算

bool Activity::GetVelocityDelta(
    const size_t index_curr, const size_t index_prev,
    const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev, 
    double &delta_t, Eigen::Vector3d &velocity_delta
) {
    //
    // TODO: this could be a helper routine for your own implementation
    //
    if (
        index_curr <= index_prev ||
        imu_data_buff_.size() <= index_curr
    ) {
        return false;
    }

    const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
    const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

    delta_t = imu_data_curr.time - imu_data_prev.time;

    Eigen::Vector3d linear_acc_curr = GetUnbiasedLinearAcc(imu_data_curr.linear_acceleration, R_curr);
    Eigen::Vector3d linear_acc_prev = GetUnbiasedLinearAcc(imu_data_prev.linear_acceleration, R_prev);
    
    velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev);

    return true;
}

5.3、位姿更新

void Activity::UpdateOrientation(
    const Eigen::Vector3d &angular_delta,
    Eigen::Matrix3d &R_curr, Eigen::Matrix3d &R_prev
) {
    //
    // TODO: this could be a helper routine for your own implementation
    //
    // magnitude:
    double angular_delta_mag = angular_delta.norm();
    // direction:
    Eigen::Vector3d angular_delta_dir = angular_delta.normalized();

    // build delta q:
    double angular_delta_cos = cos(angular_delta_mag/2.0);
    double angular_delta_sin = sin(angular_delta_mag/2.0);
    Eigen::Quaterniond dq(
        angular_delta_cos, 
        angular_delta_sin*angular_delta_dir.x(), 
        angular_delta_sin*angular_delta_dir.y(), 
        angular_delta_sin*angular_delta_dir.z()
    );
    Eigen::Quaterniond q(pose_.block<3, 3>(0, 0));
    
    // update:
    q = q*dq;
    
    // write back:
    R_prev = pose_.block<3, 3>(0, 0);
    pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
    R_curr = pose_.block<3, 3>(0, 0);
}

5.3.1、旋转矢量到四元数

    // build delta q:
    double angular_delta_cos = cos(angular_delta_mag/2.0);
    double angular_delta_sin = sin(angular_delta_mag/2.0);
    Eigen::Quaterniond dq(
        angular_delta_cos, 
        angular_delta_sin*angular_delta_dir.x(), 
        angular_delta_sin*angular_delta_dir.y(), 
        angular_delta_sin*angular_delta_dir.z()
    );
    Eigen::Quaterniond q(pose_.block<3, 3>(0, 0));

 

5.3.2、位姿更新

    q = q*dq;

5.33、四元数到旋转矩阵

// write back:
    R_prev = pose_.block<3, 3>(0, 0);
    pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
    R_curr = pose_.block<3, 3>(0, 0);

5.4、位置更新

void Activity::UpdatePosition(const double &delta_t, const Eigen::Vector3d &velocity_delta) {
    //
    // TODO: this could be a helper routine for your own implementation
    //
    pose_.block<3, 1>(0, 3) += delta_t*vel_ + 0.5*delta_t*velocity_delta;
    vel_ += velocity_delta;
}