Проблема проекции в фильтре Калмана для навигации

В настоящее время я работаю над простым и небольшим фильтром Калмана для GPS-навигации. Я получаю от своего GPS-датчика текущее местоположение, угол курса и скорость. Таким образом, фильтр Калмана должен объединить текущее измерение и линейное движение, начиная с предыдущего места, предполагая постоянную скорость и курсовой угол.

Моя проблема состоит в том, чтобы выбрать полезное пространство, в котором фильтр Калмана может работать в хорошем состоянии. путь.

Подход к местной системе координат:

Если я выберу местную систему координат (север [метр], восток [метр]) с предыдущим местоположением в начале координат, я смогу легко предсказать новое местоположение, но как преобразовать новое измерение (широту / долготу) в мою локальную систему координат с помощью эллипсоида wgs-84? и как преобразовать мои новые прогнозы в моей локальной системе координат в широту / долготу, также используя эллипсоид wgs-84?

Итак, мне нужны две функции:

f: = (lat_ref, lng_ref, lat, lng) -> (x, y)

g: = (lat_ref, lng_ref, x, y) -> (lat, lng) (это также можно сделать с помощью Винсенти )

Подход к глобальной системе координат:

Я нашел Алгоритм Винсенти , который вычисляет новое местоположение, исходя из исходного местоположения, расстояния и курсового угла на любом эллипсоиде. Этот алгоритм работает нормально, но я не понимаю, как использовать этот алгоритм внутри фильтра Калмана, который работает в глобальной системе координат.

Есть ли какие-нибудь идеи или предложения, как решить одну из моих проблем?