Combining sensors for orientation
We've seen how we combined the accelerometer and gyroscope to get smooth readings for pitch and roll. We can combine the sensors again to correctly orient and smooth the magnetometer readings too. This system allows us to approximate the absolute orientation of the robot.
Take a look at the following data flow to see what we are doing—it builds on the previous stages:
Figure 16.17 – Fusing all three sensors
Figure 16.17 starts on the left with data from our previous stages. We have the filtered pitch and roll in gray because it's also an output. There's the calibrated gyroscope yaw, delta time, and also the calibrated magnetometer as inputs. The filtered pitch and roll go through the tilt-compensate box, where we rotate the magnetometer vector. The magnetometer data then goes through an xy-to-polar box, using the atan2 function to get a heading.
Above this, the calibrated gyroscope yaw...
