Skip to content
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

unicycle motion model: add option to rotate the process noise covariance to the current state orientation #325

Open
wants to merge 8 commits into
base: devel
Choose a base branch
from
+185 −8
Open

unicycle motion model: add option to rotate the process noise covariance to the current state orientation #325

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
66cd212
unicycle motion model: add option to rotate the process noise covaria…
fabianhirmann May 9, 2023
7238f0d
fix linter errors for unicycle motion model: rotate process noise cov…
fabianhirmann May 9, 2023
05abcac
unicycle motion model: apply rotation of process noise covariance onl…
fabianhirmann May 10, 2023
555ac04
unicycle motion model: rotate process noise with covariance: fixes fo…
fabianhirmann May 10, 2023
04592ef
unicycle motion model: rotate process noise covariance with state ori…
fabianhirmann May 10, 2023
7f8b97a
unicycle motion model: create unit test for generateMotionModel and n…
fabianhirmann May 15, 2023
ecb11a0
unicycle motion model: revert previous changes which created a separa…
fabianhirmann May 16, 2023
39bbb99
Update test_unicycle_2d to rostest instead of pure gtest after now ro…
fabianhirmann Jul 13, 2023
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
5 changes: 4 additions & 1 deletion fuse_models/include/fuse_models/unicycle_2d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
*
* @param[in] state1 The first/oldest state
* @param[in] state2 The second/newest state
* @param[in] process_noise_covariance The process noise covariance, after it is scaled and multiplied by dt
* @param[in] process_noise_covariance The process noise covariance, after it is rotated, scaled and multiplied by dt
*/
static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2,
const fuse_core::Matrix8d& process_noise_covariance);
Expand All @@ -189,6 +189,9 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
fuse_core::Matrix8d process_noise_covariance_; //!< Process noise covariance matrix
bool scale_process_noise_{ false }; //!< Whether to scale the process noise covariance pose by the norm
//!< of the current state twist
bool rotate_process_noise_covariance_to_state_orientation_{ false }; //!< Whether to rotate the process noise
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As I said before, I wonder if this should always be done. I sounds like it's probably the right thing to do. In that case, I think we should always do it, and we wouldn't need a ROS param at all. 🤔

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I also believe that this should always be done and would enable it for all our robots. That's why I now also removed the ROS parameter with the latest commit.
The only reason why I created a ROS parameter before and set it to false by default was that existing applications using fuse are not changed and remain backwards compatibility. If you agree that this is no issue, then I would be happy to have it always enabled. Otherwise I will just revert the last commit and I'm fine with a ROS parameter too.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If the math is wrong, then I would consider it a bug. I see no need for an option to retain the wrong math either.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How can we confirm it was a bug?

Is it something we can clearly see from the math?

@fabianhirmann Would you be able to provide a unit test that only passes with your changes but didn't before? 🤔

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just pushed a unit test for it that does not pass before my changes but only with my changes. There I also wrote an explanation why I expect that the final process noise covariance should be rotated. Summarized my perspective is that the process noise covariance is defined in the robot frame so when the position is defined in the world frame, we need to rotate the process noise covariance to the world frame (using the yaw angle that defines the orientation relation between the robot frame and world frame).
An example, imagine we are defining the process noise covariance with cov_robot_xx = 2.0 and cov_robot_yy = 1.0 (assuming that the process noise covariance is in the robot frame there is then less process noise covariance in the lateral direction than the longitudinal direction). Then, when having yaw angle = 90°, I would expect that the final process noise covariance applied to the position in the world frame would have cov_world_xx = 1.0 and cov_world_yy = 2.0 because the orientation of the robot is upwards and the covariance must then get 90° rotated.

//!< covariance to the orientation of the
//!< current state using its yaw angle
double velocity_norm_min_{ 1e-3 }; //!< The minimum velocity/twist norm allowed when scaling the
//!< process noise covariance
bool disable_checks_{ false }; //!< Whether to disable the validation checks for the current and predicted state,
Expand Down
18 changes: 17 additions & 1 deletion fuse_models/src/unicycle_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,10 @@ void Unicycle2D::onInit()
process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal();

private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_);
private_node_handle_.param("rotate_process_noise_covariance_to_state_orientation",
rotate_process_noise_covariance_to_state_orientation_,
rotate_process_noise_covariance_to_state_orientation_);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Indentation seems to be missing one space for each line here:

Suggested change
rotate_process_noise_covariance_to_state_orientation_,
rotate_process_noise_covariance_to_state_orientation_);
rotate_process_noise_covariance_to_state_orientation_,
rotate_process_noise_covariance_to_state_orientation_);


Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the extra lines for the others are only because they need extra work other than just calling param, so I wouldn't add a new line here:

Suggested change

private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_);

private_node_handle_.param("disable_checks", disable_checks_, disable_checks_);
Expand Down Expand Up @@ -348,8 +352,20 @@ void Unicycle2D::generateMotionModel(
state_history_.emplace(beginning_stamp, std::move(state1));
state_history_.emplace(ending_stamp, std::move(state2));

// Scale process noise covariance pose by the norm of the current state twist

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove extra empty line:

Suggested change

auto process_noise_covariance = process_noise_covariance_;

// Rotate the process noise covariance with the yaw angle of the current state orientation
if (rotate_process_noise_covariance_to_state_orientation_)
{
auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This can be const:

Suggested change
auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();
const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();

// apply only to x and y position as the other state variables are already along the
// current state orientation
process_noise_covariance.topLeftCorner<2, 2>() =
rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose();
}

// Scale process noise covariance pose by the norm of the current state twist
if (scale_process_noise_)
{
common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw,
Expand Down