Nonlinear position estimator

Change-Id: Ibedfbf08acc1fc33c0cf363b9fc1a5148c097441
Merge "Nonlinear position estimator"