New fusion matlab algorithm

This is the current version. The former one has several mistakes.