Complementary filter reference frame

[Alpistinho]

Hello,

I've found an old issue mentioning that the complementary filter reports its orientation in NWU convention. Since I've needed it to publish it as ENU, I did a super quick workaround and rotated the quaternion at the end:

void ComplementaryFilterROS::publish(
    const sensor_msgs::Imu::ConstPtr& imu_msg_raw)
{
  // Get the orientation:
  double q0, q1, q2, q3;
  filter_.getOrientation(q0, q1, q2, q3);
  tf::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3);
  
  //Rotate orientation to report in ENU coordinate convention instead of NWU. -- Alpistinho
  tf::Quaternion qRot = tf::createQuaternionFromRPY(0, 0, M_PI/2);
  q = qRot*q;
  q.normalize();

Is this ok or am I completely crazy?

Thanks for the attention,

Daniel

[mintar]

Hello Daniel,

I haven't tested your code, but I think it should be correct. There are some tests in imu_filter_madgwick, maybe you can adapt them to imu_complementary_filter to check whether your implementation is correct.

Best wishes,
Martin

[robertogl]

Hi Daniel, If I remember correctly (I know I could take a look at the code but I am lazy 😑 ) the publish function publishes the quaternion after it has been inverted to represent the rotation from local to global, instead of global to local as it is initially calculated. It that's the case, I think you should apply the opposite rotation (0, 0, -M_PI/2) otherwise instead of getting ENU you get WSU ☺. But the best thing to do is always run the code to check the behavior. Also, It would be great to have a parameter that allows the user to choose between ENU and NWU. Thank you for your changes. Best, Roberto

…

On Thu, Oct 25, 2018 at 6:23 PM Daniel Moreira Guimarães < ***@***.***> wrote: Hello, I've found an old issue mentioning that the complementary filter reports its orientation in NWU convention. Since I've needed it to publish it as ENU, I did a super quick workaround and rotated the quaternion at the end: void ComplementaryFilterROS::publish( const sensor_msgs::Imu::ConstPtr& imu_msg_raw) { // Get the orientation: double q0, q1, q2, q3; filter_.getOrientation(q0, q1, q2, q3); tf::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3); //Rotate orientation to report in ENU coordinate convention instead of NWU. -- Alpistinho tf::Quaternion qRot = tf::createQuaternionFromRPY(0, 0, M_PI/2); q = qRot*q; q.normalize(); Is this ok or am I completely crazy? Thanks for the attention, Daniel — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#91>, or mute the thread <https://github.com/notifications/unsubscribe-auth/ACGPK9Rul8tlZ9nDkKt1E4ZDNlZUxw4Jks5uojnWgaJpZM4X7N4_> .

[Alpistinho]

Hello,

Thanks for the answers.

In the short tests I've done it seems to match the Madgwick filter output configured to ENU, so I thought it was ok.
I will try to make it configurable by parameter and send a pull request for it

About the tests @mintar mentioned, would you mean some kind of unit test?

Best regards,

Daniel

[mintar]

@Alpistinho: Sorry for not replying earlier, somehow I missed your reply 2 weeks ago. Yes, I mean the unit tests here:

https://github.com/ccny-ros-pkg/imu_tools/tree/4ebe1628aaac533b5f791dcfd141a61c96214b6b/imu_filter_madgwick/test

If your output matches that of the imu_filter_madgwick, it should be correct. Looking forward to the pull request! :)

[Alpistinho]

Hi @mintar,

Don't worry, I have been pretty busy lately too.
I will have a look on these tests and check if my modification make sense.
You can close this issue if you want.

Thanks,

Daniel

[Sollimann]

Are you using ENU convention now for the complementary filter? I am thinking about using your package, but I have to make sure its ENU convention.

Thanks

[Alpistinho]

Hi @Sollimann,

Unfortunately I had to revert to use the Madgwick due to schedule issues, but I was pretty sure the convention was right using what I described above.

I still intend to implement what @mintar suggested about the unit tests, but I had to prioritize other things right now.

[mintar]

@Sollimann The imu_complimentary_filter still uses the NWU convention, but imu_filter_madgwick has a parameter to choose the convention (ENU by default).

[maxpolzin]

Hi @Alpistinho and @robertogl,

Have you clarified whether the rotation has to be M_PI/2 or -M_PI/2 to report the orientation in ENU frame?

[mintar]

@maxpolzin : The easiest way to check is to run the imu_filter_madgwick and imu_complementary_filter in parallel. The imu_filter_madgwick uses ENU by default.

[maxpolzin]

Thx, excellent point. I created a draft PR.

The rotation was indeed +PI/2. Quaternions have the ambiguity of q = - q. I am outputting -q to have the obtain the same numeric values as the madgwick filter.