I am using robot_localization to fuse gps and imu referencing this Tom Moore answer: http://answers.ros.org/question/200071/how-to-fuse-imu-gps-using-robot_localization/
My setup is the same with a full-size SUV vehicle, Beaglebone Black board, Phidgets imu w/mag, ublox gps, with no odometry sensors. I am running Ubuntu 14.04.4 LTS, deb 4.1.15-ti-rt-r40 armv7l, ros indigo, and robot_localization with the ekf filter.
I've been able to produce decent results, but the mobile trial awaits resolving issues, so everything is tested in the static position. I've calibrated the magnetometer for hard and soft iron on the vehicle and incorporate the factors in the driver. I use an overall launch file that separately calls the devices and robot_localization. REP's 103 and 105 were adhered to with the imu (as far as I can tell), since the raw mag points (X) east at zero and the other checks (Y and Z) work out. Declination is accounted for in the navsat_transform launch file.
The issue arises with the diagnostic output where this occurs:
level: 1
name: ekf_localization: odometry/filtered topic status
message: Frequency too low.
hardware_id: none
values:
-
key: Events in window
value: 17
-
key: Events since startup
value: 172
-
key: Duration of window (s)
value: 16.443676
-
key: Actual frequency (Hz)
value: 1.033832
-
key: Minimum acceptable frequency (Hz)
value: 55.800000
-
key: Minimum acceptable frequency (Hz)
value: 72.600000
I've checked the frequencies of all the incoming topics and they all seem ok except for the /tf topic which also shows around 1 hz. The transform frames correctly seem to tie odom to base_link with child frames phid_imu and gps.
A secondary factor may be in play is the 0 yaw covariance factor produced by the Phidgets imu driver. I cannot seem to find where this can be adjusted to help convergence. The gps is running at 4hz(fortunately) so it will determine all gps inputs (ie. /odometry/gps).
I am asking how to resolve the low ekf_localization diagnostic issue and will likely have to ask another question about the yaw covariance in the Phidgets driver.
I've placed the diagnostic output, device output, launch files, node graph, and transform tree on Dropbox https://www.dropbox.com/sh/u3kznp4oiygbuan/AADAD_QmU_LieGK3y_sbxR65a?dl=0
Any ideas towards this end? Please forgive the text formatting.
Regards and Thank you,
b2256
EDIT 03/10/16 Per Tom Moore's comment below, I did perform the ekf/imu test and found that if the data rate value in the imu was slowed to 40ms, the frequencies of /odometry/filtered and /tf were helped. I then implemented the full launch with the same result although it takes a few minutes to stabilize. Will these values at 25 hz be sufficient for operation? Additional tweaks involved reducing the ekf data rate to 28 hz hoping to stabilize (with limited success at ~25.6 hz, std dev 0.0155s, and max of 0.353s). Note that these results were via ssh (from a Windows laptop) into the BBB, so some latency is expected. Are there any other adjustments that need to be made?
EDIT 03/13/16
Although I may not have found a final solution, but with Tom Moore's troubleshooting strategy and modifying the output frequency and the ekf_localization frequency seemed to optimize the system to within usable results (~21 hz for the desired topics.) Tested filter with new bagfiles and results were acceptable for my current use case. Many thanks to Tom Moore and all others that contribute here.
EDIT 03/14/16 Current launch files and new Bagfiles from 2 runs here: https://www.dropbox.com/sh/u3kznp4oiygbuan/AADAD_QmU_LieGK3y_sbxR65a?dl=0 Note cpu results in cpu_utilization.odt CPU is overutilized at 90%+!! I am running off of an sd card but no swap shows up (previously reserved a lot.) Thank you so much Tom Moore for helping to keep the dialog going.
Originally posted by b2256 on ROS Answers with karma: 162 on 2016-03-08
Post score: 1
Original comments
Comment by Tom Moore on 2016-03-08:
Why is tf publishing at 1 Hz? The filter is going to look up transforms from the frame_ids of your sensor inputs to the frame_id of your world or base_link frames. Also, can you please post sample messages for each input?
Comment by b2256 on 2016-03-08:
Tom, I do not have any idea why tf is at 1 Hz. Nor where I might find the params? The sensor inputs are included in the 030716_diagnostics.odt file from the Dropbox link. The node diagram as well as the transform tree are there as well. Any advice would be greatly appreciated.
Comment by Tom Moore on 2016-03-08:
Can you try stripping out everything from the launch file and starting with one sensor alone?
Comment by b2256 on 2016-03-08:
Yes. Should I start with just the imu.launch? That is where the call for the madgwick (pre)filter (imu_filter_madgwick/ImuFilterNodelet) is setup. I expect that it will be not until early this evening before I can get back to it (at work.) Thank you in advance for an ongoing dialog.
Comment by Tom Moore on 2016-03-08:
No problem. Which sensor you use first is irrelevant; what matters is that you run with just one sensor and see if the behavior improves. I'm just trying to troubleshoot sensors.
Comment by b2256 on 2016-03-09:
I've now tried imu only and gps only; both work as expected. When the ekf and navsat are added topics /gps/filtered and /tf go to around 1 hz. I've also tried on a desktop as opposed to the BBB, and frequencies are excellent. An intermittent imu connection problem also fixed. More on Dropbox link
Comment by Tom Moore on 2016-03-09:
No, sorry, what I meant was run the EKF with, for example, just the IMU as input. Don't run navsat_transform_node. Strange that it runs on the desktop. Are you building from source?
Comment by b2256 on 2016-03-10:
No, not from source but it did work. See edit above for continuation.
Comment by Tom Moore on 2016-03-13:
I find that it's strange that your data rates are suffering as much as they are. That's not a very heavy load for the EKF; I've done more with similar boards and still maintained the specified EKF rate. What's the CPU usage like?
Comment by b2256 on 2016-03-14:
Even checked swappiness. No change, topics still in area of 20-25 hz. According to glances app around 50% of cpu is used on sdcard based ekf filter. Not sure where to go from here. See bagfiles, maybe something there that I cannot see?