GPS

import numpy as np
from scipy.optimize import least_squares
import random

# Function to compute residuals (differences between observed and calculated distances)
def residuals(params, satellite_positions, distances):
	x, y, z, delta_t = params  # Position of the receiver and clock error
	calculated_distances = np.sqrt((satellite_positions[:, 0] - x)**2 +
								   (satellite_positions[:, 1] - y)**2 +
								   (satellite_positions[:, 2] - z)**2) + c * delta_t
	return calculated_distances - distances


# Convert (x, y, z) ECEF universe coordinates to Earth Latitude, Longitude, and Altitude (LLA)
def ecef_to_lla(x, y, z):
	# WGS-84 ellipsoid constants
	a = 6378137.0  # Semi-major axis in meters
	e = 8.1819190842622e-2  # Eccentricity
	
	# Calculate longitude
	lon = np.arctan2(y, x)
	
	# Iterative calculation for latitude and altitude
	p = np.sqrt(x**2 + y**2)
	lat = np.arctan2(z, p * (1 - e**2))
	for _ in range(5):  # Iteratively improve latitude calculation
		N = a / np.sqrt(1 - e**2 * np.sin(lat)**2)
		alt = p / np.cos(lat) - N
		lat = np.arctan2(z + e**2 * N * np.sin(lat), p)
	
	# Convert from radians to degrees
	lat = np.degrees(lat)
	lon = np.degrees(lon)
	
	return lat, lon, alt


c = 299792458  # Speed of light in meters per second


if __name__ == '__main__':
	# Example satellite positions (x, y, z in meters)
	satellite_positions = np.array([
		[15600000, 7540000, 20140000],   # Satellite 1 position
		[18760000, 2750000, 18610000],   # Satellite 2 position
		[17610000, 14630000, 13480000],  # Satellite 3 position
		[19170000, 610000, 18390000],    # Satellite 4 position
		[15890000, 6900000, 19700000]    # Satellite 5 position
	])

	# Simulated time delays from each satellite to receiver (in seconds)
	# These would be the time differences observed by the GPS receiver.
	# We'll assume the times have been adjusted relative to the first satellite.
	time_delays = np.array([0.19122486, 0.19430291, 0.18634412, 0.19425415, 0.18664831])

	# Initial guess for the receiver's position (x, y, z) and clock error (delta_t)
	
	initial_guess = [0, 0, 0, 0]

	# Convert time delays to distances
	distances = (time_delays) * c  # in meters

	# Solve the system using least squares minimization
	result = least_squares(residuals, initial_guess, args=(satellite_positions, distances))

	# Extract the solution
	x, y, z, delta_t = result.x

	# Convert the (x, y, z) solution to Latitude, Longitude, and Altitude
	latitude, longitude, altitude = ecef_to_lla(x, y, z)
	print(f"Receiver position (Altitude, Latitude, Longitude): ({altitude:.2f}, {latitude:.6f}, {longitude:.6f} meters)")