Improved rotation matrix renormalization

Added the ability to define a heading
added ability to set misalignment of sensor