# import numpy as np
# import necessary libraries
from math import pi
from sympy import Matrix, symbols, atan, sqrt
# Parametric model equations in form of l = f(x)
xP, yP, x1, y1, x2, y2, x3, y3 = symbols('xP yP x1 y1 x2 y2 x3 y3', real=True)
beta1 = atan((xP-x1)/(yP-y1)) - atan((x2-x1)/(y2-y1))
beta2 = atan((xP-x2)/(yP-y2)) - atan((x3-x2)/(y3-y2))
beta3 = atan((x2-x3)/(y2-y3)) - atan((xP-x3)/(yP-y3))
# Parametric model equations in matrix form
X = Matrix([beta1, beta2, beta3])
Y = Matrix([[xP, yP]]); X
# Jacobian matrix wrt unknown parameters
J = X.jacobian(Y); J
# Jacobian evaluated at given values as A matrix
A = J.evalf(subs = {xP: 200, yP: 400, x1: 600, y1: 800, x2: 900, y2: 700, x3: 600, y3: 100}); A
# Weight matrix P
num = len(X) # determines dimension of weight matrix
P = np.zeros((num, num), float) # initialise the weight matrix with zeroes
invP = np.reciprocal([((2/3600)*pi/180)**2, ((2/3600)*pi/180)**2, ((2/3600)*pi/180)**2]) # compute inverse of standard deviation squared
np.fill_diagonal(P, invP) # fill P diagonal with inverse of standard deviation squared
P = Matrix(P); P
# Matrix of coefficients of normal equations N
N = A.transpose()@P@A
Q = N.inv(); display(N, Q)
sxx = sqrt(Q[0,0]); syy = sqrt(Q[1,1]); sxy = Q[0,1]
print(f'sdx = {sxx:.4f}m sdy = {syy:.4f}m sdxy = {sxy:.4e}')
z = sqrt((Q[0,0]-Q[1,1])**2+4*Q[0,1]**2)
lmb1 = 0.5*(Q[0,0]+Q[1,1]+z)
lmb2 = 0.5*(Q[0,0]+Q[1,1]-z)
print(f'z = {z:.5e} lambda1 = {lmb1:.5e} lambda2 = {lmb2:.5e}')
# chi-squared test with similar proportions
from scipy.stats import chi2
confidence = 0.95; dof = 2; critical = chi2.ppf(confidence, dof)
print(f'chi2_score: {critical:.3f}')
def decimaltodms(Decimal): # Function converts decimal degrees to degree minutes and seconds
dd = int(Decimal); m = int((Decimal - dd)*60); s = (Decimal - dd - m/60)*3600
if s == 0: return f"{dd}° {m}' {s}\""
if s > 0: return f"{dd}° {m}' {s:.1f}\""
else: return f"{abs(dd)}° {abs(m)}' {abs(s):.1f}\""
ast = sqrt(lmb1); bst = sqrt(lmb2); k = sqrt(critical); a95 = ast*k; b95 = bst*k
theta = atan((lmb1-Q[1,1])/Q[0,1])
print(f'ast = {ast:.3e}m bst = {bst:.3e}m theta = {decimaltodms(theta * 180 / pi)}')
print(f'a95 = {a95:.4f}m b95 = {b95:.4f}m theta = {decimaltodms(theta * 180 / pi)}')