diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index 249b8392e..66617fcdf 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -68,6 +68,11 @@ namespace fuse_models * Variable order is (x, y, z, roll, pitch, yaw, * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc). + * - ~velocity_decay (double, default: 0.0) Exponential velocity decay rate in 1/s. When > 0, + * the predicted velocity is multiplied by exp(-k*dt) each + * step, decaying toward zero when no velocity observations + * arrive. Prevents indefinite drift after the odometry + * source goes silent. Set to 0.0 to disable (default). */ class Omnidirectional3D : public fuse_core::AsyncMotionModel { @@ -229,6 +234,10 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel bool disable_checks_{ false }; //!< Whether to disable the validation checks for the current and //!< predicted state, including the process noise covariance after //!< it is scaled and multiplied by dt + double velocity_decay_{ 0.0 }; //!< Exponential decay rate for velocity (1/s). + //!< When > 0, predicted velocity decays toward zero when no + //!< velocity observations arrive, preventing drift after the + //!< odometry source goes silent. StateHistory state_history_; //!< History of optimized graph pose estimates }; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index f38c1d4e2..64c2f53f1 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -74,6 +74,8 @@ namespace fuse_models * @param[out] acc_linear2_x - Second X acceleration * @param[out] acc_linear2_y - Second Y acceleration * @param[out] acc_linear2_z - Second Z acceleration + * @param[in] velocity_decay - Exponential velocity decay rate (1/s, default 0.0 = no decay). + * Predicted velocity is multiplied by exp(-velocity_decay * dt). */ template inline void predict(const T position1_x, const T position1_y, const T position1_z, const T orientation1_r, @@ -82,7 +84,8 @@ inline void predict(const T position1_x, const T position1_y, const T position1_ const T acc_linear1_x, const T acc_linear1_y, const T acc_linear1_z, const T dt, T& position2_x, T& position2_y, T& position2_z, T& orientation2_r, T& orientation2_p, T& orientation2_y, T& vel_linear2_x, T& vel_linear2_y, T& vel_linear2_z, T& vel_angular2_r, T& vel_angular2_p, - T& vel_angular2_y, T& acc_linear2_x, T& acc_linear2_y, T& acc_linear2_z) + T& vel_angular2_y, T& acc_linear2_x, T& acc_linear2_y, T& acc_linear2_z, + double velocity_decay = 0.0) { // 3D material point projection model which matches the one used by r_l. const T sr = ceres::sin(orientation1_r); @@ -117,13 +120,19 @@ inline void predict(const T position1_x, const T position1_y, const T position1_ orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; - vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; - vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; + // Exponential velocity decay: when velocity_decay > 0, predicted velocity decays toward zero + // each prediction step. This prevents indefinite drift when the velocity sensor goes silent + // (e.g., wheel odometry stops publishing after a navigation controller deactivates). + // decay_factor = 1.0 when velocity_decay = 0 (no decay, preserves existing behavior). + const T decay_factor = ceres::exp(T(-velocity_decay) * dt); - vel_angular2_r = vel_angular1_r; - vel_angular2_p = vel_angular1_p; - vel_angular2_y = vel_angular1_y; + vel_linear2_x = vel_linear1_x * decay_factor + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y * decay_factor + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z * decay_factor + acc_linear1_z * dt; + + vel_angular2_r = vel_angular1_r * decay_factor; + vel_angular2_p = vel_angular1_p * decay_factor; + vel_angular2_y = vel_angular1_y * decay_factor; acc_linear2_x = acc_linear1_x; acc_linear2_y = acc_linear1_y; @@ -168,6 +177,8 @@ inline void predict(const T position1_x, const T position1_y, const T position1_ * @param[out] acc_linear2_y - Second Y acceleration * @param[out] acc_linear2_z - Second Z acceleration * @param[out] jacobians - Jacobians wrt the state + * @param[in] velocity_decay - Exponential velocity decay rate (1/s, default 0.0 = no decay). + * Predicted velocity is multiplied by exp(-velocity_decay * dt). */ inline void predict(double const position1_x, double const position1_y, double const position1_z, double const orientation1_r, double const orientation1_p, double const orientation1_y, @@ -178,7 +189,7 @@ inline void predict(double const position1_x, double const position1_y, double c double& orientation2_p, double& orientation2_y, double& vel_linear2_x, double& vel_linear2_y, double& vel_linear2_z, double& vel_angular2_r, double& vel_angular2_p, double& vel_angular2_y, double& acc_linear2_x, double& acc_linear2_y, double& acc_linear2_z, double** jacobians, - double* jacobian_quat2rpy) + double* jacobian_quat2rpy, double velocity_decay = 0.0) { // 3D material point projection model which matches the one used by r_l. double const sr = ceres::sin(orientation1_r); @@ -213,13 +224,15 @@ inline void predict(double const position1_x, double const position1_y, double c orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; - vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; - vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; + double const decay_factor = std::exp(-velocity_decay * dt); + + vel_linear2_x = vel_linear1_x * decay_factor + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y * decay_factor + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z * decay_factor + acc_linear1_z * dt; - vel_angular2_r = vel_angular1_r; - vel_angular2_p = vel_angular1_p; - vel_angular2_y = vel_angular1_y; + vel_angular2_r = vel_angular1_r * decay_factor; + vel_angular2_p = vel_angular1_p * decay_factor; + vel_angular2_y = vel_angular1_y * decay_factor; acc_linear2_x = acc_linear1_x; acc_linear2_y = acc_linear1_y; @@ -317,11 +330,11 @@ inline void predict(double const position1_x, double const position1_y, double c jacobian(2, 1) = sr * cp * dt; jacobian(2, 2) = cr * cp * dt; // partial derivatives of vel_linear2_x wrt vel_linear1 - jacobian(6, 0) = 1.0; + jacobian(6, 0) = decay_factor; // partial derivatives of vel_linear2_y wrt vel_linear1 - jacobian(7, 1) = 1.0; + jacobian(7, 1) = decay_factor; // partial derivatives of vel_linear2_z wrt vel_linear1 - jacobian(8, 2) = 1.0; + jacobian(8, 2) = decay_factor; } // Jacobian wrt vel_angular1 @@ -341,11 +354,11 @@ inline void predict(double const position1_x, double const position1_y, double c jacobian(5, 1) = sr * cpi * dt; jacobian(5, 2) = cr * cpi * dt; // partial derivatives of vel_angular2_r wrt vel_angular1 - jacobian(9, 0) = 1.0; + jacobian(9, 0) = decay_factor; // partial derivatives of vel_angular2_p wrt vel_angular1 - jacobian(10, 1) = 1.0; + jacobian(10, 1) = decay_factor; // partial derivatives of vel_angular2_y wrt vel_angular1 - jacobian(11, 2) = 1.0; + jacobian(11, 2) = decay_factor; } // Jacobian wrt acc_linear1 @@ -398,13 +411,14 @@ inline void predict(double const position1_x, double const position1_y, double c template inline void predict(const T* const position1, const T* const orientation1, const T* const vel_linear1, const T* const vel_angular1, const T* const acc_linear1, const T dt, T* const position2, - T* const orientation2, T* const vel_linear2, T* const vel_angular2, T* const acc_linear2) + T* const orientation2, T* const vel_linear2, T* const vel_angular2, T* const acc_linear2, + double velocity_decay = 0.0) { predict(position1[0], position1[1], position1[2], orientation1[0], orientation1[1], orientation1[2], vel_linear1[0], vel_linear1[1], vel_linear1[2], vel_angular1[0], vel_angular1[1], vel_angular1[2], acc_linear1[0], acc_linear1[1], acc_linear1[2], dt, position2[0], position2[1], position2[2], orientation2[0], orientation2[1], orientation2[2], vel_linear2[0], vel_linear2[1], vel_linear2[2], vel_angular2[0], - vel_angular2[1], vel_angular2[2], acc_linear2[0], acc_linear2[1], acc_linear2[2]); + vel_angular2[1], vel_angular2[2], acc_linear2[0], acc_linear2[1], acc_linear2[2], velocity_decay); } /** * @brief Given a state and time delta, predicts a new state @@ -424,7 +438,7 @@ inline void predict(fuse_core::Vector3d const& position1, Eigen::Quaterniond con fuse_core::Vector3d const& vel_linear1, fuse_core::Vector3d const& vel_angular1, fuse_core::Vector3d const& acc_linear1, double const dt, fuse_core::Vector3d& position2, Eigen::Quaterniond& orientation2, fuse_core::Vector3d& vel_linear2, - fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2) + fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2, double velocity_decay = 0.0) { fuse_core::Vector3d rpy(fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), @@ -434,7 +448,7 @@ inline void predict(fuse_core::Vector3d const& position1, Eigen::Quaterniond con vel_linear1.z(), vel_angular1.x(), vel_angular1.y(), vel_angular1.z(), acc_linear1.x(), acc_linear1.y(), acc_linear1.z(), dt, position2.x(), position2.y(), position2.z(), rpy.x(), rpy.y(), rpy.z(), vel_linear2.x(), vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), - acc_linear2.y(), acc_linear2.z()); + acc_linear2.y(), acc_linear2.z(), velocity_decay); // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * @@ -461,7 +475,8 @@ inline void predict(fuse_core::Vector3d const& position1, Eigen::Quaterniond con fuse_core::Vector3d const& vel_linear1, fuse_core::Vector3d const& vel_angular1, fuse_core::Vector3d const& acc_linear1, double const dt, fuse_core::Vector3d& position2, Eigen::Quaterniond& orientation2, fuse_core::Vector3d& vel_linear2, - fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2, fuse_core::Matrix15d& jacobian) + fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2, fuse_core::Matrix15d& jacobian, + double velocity_decay = 0.0) { double quat[4] = { orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z() }; double rpy[3]; @@ -491,7 +506,7 @@ inline void predict(fuse_core::Vector3d const& position1, Eigen::Quaterniond con vel_linear1.z(), vel_angular1.x(), vel_angular1.y(), vel_angular1.z(), acc_linear1.x(), acc_linear1.y(), acc_linear1.z(), dt, position2.x(), position2.y(), position2.z(), rpy[0], rpy[1], rpy[2], vel_linear2.x(), vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), - acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy); + acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy, velocity_decay); // TODO(henrygerardmoore): figure out how to fix this // see https://github.com/locusrobotics/fuse/pull/354#discussion_r1884288806 diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 068681238..e9fd711f3 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -96,7 +96,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateCostFunction(double const dt, fuse_core::Matrix15d const& A); + Omnidirectional3DStateCostFunction(double const dt, fuse_core::Matrix15d const& A, double velocity_decay = 0.0); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -161,7 +161,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 dt_, position_pred[0], position_pred[1], position_pred[2], orientation_pred_rpy[0], orientation_pred_rpy[1], orientation_pred_rpy[2], vel_linear_pred[0], vel_linear_pred[1], vel_linear_pred[2], vel_angular_pred[0], vel_angular_pred[1], vel_angular_pred[2], acc_linear_pred[0], acc_linear_pred[1], acc_linear_pred[2], - jacobians, j1_quat2rpy); + jacobians, j1_quat2rpy, velocity_decay_); residuals[0] = parameters[5][0] - position_pred[0]; residuals[1] = parameters[5][1] - position_pred[1]; @@ -267,12 +267,14 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 private: double dt_; + double velocity_decay_{ 0.0 }; fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root //!< information matrix }; -Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction(double const dt, fuse_core::Matrix15d const& A) - : dt_(dt), A_(A) +Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction(double const dt, fuse_core::Matrix15d const& A, + double const velocity_decay) + : dt_(dt), velocity_decay_(velocity_decay), A_(A) { } diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index c85091a70..a12aef671 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -104,7 +104,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint fuse_variables::VelocityLinear3DStamped const& velocity_linear2, fuse_variables::VelocityAngular3DStamped const& velocity_angular2, fuse_variables::AccelerationLinear3DStamped const& acceleration_linear2, - fuse_core::Matrix15d const& covariance); + fuse_core::Matrix15d const& covariance, double velocity_decay = 0.0); /** * @brief Destructor @@ -163,6 +163,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint protected: double dt_; //!< The time delta for the constraint + double velocity_decay_{ 0.0 }; //!< Exponential velocity decay rate (1/s), forwarded to predict() fuse_core::Matrix15d sqrt_information_; //!< The square root information matrix private: @@ -181,6 +182,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint { archive& boost::serialization::base_object(*this); archive& dt_; + archive& velocity_decay_; archive& sqrt_information_; } }; diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index a1d06f350..5d5c4e71e 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -231,6 +231,15 @@ void Omnidirectional3D::onInit() disable_checks_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "disable_checks"), disable_checks_); + velocity_decay_ = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "velocity_decay"), velocity_decay_); + + if (velocity_decay_ < 0.0) + { + throw std::runtime_error("Invalid negative velocity_decay of " + std::to_string(velocity_decay_) + + " specified. Must be >= 0."); + } + double buffer_length = 3.0; buffer_length = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "buffer_length"), buffer_length); @@ -284,7 +293,7 @@ void Omnidirectional3D::generateMotionModel(rclcpp::Time const& beginning_stamp, { predict(base_state.position, base_state.orientation, base_state.vel_linear, base_state.vel_angular, base_state.acc_linear, (beginning_stamp - base_time).seconds(), state1.position, state1.orientation, - state1.vel_linear, state1.vel_angular, state1.acc_linear); + state1.vel_linear, state1.vel_angular, state1.acc_linear, velocity_decay_); } else { @@ -308,7 +317,8 @@ void Omnidirectional3D::generateMotionModel(rclcpp::Time const& beginning_stamp, // Now predict to get an initial guess for the state at the ending stamp StateHistoryElement state2; predict(state1.position, state1.orientation, state1.vel_linear, state1.vel_angular, state1.acc_linear, dt, - state2.position, state2.orientation, state2.vel_linear, state2.vel_angular, state2.acc_linear); + state2.position, state2.orientation, state2.vel_linear, state2.vel_angular, state2.acc_linear, + velocity_decay_); // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); @@ -407,7 +417,8 @@ void Omnidirectional3D::generateMotionModel(rclcpp::Time const& beginning_stamp, // Create the constraints for this motion model segment auto constraint = fuse_models::Omnidirectional3DStateKinematicConstraint::make_shared( name(), *position1, *orientation1, *velocity_linear1, *velocity_angular1, *acceleration_linear1, *position2, - *orientation2, *velocity_linear2, *velocity_angular2, *acceleration_linear2, process_noise_covariance); + *orientation2, *velocity_linear2, *velocity_angular2, *acceleration_linear2, process_noise_covariance, + velocity_decay_); // Update the output variables constraints.push_back(constraint); @@ -509,7 +520,7 @@ void Omnidirectional3D::updateStateHistoryEstimates(fuse_core::Graph const& grap predict(previous_state.position, previous_state.orientation, previous_state.vel_linear, previous_state.vel_angular, previous_state.acc_linear, (current_stamp - previous_stamp).seconds(), current_state.position, current_state.orientation, current_state.vel_linear, current_state.vel_angular, - current_state.acc_linear); + current_state.acc_linear, velocity_decay_); } } } diff --git a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp index 6d053827e..47ee794ff 100644 --- a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -57,13 +57,15 @@ Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstr fuse_variables::Position3DStamped const& position2, fuse_variables::Orientation3DStamped const& orientation2, fuse_variables::VelocityLinear3DStamped const& velocity_linear2, fuse_variables::VelocityAngular3DStamped const& velocity_angular2, - fuse_variables::AccelerationLinear3DStamped const& acceleration_linear2, fuse_core::Matrix15d const& covariance) + fuse_variables::AccelerationLinear3DStamped const& acceleration_linear2, fuse_core::Matrix15d const& covariance, + double const velocity_decay) : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), velocity_linear1.uuid(), velocity_angular1.uuid(), acceleration_linear1.uuid(), position2.uuid(), orientation2.uuid(), velocity_linear2.uuid(), velocity_angular2.uuid(), acceleration_linear2.uuid() }) , // NOLINT dt_((position2.stamp() - position1.stamp()).seconds()) + , velocity_decay_(velocity_decay) , sqrt_information_(covariance.inverse().llt().matrixU()) { } @@ -89,7 +91,7 @@ void Omnidirectional3DStateKinematicConstraint::print(std::ostream& stream) cons ceres::CostFunction* Omnidirectional3DStateKinematicConstraint::costFunction() const { - return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_); + return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_, velocity_decay_); // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // diff --git a/fuse_models/test/test_omnidirectional_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp index 26e97be53..91423d285 100644 --- a/fuse_models/test/test_omnidirectional_3d_predict.cpp +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -490,3 +490,108 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); } + +TEST(Predict, VelocityDecayZeroIsNoOp) +{ + // GIVEN a state with known non-zero velocity and zero decay + fuse_core::Vector3d position1(1.0, 2.0, 0.0); + Eigen::Quaterniond orientation1(Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ())); + fuse_core::Vector3d vel_linear1(0.5, 0.3, 0.0); + fuse_core::Vector3d vel_angular1(0.0, 0.0, 0.1); + fuse_core::Vector3d acc_linear1(0.0, 0.0, 0.0); + double const dt = 0.05; + + fuse_core::Vector3d position2_default, position2_zero; + Eigen::Quaterniond orientation2_default, orientation2_zero; + fuse_core::Vector3d vel_linear2_default, vel_angular2_default, acc_linear2_default; + fuse_core::Vector3d vel_linear2_zero, vel_angular2_zero, acc_linear2_zero; + + // WHEN predicting with default (no decay arg) and explicit velocity_decay = 0.0 + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2_default, + orientation2_default, vel_linear2_default, vel_angular2_default, acc_linear2_default); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2_zero, + orientation2_zero, vel_linear2_zero, vel_angular2_zero, acc_linear2_zero, 0.0); + + // THEN results are identical — velocity_decay = 0.0 is a no-op + EXPECT_DOUBLE_EQ(vel_linear2_default.x(), vel_linear2_zero.x()); + EXPECT_DOUBLE_EQ(vel_linear2_default.y(), vel_linear2_zero.y()); + EXPECT_DOUBLE_EQ(vel_angular2_default.z(), vel_angular2_zero.z()); +} + +TEST(Predict, VelocityDecayReducesVelocity) +{ + // GIVEN a robot with residual velocity, no acceleration, and decay enabled + fuse_core::Vector3d position1(0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1(Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ())); + fuse_core::Vector3d vel_linear1(0.2, 0.15, 0.0); + fuse_core::Vector3d vel_angular1(0.0, 0.0, 0.1); + fuse_core::Vector3d acc_linear1(0.0, 0.0, 0.0); + double const dt = 0.05; + double const k = 1.0; + double const expected_decay_factor = std::exp(-k * dt); + + fuse_core::Vector3d position2, position2_no_decay; + Eigen::Quaterniond orientation2, orientation2_no_decay; + fuse_core::Vector3d vel_linear2, vel_angular2, acc_linear2; + fuse_core::Vector3d vel_linear2_no_decay, vel_angular2_no_decay, acc_linear2_no_decay; + + // WHEN predicting with decay enabled + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2, k); + + // THEN linear velocity is multiplied by decay_factor = exp(-k * dt) + EXPECT_NEAR(vel_linear2.x(), vel_linear1.x() * expected_decay_factor, 1e-9); + EXPECT_NEAR(vel_linear2.y(), vel_linear1.y() * expected_decay_factor, 1e-9); + EXPECT_NEAR(vel_linear2.z(), vel_linear1.z() * expected_decay_factor, 1e-9); + + // AND angular velocity decays by the same factor + EXPECT_NEAR(vel_angular2.z(), vel_angular1.z() * expected_decay_factor, 1e-9); + + // WHEN predicting with no decay for comparison + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2_no_decay, + orientation2_no_decay, vel_linear2_no_decay, vel_angular2_no_decay, acc_linear2_no_decay, 0.0); + + // THEN velocity with decay is strictly smaller in magnitude + EXPECT_GT(std::abs(vel_linear2_no_decay.x()), std::abs(vel_linear2.x())); + EXPECT_GT(std::abs(vel_linear2_no_decay.y()), std::abs(vel_linear2.y())); +} + +TEST(Predict, VelocityDecayJacobiansMatchDecayFactor) +{ + // GIVEN a state and a known decay rate + double const k = 1.0; + double const dt = 0.05; + double const expected_decay_factor = std::exp(-k * dt); + + double position1[3] = { 0.0, 0.0, 0.0 }; + double orientation1[3] = { 0.0, 0.0, 0.0 }; + double vel_linear1[3] = { 0.2, 0.15, 0.0 }; + double vel_angular1[3] = { 0.0, 0.0, 0.1 }; + double acc_linear1[3] = { 0.0, 0.0, 0.0 }; + double position2[3], orientation2[3], vel_linear2[3], vel_angular2[3], acc_linear2[3]; + + std::array, 5> J_data; + std::array jacobians = { J_data[0].data(), J_data[1].data(), J_data[2].data(), J_data[3].data(), + J_data[4].data() }; + double j_quat2rpy[12] = {}; + + // WHEN computing analytical Jacobians with decay enabled + fuse_models::predict(position1[0], position1[1], position1[2], orientation1[0], orientation1[1], orientation1[2], + vel_linear1[0], vel_linear1[1], vel_linear1[2], vel_angular1[0], vel_angular1[1], + vel_angular1[2], acc_linear1[0], acc_linear1[1], acc_linear1[2], dt, position2[0], position2[1], + position2[2], orientation2[0], orientation2[1], orientation2[2], vel_linear2[0], vel_linear2[1], + vel_linear2[2], vel_angular2[0], vel_angular2[1], vel_angular2[2], acc_linear2[0], + acc_linear2[1], acc_linear2[2], jacobians.data(), j_quat2rpy, k); + + // THEN d(vel_linear2_i)/d(vel_linear1_i) = decay_factor (not 1.0) + Eigen::Map> J_vel_linear(J_data[2].data()); + EXPECT_NEAR(J_vel_linear(6, 0), expected_decay_factor, 1e-9); + EXPECT_NEAR(J_vel_linear(7, 1), expected_decay_factor, 1e-9); + EXPECT_NEAR(J_vel_linear(8, 2), expected_decay_factor, 1e-9); + + // AND d(vel_angular2_i)/d(vel_angular1_i) = decay_factor (not 1.0) + Eigen::Map> J_vel_angular(J_data[3].data()); + EXPECT_NEAR(J_vel_angular(9, 0), expected_decay_factor, 1e-9); + EXPECT_NEAR(J_vel_angular(10, 1), expected_decay_factor, 1e-9); + EXPECT_NEAR(J_vel_angular(11, 2), expected_decay_factor, 1e-9); +}