Fixed a bug in the model, now I need to finish the model in matlab to verify it.