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
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
以上。
Author And Source
この問題について(GPSの研究 その9), 我々は、より多くの情報をここで見つけました https://qiita.com/ohisama@github/items/3380556e8d27b8a01d09著者帰属：元の著者の情報は、元のURLに含まれています。著作権は原作者に属する。
Content is automatically searched and collected through network algorithms . If there is a violation . Please contact us . We will adjust (correct author information ,or delete content ) as soon as possible .