GPSとUnity内のオブジェクトの動きを連動させたい

前提・実現したいこと

Andorid端末で現在地と移動先の緯度経度を取得し、それを直交座標（X,Y,Z座標）になおし、その差を計算し、それを基にして、Unity内のオブジェクトを動かしたいと考えています。
イメージ的にはポケモンGOに近いモノです。
・一定の時間で更新される緯度経度を更新前の値と更新後の値の差分を取りたいと考えており、その一つ前の値を保持する方法
・緯度経度を直交座標に直すための処理を何度も繰り返しソースに書かなくてもよい方法
この2点を解決したいと考えております。

該当のソースコード

C#
1using System.Collections;
2using System.Collections.Generic;
3using UnityEngine;
4using Mathd;
5using System;
6
7/// <summary>
8/// 座標の変換用クラス。なるべくシンプルに必要な変換だけ。
9/// 測地系はWGS84を使用。
10/// 計算方法は「世界測地系と座標変換　飛田幹夫　日本測量協会」を参考にした。
11/// </summary>
12public class GPS_move : MonoBehaviour
13{
14    private const double a = 6378137.0;
15    private const double f = 1.0 / 298.257223563;
16    private const double e2 = f * (2.0 - f);
17    /// <summary>経緯度取得間隔（秒）</summary>
18    private const float IntervalSeconds = 1.0f;
19    /// <summary>ロケーションサービスのステータス</summary>
20    private LocationServiceStatus locationServiceStatus;
21    /// <summary>経度</summary>
22    public float Longitude { get; private set; }
23    /// <summary>経度</summary>
24    public float Latitude { get; private set; }
25    ///<summary>高度</summary>
26    public float altitude { get; private set; }
27    ///<summary>水平方向の精度</summary>
28    public float horizontalAccuracy { get; private set; }
29    ///<summary>垂直方向の精度</summary>
30    public float verticalAccuracy { get; private set; }
31    /// <summary>緯度経度情報が取得可能か</summary>
32    /// <returns>可能ならtrue、不可能ならfalse</returns>
33    public bool CanGetLonLat()
34    {
35        return Input.location.isEnabledByUser;
36    }
37
38    private IEnumerator Start()
39    {
40        while (true)
41        {
42            locationServiceStatus = Input.location.status;
43            if (Input.location.isEnabledByUser)
44            {
45                switch (locationServiceStatus)
46                {
47                    case LocationServiceStatus.Stopped:
48                        Input.location.Start();
49                        break;
50                    case LocationServiceStatus.Running:
51                        Longitude = Input.location.lastData.longitude;
52                        Latitude = Input.location.lastData.latitude;
53                        altitude = Input.location.lastData.altitude;
54                        horizontalAccuracy = Input.location.lastData.horizontalAccuracy;
55                        verticalAccuracy = Input.location.lastData.verticalAccuracy;
56
57                        break;
58                    default:
59                        break;
60                }
61            }
62
63　　　　　　//直交座標に直す
64            Longitude = Math.PI * Longitude / 180.0;
65            Latitude = Math.PI * Latitude / 180.0;
66
67            double N = a / Math.Sqrt(1.0 - e2 * Math.Pow(Math.Sin(Longitude), 2.0));
68
69            double X = (N + altitude) * Math.Cos(Longitude) * Math.Cos(Latitude);
70            double Z = (N + altitude) * Math.Cos(Longitude) * Math.Sin(Latitude);
71            double Y = (N * (1.0 - e2) + altitude) * Math.Sin(Longitude);
72
73            var vec3d = new Vector3d(X, Y, Z);
74            yield return new WaitForSeconds(IntervalSeconds);
75
76
77        }
78    }
79
80
81
82}