Fixing fusion and navi matlab algo.