Initial rotation for the imu_filter_madgwick etc.

[PaulKemppi]

Hello,

Thanks again for the excellent contribution to the ROS community. I have still few suggestions related to the imu_filter:

Is there some reason why the initial orientation is set to (1, 0, 0, 0) and not based on e.g. the first acceleration vector (that could provide good estimate for the 2 degrees of freedom assuming that the sensor is initially stationary)? If the filter gain is set to a small value (0.01), it takes quite long time for the filter to converge especially if the sensor happens to have totally different initial attitude compared to the (1, 0, 0, 0). On the other hand, setting the gain to 1.0, will result faster convergence, but more noise is then introduced to the output later on.

How about adding a possibility to adjust the gain parameters via dynamic reconfigure (http://ros.org/wiki/dynamic_reconfigure) on the fly? That would be a convenient way to search the optimal gain values for a specific sensor / application.

Best Regards,
Paul

[idryanov]

On 15-Oct-12 17:01, PaulKemppi wrote:

Hello,

Thanks again for the excellent contribution to the ROS community. I have
still few suggestions related to the imu_filter:

Is there some reason why the initial orientation is set to (1, 0, 0, 0)
and not based on e.g. the first acceleration vector (that could provide
good estimate for the 2 degrees of freedom assuming that the sensor is
initially stationary)? If the filter gain is set to a small value
(0.01), it takes quite long time for the filter to converge especially
if the sensor happens to have totally different initial attitude
compared to the (1, 0, 0, 0). On the other hand, setting the gain to
1.0, will result faster convergence, but more noise is then introduced
to the output later on.

How about adding a possibility to adjust the gain parameters via dynamic
reconfigure (http://ros.org/wiki/dynamic_reconfigure) on the fly? That
would be a convenient way to search the optimal gain values for a
specific sensor / application.

Best Regards,
Paul

—
Reply to this email directly or view it on GitHub
#4.

Both sound like excellent suggestions. I will make the appropriate changes

[idryanov]

Dynamic reconfigure for the gain param has been added.

[idryanov]

Added initialization of the orientation (roll & pitch) based on the acceleration vector.

I don't have an IMU at hand, could someone verify that this is working? I will close the ticket upon confirmation.

TODO: initialize the yaw from the magentic orientation, when using magnetic field readings.

[sameerparekh]

I tried this with an IMU I have handy and I couldn't tell any difference in the behavior between before and after this change. I fired up the filter when the IMU was oriented randomly and not pointed in any particular direction.

On Monday, November 5, 2012 at 5:45 PM, Ivan Dryanovski wrote:

Added initialization of the orientation (roll & pitch) based on the acceleration vector.
I don't have an IMU at hand, could someone verify that this is working? I will close the ticket upon confirmation.
TODO: initialize the yaw from the magentic orientation, when using magnetic field readings.

—
Reply to this email directly or view it on GitHub (#4 (comment)).

[idryanov]

Just to confirm: with the new changes, when you set the gain low (like
0.01) does it still give you a slow convergence to the right angle?

On Tue, Nov 6, 2012 at 5:23 PM, Sameer Parekh notifications@github.comwrote:

I tried this with an IMU I have handy and I couldn't tell any difference
in the behavior between before and after this change. I fired up the filter
when the IMU was oriented randomly and not pointed in any particular
direction.

On Monday, November 5, 2012 at 5:45 PM, Ivan Dryanovski wrote:

Added initialization of the orientation (roll & pitch) based on the
acceleration vector.
I don't have an IMU at hand, could someone verify that this is working?
I will close the ticket upon confirmation.
TODO: initialize the yaw from the magentic orientation, when using
magnetic field readings.

—
Reply to this email directly or view it on GitHub (
#4 (comment)).

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/4#issuecomment-10130381.

[sameerparekh]

Yes, it does.

Sameer Parekh
Falkor Systems, Inc.

On Tuesday, November 6, 2012 at 5:46 PM, Ivan Dryanovski wrote:

Just to confirm: with the new changes, when you set the gain low (like
0.01) does it still give you a slow convergence to the right angle?

[idryanov]

If anyone is willing to record a small bag of raw imu data I could pursue this further. It would make a nice addition to the package to have a small test set. Thanks!

[sameerparekh]

I've submitted a pull request with data from both my AR.drone's IMU and my spark fun razor IMU. The spark fun IMU appears to have a very large bias on the gyro though. I wasn't exactly sure what kind of IMU data is best to test the filter with however.

On Tuesday, November 6, 2012 at 10:24 PM, Ivan Dryanovski wrote:

If anyone is willing to record a small bag of raw imu data I could pursue this further. It would make a nice addition to the package to have a small test set. Thanks!

—
Reply to this email directly or view it on GitHub (#4 (comment)).

[mintar]

First off, thanks a lot for your great work! We bought a Phidgets IMU and run it with your ROS driver and imu_filter_magdwick. Works like a charm (after deactivating the magnetometer).

I've found a small bug in the orientation initialization, though. It doesn't work with upside-down mounted IMUs. I've hacked a workaround here: uos/imu_tools@44b8781. Also, I'll add a small test rosbag from the phidgets IMU and submit a pull request for that.

[PaulKemppi]

Hi,

Still about the initial orientation: At least with my Phidgets IMU (model 1044), the initial orientation was still occasionally wrong depending on the initial attitude of the sensor. By negating the pitch the initial orientation seems to be always correct. I tested the initialization in 6 different cases (x up/down, y up/down, z up/down),

double sign = copysignf(1.0, lin_acc.z);
double pitch = -atan2(lin_acc.x, sqrt(lin_acc.y_lin_acc.y + lin_acc.z_lin_acc.z));
double roll = atan2(lin_acc.y, sign * sqrt(lin_acc.x_lin_acc.x + lin_acc.z_lin_acc.z));

[lorenzoriano]

The code from @PaulKemppi seems to work fine with me, on a Phidget Spatial.

[mintar]

Ok, I merged this change into all 3 branches (master, groovy, fuerte). Tested on Phidgets Spatial 1044.