Added formation model and controller