Sensor fusion in Gazebo
So far we used a simulator i introduced in a previous section. Now we need to do the same with Gazebo simulator.
There is nothing new in the code except for some really minor changes, so let's focus on testing instead.
Important note. In the previous section, a two wheeled robot was introduced. We are still using it. First of all, odometer will simply not work with 4 wheeled robot, not Gazebo odometer. It is ok, as we decided to not use odometer except for an indoor environment. But there is the second reason: we want same code to generate same trajectories in our home made simulator and in Gazebo. And it is only possible with 2 wheeled robot.
GPS
As I use SDF world with slippery area (slip.sdf), robot at some point slips and slides sideways. You can see the ground truth black trajectory bending, while slippage unaware odometry path (green) remains unchanged.
Kalman trajectory (magenta) follows the real path, and has error better than original GPS data:
Accelerometer
As accelerometer "knows" when we change the speed, it follows the ground truth trajectory: nothing unexpected here.
Gyroscope
Trajectory is trying to follow the "ideal" path with no slippsge, and has large deviations due to the fact that gyro doesn't provide info about linear velocity. Note that "command" does provide such info, so we can, in theory, improve this result, either by increasing filter's confidence in commands, or perhaps, by investigating why the velocity became wrong. Probably, slippage term...
Accelerometer plus GPS
Trajectory improved compared to just GPS:
| GPS | Accelerometer plus GPS |
|
|
|
|
|
GPS plus Gyroscope
As gyroscope doesn't provide info we need to improve coordinates... The error increases. Note that it is within error range, if we run same simulation again, it might decrease. But it definitely didn't improve.
GPS plus Magnetometer
Again, we see slightly larger error in Kalman coordinates. It is within the error range, but what does it mean nevertheless?
Our magnetometer is probably not accurate enough.
Accelerometer plus GPS plus Gyroscope
Accuracy increased and so did heading accuracy.
Accelerometer plus GPS plus Magnetometer
Accelerometer plus GPS plus Gyroscope plus Magnetometer
Paying attention to data
A nice example of how data can damage your filter's performance. Compare:
vs
Here is the difference:
vs
And the actual trajectory is (no slippage)
vs
The problem is in an abrupt "stop" command at the end of each cycle that confuses the filter adding error to it: robot can not stop abruptly (not with our xacro) so it creates discrepancy that model doesn't take into consideration.
Conclusion
This is pretty much all we can squeeze out of a non-specialized Kalman filter. It is possible to improve it by adjusting it to a specific robot. Particularly, I chose one of the most uncomfortable designs (dif. drive) and a world with the lot of slippage. But it is better than ignoring those to "keep the code simple". I think.
Plans:
Make localization code work in 3d world.
Add landmarks support
Add computer vision support (like staying in the middle of the road)
and so on