Development of a Sensor Fusion Algorithm in the Matlab System