In actual scenarios, sensor observation will be influenced by various kinds of factors. There might be noises, sudden errors or steady shifts during the robot operation. Relying on any single sensor cannot always guarantee a correct perception of outside environment.
For example, the inertia measurement unit (IMU) observation may have noises and accumulated error as shown in the figure below.
The simulation shows that even though some sensors are having errors throughout the whole process and some sources provide huge sudden error, the final fusion result still keeps on the right track.
The algorithm was initially realized in visualized MATLAB environment to accelerate the system development. After the simulation verification, embedded C code is generated, modified, and downloaded to embedded boards.
High-performance Teensy® 3.2 and 3.5 boards are connected as fusion center:
ROS system is used as the core unit and exchange data among sensors and fusion processing boards. And the modularity allows the extension to more sensors.