GX5-25 KF IMU publishing

[samkys]

This PR allows the user to set and publish the filtered IMU data from the GX5-25. Currently the only filtered data the original driver output was the CF data for the -25 and the Nav data from the GPS enabled devices. I wanted access to the KF data from the -25 since we can get the accelerations with gravity removed as well as the KF orientation covariance values. This allows dynamic covariance publishing rather than the static ones that were previously set. You can still use the static values if you publish just the standard IMU data.

[samkys]

Don't merge this quite yet. There is something funny that I don't like about how the filtered data implementation works on the -25 model. Just realized that if you want to publish filtered data, it still relies on the AHRS imu setup rate. So, if you want really high data rates you will need to choose between either KF IMU data, CF data, or crank up the baud rate.

[samkys]

Few fixes should fix my last comment. Will be testing with it today and report back.

[samkys]

Got a chance to try it out. It works better now but there still is one more thing I want to try and fix.

[samkys]

@bsb808 @tonybaltovski Should be good to go now. I tested it with a -25 and we are able to get the filtered IMU data out. It is setup so you can put it into the old setup, i.e. no filtered IMU output or put it into the new mode with filtered IMU output, or both on or off.

The benefit to this PR is that with the filtered IMU output the IMU removes the gravity vector for us as well as compensates for angular and acceleration bias. With the filtered data you are not required to remove gravity as it's just a parameter in the launch file but it is helpful to prior to feeding it into an EKF, say robot_localization.

[samkys]

Any feedback?

[tonybaltovski]

Just one last nit before we can merge this

[samkys]

@tonybaltovski How does that look?

[tonybaltovski]

Thanks @samkys