Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix imu system #36

Draft
wants to merge 3 commits into
base: master
Choose a base branch
from
Draft

Fix imu system #36

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4a2b06f
Fix imu system
nyxrobotics Jan 9, 2024
8f63b74
Add missing normalize()
nyxrobotics Jan 10, 2024
1feaf1e
Use quaternion
nyxrobotics Jan 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 16 additions & 29 deletions src/hdl_localization/pose_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,17 +11,13 @@ PoseSystem::PoseSystem()
PoseSystem::VectorXt PoseSystem::f(const VectorXt& state) const
{
VectorXt state_next(16);

Vector3t pt = state.middleRows(0, 3);
Vector3t vt = state.middleRows(3, 3);
Quaterniont qt(state[6], state[7], state[8], state[9]);
qt.normalize();

Vector3t acc_bias = state.middleRows(10, 3);
Vector3t gyro_bias = state.middleRows(13, 3);

// position
state_next.middleRows(0, 3) = pt + vt * dt_; //
state_next.middleRows(0, 3) = pt + vt * dt_;

// velocity
state_next.middleRows(3, 3) = vt;
Expand All @@ -39,7 +35,6 @@ PoseSystem::VectorXt PoseSystem::fImu(const VectorXt& state, const Eigen::Vector
const Eigen::Vector3f& imu_gyro) const
{
VectorXt state_next(16);

Vector3t pt = state.middleRows(0, 3);
Vector3t vt = state.middleRows(3, 3);
Quaterniont qt(state[6], state[7], state[8], state[9]);
Expand All @@ -48,26 +43,21 @@ PoseSystem::VectorXt PoseSystem::fImu(const VectorXt& state, const Eigen::Vector
Vector3t acc_bias = state.middleRows(10, 3);
Vector3t gyro_bias = state.middleRows(13, 3);

const Vector3t& raw_acc = imu_acc;
const Vector3t& raw_gyro = imu_gyro;

// position
state_next.middleRows(0, 3) = pt + vt * dt_; //
Vector3t pt_next = pt + vt * dt_;
state_next.middleRows(0, 3) = pt_next;

// velocity
Vector3t g(0.0f, 0.0f, 9.80665f);
Vector3t acc = qt * (raw_acc - acc_bias);
state_next.middleRows(3, 3) = vt + (acc - g) * dt_;
// state_next.middleRows(3, 3) = vt; // + (acc - g) * dt_; // acceleration didn't contribute to accuracy due to
// large noise
Vector3t acc = qt * (imu_acc - acc_bias) - g;
Vector3t vt_next = vt + acc * dt_;
state_next.middleRows(3, 3) = vt_next;

// orientation
Vector3t gyro = raw_gyro - gyro_bias;
Quaterniont dq(1, gyro[0] * dt_ / 2, gyro[1] * dt_ / 2, gyro[2] * dt_ / 2);
dq.normalize();
Quaterniont qt_next = (qt * dq).normalized();
Vector3t gyro = imu_gyro - gyro_bias;
Quaterniont dq = Quaterniont(0, gyro[0] * dt_ / 2.0, gyro[1] * dt_ / 2.0, gyro[2] * dt_ / 2.0) * qt;
Quaterniont qt_next = Quaterniont(qt.w() + dq.w(), qt.x() + dq.x(), qt.y() + dq.y(), qt.z() + dq.z()).normalized();
state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z();

state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration
state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity

Expand All @@ -83,22 +73,19 @@ PoseSystem::VectorXt PoseSystem::fOdom(const VectorXt& state, const Eigen::Vecto
Quaterniont qt(state[6], state[7], state[8], state[9]);
qt.normalize();

Vector3t lin_vel_raw = odom_twist_lin;
Vector3t ang_vel_raw = odom_twist_ang;

// position
state_next.middleRows(0, 3) = pt + vt * dt_;
Vector3t pt_next = pt + vt * dt_;
state_next.middleRows(0, 3) = pt_next;

// velocity
Vector3t vel = qt * lin_vel_raw;
state_next.middleRows(3, 3) = vel;
Vector3t vt_next = qt * odom_twist_lin;
state_next.middleRows(3, 3) = vt_next;

// orientation
Quaterniont dq(1, ang_vel_raw[0] * dt_ / 2, ang_vel_raw[1] * dt_ / 2, ang_vel_raw[2] * dt_ / 2);
dq.normalize();
Quaterniont qt_next = (qt * dq).normalized();
Quaterniont dq =
Quaterniont(0, odom_twist_ang[0] * dt_ / 2.0, odom_twist_ang[1] * dt_ / 2.0, odom_twist_ang[2] * dt_ / 2.0) * qt;
Quaterniont qt_next = Quaterniont(qt.w() + dq.w(), qt.x() + dq.x(), qt.y() + dq.y(), qt.z() + dq.z()).normalized();
state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z();

state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration
state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity

Expand Down