Moved simUpdate to utilities
Removed optimizations for readability
Merge branch 'EKF-Revert' of github.com:jemofthewest/DOECPF into EKF-Revert