jhy
/
NotesUESTC
forked from logzhan/NotesUESTC
1
0
Fork 0
Code Pull Requests Activity
NotesUESTC/算法开发笔记/组合导航/GPS算法工具.md

985 B
Raw Blame History

GPS 算法Python工具

一、距离的计算

#--------------------------------------------------------------------------
# Function    : cal_distance
# Descripiton : 给定经纬度, 计算两点之间距离m
# Author      : zhanli@vivo.com
# Date        : 2022-07-06
#--------------------------------------------------------------------------
def cal_distance(lat1,lon1,lat2,lon2):
	dis = 0
	# WGS-84椭球体长半轴
	WGS84_RE = 6378136.49				        
	# 第一偏向率的平方，e^2(WGS-84)
	WGS84_ECCENTR2 = 6.69437999014e-3
	# 角度转弧度系数
	DEG2RAD = 0.017453292                      

	lat = lat1 * DEG2RAD

	sinLat = math.sin(lat)
	p = 1 - WGS84_ECCENTR2 * (sinLat * sinLat)
	temp =  math.sqrt(p)

	rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (p*temp)
	rnh = WGS84_RE * math.cos(lat) / temp

	rmh *= DEG2RAD
	rnh *= DEG2RAD

	latDiff = rmh*(lat1 - lat2)
	lonDiff = rnh*(lon1 - lon2)

	dis = math.sqrt(latDiff*latDiff + lonDiff*lonDiff)
	
	return dis