Robust Model Aggregation for Heterogeneous Federated Learning: Analysis and Optimizations