主要目的:利用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、角速度解算
/**
* @brief get angular delta
* @param index_curr, current imu measurement buffer index
* @param index_prev, previous imu measurement buffer index
* @param angular_delta, angular delta output
* @return true if success false otherwise
*/
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、速度解算
/**
* @brief get velocity delta
* @param index_curr, current imu measurement buffer index
* @param index_prev, previous imu measurement buffer index
* @param R_curr, corresponding orientation of current imu measurement
* @param R_prev, corresponding orientation of previous imu measurement
* @param velocity_delta, velocity delta output
* @return true if success false otherwise
*/
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;
}