# -*- coding: utf-8 -*-
'''
PRECISION DU PERCEMENT D'UN TUNNEL

Polygonale lancée à N côtés, avec détermination gyroscopique de l'azimut
géographique à la fin de chaque section de n côtés. Pour n > N, on ne
considère aucune mesure gyroscopique.

Hypothèse 1: portail parfait
On néglige l'imprécision de la position à l'entrée du tunnel (écart-type
latéral), ainsi que celle du gisement de l'orientation initiale vers un
point extérieur.

Hypothèse 2: calibration parfaite du gyroscope
Grâce aux coordonnées approchées des points, on peut calculer la valeur
exacte de la convergence du méridien pour chaque côté et convertir les
azimuts géographiques en gisements sans perte de précision. Bref, c'est
comme si l'on mesurait directement des gisements avec le gyroscope.

Idée conductrice: le gisement d'un côté mesuré directement avec le
gyroscope doit correspondre au gisement obtenu indirectement par
cheminement polygonal.

1. On calcule les gisements de la polygonale lancée par propagation
   des mesures angulaires, supposées indépendantes. Toutefois, les
   gisements sont corrélés puisqu'une mesure angulaire influence tous les
   gisements suivants. En fin de compte, c'est comme si l'on avait
   observé des gisements corrélés, que l'on peut traiter comme des
   mesures indirectes.

2. Les mesures gyroscopiques sont considérées comme des mesures
   directes, indépendantes entre elles. Chaque mesure gyroscopique donne
   lieu à une condition: les gisements directs et indirectes doivent
   être égaux.

A partir des gisements, on calcule l'écart transversal par propagation.
Pour cette préanalyse, on ne dispose pas de mesures, mais seulement de
leur disposition (polygonale tendue, côtés égaux) et de leur écart-type a
priori. En appliquant la propagation de variance, on obtient l'écart-type
de l'écart transversal. Le procédé est identique, quelle que soit la
corrélation entre les gisements.

Si on applique la propagation de variance après compensation, l'opération
est identique, mais avec d'autres variances et covariances pour les
gisements (matrice N*N). A ce stade, on bénéficie de l'apport des mesures
gyroscopiques.

En faisant varier la densité des mesures gyroscopiques, donc en modifiant
n, on peut chercher l'optimum entre le coût et la précision. C'est tout
l'intérêt d'une préanalyse!

Exemple
Pour une polygonale à 50 côtés avec un azimut gyroscopique mesuré après
chaque section de 5 côtés, on peut poser 9 conditions (6e, 11e, ..., 46e
côté). Dans ce cas, le problème consiste à appliquer 9 conditions à 59
observations, dont 50 sont corrélées entre elles (les gisements
indirects).

ESO - TOPO

-------------------------------------------------------------------------
'''

import numpy as np
import matplotlib.pyplot as plt

# données

# N = int(input("nombre de côtés de la polygonale tendue: "))
N = 49

# s = int(input("longueur d'un côté en [m]: "))
s = 400.0 # [m]

# sigmalfa = float(input("écart-type d'une mesure d'angle en [mgon]: "))
sigmalfa = 0.2 # [mgon]

r = 0

# n = int(input("nombre de côtés par section, entre deux mesures gyroscopiques: "))
n = 25.0 # Np. optimale ??? 

if n>0:

	r = int(np.floor(N/n-0.1))
	
	if r>0:
	   # sigmagyro = float(input("écart-type d'une mesure d'azimut en [mgon]: "))
		sigmagyro = 1.0 #[mgon]

'''
-------------------------------------------------------------------------

D'abord on calcule le cheminement sans mesures gyroscopiques. Donc il
s'agit d'une polygonale lancée. On réalise une propagation de variance,
et non une compensation. On en tire les valeurs maximales des écarts-types
pour fixer l'échelle des diagrammes. Ainsi, l'effet des mesures
gyroscopiques sera facile à visualiser.

Chaque gisement dépend de la somme des mesures angulaires précédentes.
Il faut construire la matrice de propagation F telle que: dfi=F*dalfa.
C'est une matrice N*N triangulaire inférieure avec des 1.

'''

F = np.ones((N,N))
F = np.tril(F)

# matrice de covariance des gisements de la polygonale lancée
# implicitement: Qalfa = np.identity(N)

Kfi = sigmalfa**2 * F @ F.T # en mgon^2

# extraction des écarts-types des gisements et de leurs corrélations

from covmat2cormat import covmat2cormat 

[sigmafi,Rfi] = covmat2cormat(Kfi)

np.set_printoptions(precision=1, suppress=True) # 1 décimale = 1/10 mgon
print("\n","sigmafi = ","\n",sigmafi) # en mgon
   
# Ces éléments diagonaux feront l'objet d'un graphique.

np.set_printoptions(precision=2, suppress=True) # 2 décimales ≈ pourcents
print("\n","Rfi = ","\n",Rfi) # sans dimension

# Les éléments hors diagonale montrent que tous les gisements sont corrélés,
# de façon décroissante avec l'éloignement.

#-------------------------------------------------------

# Propagation des erreurs de gisements sur l'erreur transversale le long du
# cheminement. On choisit un repère local tel que l'axe des x soit
# parallèle au cheminement, ainsi y est transversal.

# Pour l'écart transversal, l'arc doit être calculé avec un angle en radians.
# Selon la théorie, il faut convertir Kfi en rad^2.

gonrad = np.pi/200.0 # facteur pour convertir des gons en radians

