Embedded sensor fusion algorithm