FactorMarginlization function implemented.  Implementation not vectorized, is imperative C-style code and should be changed.