Skip to content

Add velocity_decay to Omnidirectional3D to prevent drift when odom goes silent #34

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

Draft
bkanator wants to merge 1 commit into
base: humble
Choose a base branch
from
Draft

Add velocity_decay to Omnidirectional3D to prevent drift when odom goes silent #34

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 9 additions & 0 deletions fuse_models/include/fuse_models/omnidirectional_3d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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
};

Expand Down
67 changes: 41 additions & 26 deletions fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename T>
inline void predict(const T position1_x, const T position1_y, const T position1_z, const T orientation1_r,
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Comment on lines +129 to +131

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;
Expand Down Expand Up @@ -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,
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Comment on lines +229 to +231

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;
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -398,13 +411,14 @@ inline void predict(double const position1_x, double const position1_y, double c
template <typename T>
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
Expand All @@ -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()),
Expand All @@ -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()) *
Expand All @@ -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];
Expand Down Expand Up @@ -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
Expand Down
10 changes: 6 additions & 4 deletions fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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];
Expand Down Expand Up @@ -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)
{
}

Expand Down
4 changes: 3 additions & 1 deletion fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -181,6 +182,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint
{
archive& boost::serialization::base_object<fuse_core::Constraint>(*this);
archive& dt_;
archive& velocity_decay_;
archive& sqrt_information_;
}
};
Expand Down
19 changes: 15 additions & 4 deletions fuse_models/src/omnidirectional_3d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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
{
Expand All @@ -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_);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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_);
}
}
}
Expand Down
Loading
Loading