Added theta update step
Merge branch 'master' of https://github.com/rocket-raccoon/6998_HDDA_Project
Adding theta update step