Sensor fusion in Gazebo simulator
GPS
As you remember, I am using my own GPS simulator instead of one provided by Gazebo, as I am too lazy to transform lat/lon into Gazebo flat world coordinates.
The error is not that large: the rectangle I use for trajectory in Gazebo is smaller than one I used in no-Gazebo simulation.
Z-coordinate is modelled properly, too:
Overall Kalman error is smaller than GPS error (which has sigma equal 10):
Finally, heading is not modelled properly (which is to be expected):
Accelerometer
Accelerometer in my code is implemented via integration (UKF receives sensor data as speed, not acceleration). An advantage is, UKF doesn't have to perform integration, which is a majos source of instability. A disadvantage - the error accumulates, as generally Kalman filter is not that good at integration.
Also notice that I store global frame velocity and acceleration in UKF state vector. It is good for stability, as takes off the burden of working with local frame from UKF (and puts it to our code, when we want to pass data from real accelerometer, which will be in local frame).
Gyroscope
Gyroscope provides decent position estimation and good orientation estimation. There are some implementation details that are covered in comments, in code.
Magnetometer
Megnetometer provides decent position estimation and good orientation estimation.
GPS + Accelerometer
| GPS | GPS + Accelerometer |
|
 |
|
 |
|
 |
|
 |
As Kalman filter got new data (and data can be used in the content of a task), the quality of a simulation improves.
GPS + Gyroscope
| GPS | GPS + Gyroscope |
|
 |
|
 |
|
 |
|
 |
As Kalman filter got new data (and data can be used in the content of a task), the quality of a simulation improves. Improvement is not that large here, as robot's orientation does not, generally, help in finding its coordinates, yet it is noticeable.
GPS + Magnetometer
| GPS | GPS + Magnetometer |
|
 |
|
 |
|
 |
|
 |
Again, we got better position (and orientation) than for GPS alone.
Navigation.py code
Let's walk through the code of Navigation.py file. Keep in mind that if kalman.py is not present, the imitation will be used instead of real localization. It will work in Gazebo, but not in a real world.