Rename ONE -> ONE128, ZERO -> ZERO128 and lonLatH -> lonLatH128
Add tests for `ecef_from_wgs84'
Calculate some constants
Select a stable cost function according to (xy, z)

Previous cost function was very sensitive to small deviation of lat around PI/2.
This commit introduces two cost functions, one is suitable for lat value around PI/2, the other is suitable around 0.