Integrate GPS properly in robot_localization
How do I properly integrate the gps data for the global pose estimation in robot_localization pkg for a real-world car? I fused imu and twist data to get the local estimation. They both are working fine. Now, I want to use the GPS data too. Do I need to run 2 instances of robot_localization pkg? Or one is enough? I tried the following and but it does not work well.
ekf_local: imu/data + geometry_twist = odometry/filtered/local
From the imu, I used only angular z(yaw) velocity and linear x(roll) or forward acceleration. From the twist, I used linear x velocity and angular yaw velocity.
navsat_node: imu/data + gps/fix + odometry/filtered/local = odometry_gps
ekf_global in an empty map: imu/data + geometry_twist + odometry_gps = odometry/filtered/global
Is this the correct configuration? I also tried the following with only one robot_localization instance but it throws an error something like
Nan in filter output, incorrect process, noise, or covariance setup
I used the output of the ekf_global node as an input to the navsat_node. Similarly, the output of navsat_node as an input to the ekf_global node. I found this configuration in the package ros wiki page.
navsat_node: imu/data + gps/fix + odometry/filtered = odometry_gps
ekf_global in an empty map: imu/data + geometry_twist + odometry_gps = odometry/filtered
My 2nd question is when I plot the twist_yaw_velocity and imu_yaw_velocity, they always differ by an offset and the fused output velocity follows the twist data 99% time. It seems not to consider the imu at all. Is there any reason for it?
Edit 1: This is the plot of twist yaw velocity and imu yaw velocity.
What is the accuracy of your GPS data under operating conditions (e.g. 100 km/hour), compared to ground truth?
Is your IMU mounted at base_link, or somewhere away from base_link?
@Mike Scheutzow 1. I could not test that. But it is an RTK GNSS receiver. 2. IMU is not mounted on base_link. That's why I am publishing a static transform from base_link to imu_frame.
The "accuracy of the GPS receiver" is something you look up in the specs, factoring in the environment you expect to use it in. The accuracy matters because this helps determine whether you can use the GPS data as absolute latitude/longitude, or if the data is good only for estimating the relative movement over the last N seconds.
If the IMU is not mounted at base_link, then the base_link yaw velocity and the IMU yaw velocity are not expected to match. But if you were to transform the IMU yaw velocity to base_link, it should be close.
@Mike Scheutzow Thanks for your answer. Which one of my configurations is correct? I think the second one is correct. I believe the reason for the "output filter nan error" is my simulation IMU Pose does not work. It has -1 as the first covariance element. But velocity and acceleration work fine.
There is no single, best solution to this, so I don't know if either of your configurations is "correct." If you want to assume the gps is "accurate enough", then I don't see the benefit of sending the gps data into a kalman filter at all.