Added right sensor, bug fixed with translation Matrix of sensors!
Ordered includes, unified formatting of all files