From a072dc89fe697def2078e7db4b371f0ab6d6aca9 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 15 Nov 2024 15:55:31 -0800 Subject: [PATCH] Use the initial orientation --- src/imu_model.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/imu_model.h b/src/imu_model.h index e544e743..4a3bbbef 100644 --- a/src/imu_model.h +++ b/src/imu_model.h @@ -18,7 +18,7 @@ class ImuModel { * param: orientation represented as euler * TODO: add initial angular velocity */ - virtual void set_initial_state(const Eigen::Vector3d& orientation) = 0; + virtual void set_initial_state(const Eigen::Vector3d& initial_orientation) = 0; /* * TODO: Update @@ -31,17 +31,15 @@ class ImuModel { class SimpleImuModel : public ImuModel { public: - void set_initial_state(const Eigen::Vector3d& orientation) override { - this->orientation = orientation; + void set_initial_state(const Eigen::Vector3d& initial_orientation) override { + this->initial_orientation = initial_orientation; } Eigen::Quaterniond update(uint64_t ts, const Eigen::Vector3d& la, const Eigen::Vector3d& av) override { - // TODO: compute delta t + // TODO: compute dt from consecutive ts double dt = 0.01; - orientation.x() += av.x() * dt; - orientation.y() += av.y() * dt; - orientation.z() += av.z() * dt; + Eigen::Vector3d orientation = initial_orientation + av * dt; orientation.x() += atan2(la.y(), la.z()); orientation.y() += atan2(-la.x(), sqrt(la.y() * la.y() + la.z() * la.z())); @@ -54,5 +52,5 @@ class SimpleImuModel : public ImuModel { private: // using euler angles for now - Eigen::Vector3d orientation; + Eigen::Vector3d initial_orientation; }; \ No newline at end of file