Return to Answer

The full code is available on my StackExchange Signal Processing Q51386 GitHub Repository.

The full code is available on my StackExchange Signal Processing Q51386 GitHub Repository (Look at the SignalProcessing\Q51386 folder).

Converting coordinate system is really the main reason the Extended Kalman Filter was invented.

Yet I will tell you tip, it doesn't work well in those cases.
If you use Non Linear Transformation use something that will both make things easier and better (Yea, usually it doesn't work like that, but in this case it does) - Use the Unscented Kalman Filter (UKF) which is based on the Unscented Transform.
Once you utilize that there is no need to derive the Jacobian.
All needed is to apply the non linear function $ n $ times (On each Sigma Point).

Converting coordinate system is the main reason the Extended Kalman Filter was invented.

Let me give you a tip, it doesn't work well in those cases.
If you use Non Linear Transformation use something that will both make things easier and better (Yea, usually it doesn't work like that, but in this case it does) - Use the Unscented Kalman Filter (UKF) which is based on the Unscented Transform.
Once you utilize that there is no need to derive the Jacobian.
All needed is to apply the non linear function $ n $ times (On each Sigma Point).

I implemented a general Kalman Filter Iteration with support for Extended Kalman Filter (With option for Numeric Calculation of the Jacobian).

The full code is available on my StackExchange Signal Processing Q51386 GitHub Repository.

I implemented a general Kalman Filter Iteration with support for Extended Kalman Filter (With option for Numeric Calculation of the Jacobian).
I also added option to Unsecented Kalman Filter (UKF) Iteration, so you will be able to compare.

The full code is available on my StackExchange Signal Processing Q51386 GitHub Repository.

24/08/2018: I added UKF implementation which is pretty general so you could use it in various models.