t_imu_fusion: Fix initialization of orientation from accelerometer.

This commit is contained in:
Ryan Pavlik 2019-11-11 18:13:15 -06:00
parent 08903e67b3
commit af0515232f

View file

@ -95,7 +95,7 @@ public:
// Initially, just set it to totally trust gravity. // Initially, just set it to totally trust gravity.
started_ = true; started_ = true;
quat_ = Eigen::Quaterniond::FromTwoVectors( quat_ = Eigen::Quaterniond::FromTwoVectors(
Eigen::Vector3d::UnitY(), accel.normalized()); accel.normalized(), Eigen::Vector3d::UnitY());
return true; return true;
} }
auto scale = 1. - diff; auto scale = 1. - diff;