Commit c713ca61 authored by Ryan Pavlik's avatar Ryan Pavlik Committed by Jakob Bornecrantz

t/psmv: Rotate IMU into something like camera space

parent ccd139ab
......@@ -510,7 +510,10 @@ imu_data(TrackerPSMV &t,
//! @todo use better measurements instead of the above "simple fusion"
flexkalman::predict(t.filter_state, t.process_model, dt);
auto meas = flexkalman::AbsoluteOrientationMeasurement{
t.imu.getQuat(), Eigen::Vector3d::Constant(0.01)};
Eigen::Quaterniond(
Eigen::AngleAxisd(EIGEN_PI, Eigen::Vector3d::UnitY())) *
t.imu.getQuat(),
Eigen::Vector3d::Constant(0.01)};
if (!flexkalman::correctUnscented(t.filter_state, meas)) {
fprintf(stderr,
"Got non-finite something when filtering IMU!\n");
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment