small fix
correct an error and add initialization

correct an error([mf,Pf] = fusion_CI(m2_tmp,mf,P1_tmp,Pf););
add initialization of the mf and Pf in track to system mode when mf = [0
0 0 0 0 0]'; and Pf = diag([0 0 0 0 0 0]);