Param update for MARG now much more robust