GPSの研究 その9

概要

GPSを理解したかった。
衛星の位置と、擬似距離から、受信機の位置を求めて見た。

写真

Aは、GPS衛星。
Bは、受信機。
精度は、かなり、悪い。

サンプルコード

import numpy as np
import math
from scipy.optimize import minimize
import pyproj

ecef = pyproj.Proj(proj = 'geocent', ellps = 'WGS84', datum = 'WGS84')
lla = pyproj.Proj(proj = 'latlong', ellps = 'WGS84', datum = 'WGS84')

def cost(p):
	c = 0
	#c += math.fabs(22402645.008 - math.sqrt((6262234.031370984 - p[0]) ** 2 + (-14613110.981922923 - p[1]) ** 2 + (21254935.229655903 - p[2]) ** 2))
	c += math.fabs(20565303.43 - math.sqrt((19040661.998708855 - p[0]) ** 2 + (-8311756.524602333 - p[1]) ** 2 + (16532658.634675292 - p[2]) ** 2))
	#c += math.fabs(22181773.156 - math.sqrt((26078664.69836943 - p[0]) ** 2 + (5588070.077162541 - p[1]) ** 2 + (-239561.27794465935 - p[2]) ** 2))
	#c += math.fabs(24495107.648 - math.sqrt((24470813.305349953 - p[0]) ** 2 + (-95731.17177840695 - p[1]) ** 2 + (-10965308.78942738 - p[2]) ** 2))
	c += math.fabs(21478321.484 - math.sqrt((10946425.106999923 - p[0]) ** 2 + (-13217422.981333993 - p[1]) ** 2 + (19960118.04486387 - p[2]) ** 2))
	c += math.fabs(20168944.047 - math.sqrt((17400028.443231095 - p[0]) ** 2 + (1461399.2922087098 - p[1]) ** 2 + (19887536.270703305 - p[2]) ** 2))
	c += math.fabs(23186783.133 - math.sqrt((14385911.569008492 - p[0]) ** 2 + (20514420.345839 - p[1]) ** 2 + (8765413.838559393 - p[2]) ** 2))
	c += math.fabs(21860039.258 - math.sqrt((9336918.80677975 - p[0]) ** 2 + (12048291.269695656 - p[1]) ** 2 + (21723935.800126303 - p[2]) ** 2))
	return c

p0 = [4000000, 0, 4000000]
bound = [(0, None), (0, None), (0, None)]
res = minimize(cost, p0, method = 'l-bfgs-b', bounds = bound, options = {
	'disp': True,
	'maxls': 20,
	'gtol': 1e-05,
	'eps': 1e-08,
	'maxiter': 150,
	'ftol': 2e-08,
	'maxcor': 4,
})
print (res)
lon, lat, alt = pyproj.transform(ecef, lla, res.x[0], res.x[1], res.x[2], radians = False)
print (lon, lat, alt)

結果

RUNNING THE L-BFGS-B CODE

           * * *

Machine precision = 2.220D-16
 N =            3     M =           10

At X0         1 variables are exactly at the bounds

At iterate    0    f=  4.43772D+06    |proj g|=  3.72529D+00

At iterate    1    f=  4.43769D+06    |proj g|=  3.72529D+00

At iterate    2    f=  4.43594D+06    |proj g|=  5.58794D+00
  ys=-8.007E+02  -gs= 1.530E+03 BFGS update SKIPPED

At iterate    3    f=  4.43053D+06    |proj g|=  3.72529D+00

At iterate    4    f=  2.11219D+06    |proj g|=  4.47035D+00

At iterate    5    f=  1.74121D+06    |proj g|=  3.72529D+00

At iterate    6    f=  1.24733D+06    |proj g|=  2.98023D+00

At iterate    7    f=  8.65099D+05    |proj g|=  3.72529D+00

At iterate    8    f=  6.83237D+05    |proj g|=  2.23517D+00

At iterate    9    f=  6.82295D+05    |proj g|=  1.86265D+00

At iterate   10    f=  6.20497D+05    |proj g|=  2.60770D+00

At iterate   11    f=  5.78396D+05    |proj g|=  2.23517D+00

At iterate   12    f=  5.52165D+05    |proj g|=  1.49012D+00

At iterate   13    f=  5.44790D+05    |proj g|=  1.11759D+00

At iterate   14    f=  5.38430D+05    |proj g|=  3.72529D-01

At iterate   15    f=  5.34551D+05    |proj g|=  1.49012D+00

At iterate   16    f=  5.33953D+05    |proj g|=  2.60770D+00

At iterate   17    f=  5.30030D+05    |proj g|=  7.45058D-01

At iterate   18    f=  5.29649D+05    |proj g|=  1.86265D+00
  ys=-1.886E+02  -gs= 1.258E+02 BFGS update SKIPPED

At iterate   19    f=  5.29645D+05    |proj g|=  1.86265D+00

At iterate   20    f=  5.28446D+05    |proj g|=  1.49012D+00

At iterate   21    f=  5.25145D+05    |proj g|=  1.86265D+00

At iterate   22    f=  5.25038D+05    |proj g|=  1.86265D+00

At iterate   23    f=  5.25025D+05    |proj g|=  1.86265D+00

At iterate   24    f=  5.24887D+05    |proj g|=  1.49012D+00

At iterate   25    f=  5.24159D+05    |proj g|=  7.45058D-01

At iterate   26    f=  5.24149D+05    |proj g|=  7.45058D-01

At iterate   27    f=  5.24147D+05    |proj g|=  3.72529D-01

At iterate   28    f=  5.24145D+05    |proj g|=  3.72529D-01

At iterate   29    f=  5.24144D+05    |proj g|=  7.45058D-01

At iterate   30    f=  5.24144D+05    |proj g|=  7.45058D-01
  ys=-1.064E-02  -gs= 6.878E-03 BFGS update SKIPPED

At iterate   31    f=  5.24144D+05    |proj g|=  7.45058D-01

           * * *

Tit   = total number of iterations
Tnf   = total number of function evaluations
Tnint = total number of segments explored during Cauchy searches
Skip  = number of BFGS updates skipped
Nact  = number of active bounds at final generalized Cauchy point
Projg = norm of the final projected gradient
F     = final function value

           * * *

   N    Tit     Tnf  Tnint  Skip  Nact     Projg        F
    3     31    111     35     3     1   7.451D-01   5.241D+05
  F =   524144.402850389     

CONVERGENCE: REL_REDUCTION_OF_F_<=_FACTR*EPSMCH             

 Cauchy                time 0.000E+00 seconds.
 Subspace minimization time 0.000E+00 seconds.
 Line search           time 0.000E+00 seconds.

 Total User time 0.000E+00 seconds.

      fun: 524144.4028503895
 hess_inv: <3x3 LbfgsInvHessProduct with dtype=float64>
      jac: array([ 0.74505806,  2.60770321,  0.        ])
  message: b'CONVERGENCE: REL_REDUCTION_OF_F_<=_FACTR*EPSMCH'
     nfev: 444
      nit: 31
   status: 0
  success: True
        x: array([ 5034934.99510941,        0.        ,  3999067.43098859])
0.0, 40.7, 28160

以上。