Aug28 Fixed coordinate frame issues, algorithm mostly working, needs tuning with penalty