zm0612/eskf-gps-imu-fusion

关于ComputePosition

Closed this issue · 1 comments

mask0918 commented 
void ErrorStateKalmanFilter::ComputePosition(const Eigen::Vector3d &last_vel, const Eigen::Vector3d &curr_vel,
                                             const IMUData &imu_data_0,
                                             const IMUData &imu_data_1) {
    double delta_t = imu_data_1.time - imu_data_0.time;

    pose_.block<3, 1>(0, 3) += 0.5 * delta_t * (curr_vel + last_vel) +
                               0.25 * (imu_data_0.linear_accel + imu_data_1.linear_accel) * delta_t * delta_t;
}

是不是应该改为

g_ = [0,0,-.9.8]
pose_.block<3, 1>(0, 3) += 0.5 * delta_t * (curr_vel + last_vel) + 0.25 * (imu_data_0.linear_accel - g_+ imu_data_1.linear_accel - g_) * delta_t * delta_t;
zm0612 commented

hello，确实存在问题，这里检查不仔细，目前已经修正了，感谢指出，经过测试精度有一定的提升。修改的代码如下：

eskf-gps-imu-fusion/src/eskf.cpp

Line 248 in 4b57efe

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