# Avec s en m, on obtient la matrice de covariance en m^2.
# Kyy = s**2 * F @ ((gonrad/1000)**2 * Kfi) @ F.T # en m^2
# Toutefois Kyy en mm^2 est plus facile à interpréter.
Kyy = s**2 * F @ (gonrad**2 * Kfi) @ F.T # en mm^2

[sigmay,Ryy] = covmat2cormat(Kyy)

np.set_printoptions(precision=1, suppress=True) # 1 décimale = 1/10 mm
print("\n","sigmay = ","\n",sigmay) # en mm

np.set_printoptions(precision=2, suppress=True) # 2 décimales ≈ pourcents
print("\n","Ryy = ","\n",Ryy) # sans dimension

#-------------------------------------------------------

# Les lignes suivantes construisent un dessin. La syntaxe Python ne
# fait pas partie du cours "Méthodes d'estimation". Si nécessaire,
# consultez la documentation. Surtout: conservez ce code pour l'adapter à
# d'autres usages dans d'autres cours!

x = np.arange(0,(N+1)*s/1000.0,s/1000.0)
y = np.append(0,sigmafi)
yerr = np.append(0,sigmay)
y_0  = np.zeros(N+1)

plt.subplot(2, 1, 1)
plt.plot(x,y,'r',label='sans gyroscope')
plt.xlabel('')
plt.ylabel('gisement [mgon]')
plt.title('écarts-types')
plt.legend()

plt.subplot(2, 1, 2)
plt.errorbar(x,y_0,yerr=yerr,fmt='r',ecolor='r',label='sans gyroscope')
plt.xlabel('longueur [km]')
plt.ylabel('écart transversal [mm]')

plt.legend()
plt.show()

#-------------------------------------------------------

# en cas de mesures gyroscopiques

if r>0:

	# Aux observations indirectes des gisements, il faut ajouter les
	# observations directes (via les azimuts gyroscopiques). Le vecteur des
	# observations passe de N à N+r composantes, et l'on construit une matrice
	# auxiliaire Kll de dimension (N+r)*(N+r).

	Kll = np.zeros((N+r,N+r))
	Kll[0:N,0:N] = Kfi
	Kll[N:N+r,N:N+r] = sigmagyro**2 * np.eye(r)

	# Expression des conditions, une par mesure gyroscopique. Ces conditions
	# sont linéaires, on peut construire directement B de dimension r*(N+r).

	B = np.zeros((r,N+r))

	n_int = int(np.floor(n))
	for i in range(0,r):
		B[i,n_int*(i+1)-1] = 1.0
		B[i,N+i] = -1.0
		
	# matrice de covariance des gisements compensés

	Aux = Kll @ B.T
	Klcomp = Kll - Aux @ np.linalg.inv(B @ Aux) @ Aux.T

	# extraction des écarts-types des gisements gyroscopiques,
	# ainsi que leurs corrélations

	[sigmafigyro,Rfigyro] = covmat2cormat(Klcomp[N:N+r,N:N+r])

	np.set_printoptions(precision=1, suppress=True) # 1 décimale = 1/10 mgon
	print("\n","sigmafigyro = ","\n",sigmafigyro) # en mgon
    
	np.set_printoptions(precision=2, suppress=True) # 2 décimales ≈ pourcents
	print("\n","Rfigyro = ","\n",Rfigyro) # sans dimension

    #-------------------------------------------------------

	# Après compensation, les azimuts gyro sont égaux aux gisements propagés
	# correspondants et leurs variances aussi. Donc toute l'information
	# nécessaire est contenue dans la partie supérieure gauche de la matrice.

	Kficomp = Klcomp[0:N,0:N]   # en mgon^2

	# extraction des écarts-types des gisements et de leurs corrélations

	[sigmaficomp,Rficomp] = covmat2cormat(Kficomp)

	np.set_printoptions(precision=1, suppress=True) # 1 décimale = 1/10 mgon
	print("\n","sigmaficomp = ","\n",sigmaficomp) # en mgon

	np.set_printoptions(precision=2, suppress=True) # 2 décimales ≈ pourcents
	print("\n","Rficomp = ","\n",Rficomp) # sans dimension

	#-------------------------------------------------------

	# propagation des erreurs des gisements compensés sur l'erreur transversale
	# le long du cheminement

	Kycomp = s**2 * F @ (gonrad**2 * Kficomp) @ F.T # en mm^2

	[sigmaycomp,Rycomp] = covmat2cormat(Kycomp)

	np.set_printoptions(precision=1, suppress=True) # 1 décimale = 1/10 mm
	print("\n","sigmaycomp = ","\n",sigmaycomp) # en mm

	np.set_printoptions(precision=2, suppress=True) # 2 décimales ≈ pourcents
	print("\n","Rycomp = ","\n",Rycomp)

	#-------------------------------------------------------

	# compléments des graphiques et remplacement du titre

	y_gyro = np.append(0,sigmaficomp)
	yerr_gyro = np.append(0,sigmaycomp)

	plt.subplot(2, 1, 1)
	plt.plot(x,y,'r',label='sans gyroscope')
	plt.plot(x,y_gyro,'b',label='avec gyroscope')
	plt.xlabel('')
	plt.ylabel('gisement [mgon]')
	plt.title('écarts-types')
	plt.legend(loc='upper left')

	plt.subplot(2, 1, 2)
	plt.errorbar(x,y_0,yerr=yerr,fmt='r',ecolor='r',label='sans gyroscope')
	plt.errorbar(x,y_0,yerr=yerr_gyro,fmt='b',ecolor='b',label='avec gyroscope')
	plt.xlabel('longueur [km]')
	plt.ylabel('écart transversal [mm]')
	plt.legend(loc='upper left')
	plt.show()

# et voilà le travail!