# -*- coding: utf-8 -*-
"""
Méthodes d'estimation
SIE 3e semestre, GC 5e semestre

ESO / modif Nov 26 2023

"""

import numpy as np
np.set_printoptions(precision=4, suppress=True)

print(' ')
print('------------------------------------------------------')
print("Compensation paramétrique des angles d'un quadrilatère")
print('------------------------------------------------------')

# Les angles alpha et beta sont mesurés 2 fois, gamma et theta une seule
# fois. Trois résolutions équivalentes sont proposées.

# Effacer toutes les variables préalablement stockées.


# Introduire les mesures directement dans le code.

ang = np.array([115.960, 115.940, 105.280, 105.286, 90.473, 88.282])
n = len(ang)

print("ang =")
print(ang)

print(' ')
print('1. Approche présentée en auditoire')
print(' ')
print('   x=[beta      xbulle=[beta1')
print('      gamma             gamma')
print('      theta]            theta]')
print(' ')

# Choisir les paramètres.
# On le fait en fixant leurs valeurs approchées. 
# Ici, on prend la valeur mesurée, la 1e s'il y en a plusieurs.

beta  = ang[2]
gamma = ang[4]
theta = ang[5]

# vecteur des paramètres approchés
# Pour x et pour v, on peut gagner de la place en ne distinguant pas
# "bulle" et "comp". Le contexte et les commentaires permettent d'éviter
# toute confusion.

x = np.array([beta, gamma, theta])

print("x =")
print(x)
print(' ')

u = len(x)
r = n-u

print("r =")
print(r)
print(' ')

# résidus approchés
v = ang - np.array([400-beta-gamma-theta,
           400-beta-gamma-theta,
           beta,
           beta,
           gamma,
           theta])

print("v =")
print(v)
print(' ')

# matrice des dérivées partielles
A = np.array([[-1, -1, -1],
     [-1, -1, -1],
     [1,  0, 0],
     [1,  0,  0],
     [0,  1,  0],
     [0,  0,  1]])

print("A =")
print(A)
print(' ')

# modèle stochastique des observations
Q = np.eye(n)
P = np.linalg.inv(Q)

# cofacteurs des paramètres compensés
Qxx = np.linalg.inv(A.transpose() @ P @ A)
print("Qxx =")
print(Qxx)
print(' ')

# incréments compensés
dx = Qxx @ A.transpose() @ P @ v
print("dx =")
print(dx)
print(' ')

# paramètres compensés
x = x + dx
print("x =")
print(x)
print(' ')

# résidus compensés
v = v - A @ dx
print("v =")
print(v)
print(' ')

# observations compensées
angcomp = ang - v
print("angcomp =")
print(angcomp)
print(' ')

print("> Ouf! Les mesures compensées d'un même angle sont égales!")
print(' ')

# écart-type a posteriori d'une observation de poids unitaire
sigmapos = np.sqrt(v.transpose() @ P @ v / r)
print("sigmapos = %.4f" % sigmapos)


print(' ')
print('-----------------------------------------------------')
print('2. Autres paramètres, "meilleures" valeurs approchées')
print(' ')
print('   x=[alpha     xbulle=[(alpha1+alpha2)/2')
print('      beta              ( beta1+ beta2)/2')
print('      gamma]                 gamma       ]')
print(' ')

alpha = (ang[0]+ang[1])/2;
beta  = (ang[2]+ang[3])/2;
gamma = ang[4];

x = np.array([alpha, beta, gamma])
print("x =")
print(x)
print(' ')

u = len(x)
r = n-u

v = ang - np.array([alpha,
           alpha,
           beta,
           beta,
           gamma,
           400-alpha-beta-gamma])

print("v =")
print(v)
print(' ')
 
A = np.array([ [1,  0,  0],
      [1,  0,  0],
      [0,  1,  0],
      [0,  1,  0],
      [0,  0,  1],
     [-1, -1, -1]])

print("A =")
print(A)
print(' ')

Qxx = np.linalg.inv(A.transpose() @ P @ A)
print("Qxx =")
print(Qxx)
print(' ')

dx = Qxx @ A.transpose() @ P @ v
print("dx =")
print(dx)
print(' ')

x = x + dx
print("x =")
print(x)
print(' ')

v = v - A @ dx
print("v =")
print(v)
print(' ')

angcomp = ang - v
print("angcomp =")
print(angcomp)
print(' ')

print('> aucun changement!')

print(' ')
print('----------------------------------------------')
print('3. Mêmes paramètres, valeurs approchées nulles')
print(' ')
print('   x=[alpha     xbulle=[0')
print('      beta              0')
print('      gamma]            0]')
print(' ')

alpha = 0
beta = 0
gamma = 0

x = np.array([alpha, beta, gamma])
print("x =")
print(x)
print(' ')

u = len(x)
r=n-u

v = ang - np.array([alpha,
           alpha,
           beta,
           beta,
           gamma,
           400-alpha-beta-gamma])

print("v =")
print(v)
print(' ')

A = np.array([ [1,  0,  0],
      [1,  0,  0],
      [0,  1,  0],
      [0,  1,  0],
      [0,  0,  1],
     [-1, -1, -1]])

print("A =")
print(A)
print(' ')

Qxx = np.linalg.inv(A.transpose() @ P @ A)
print("Qxx =")
print(Qxx)
print(' ')

dx = Qxx @ A.transpose() @ P @ v
print("dx =")
print(dx)
print(' ')

x = x + dx
print("x =")
print(x)
print(' ')

v = v - A @ dx
print("v =")
print(v)
print(' ')

angcomp = ang - v
print("angcomp =")
print(angcomp)
print(' ')

print('> aucun changement!')

# Dans ce cas, le modèle fonctionnel est linéaire
# et peu importent les valeurs approchées.
# La convergence est immédiate, sans itération!

# En fait, pour un modèle linéaire, on n'a pas besoin de valeurs
# approchées.

print(' ')
print('----------------------------------------------')
print('4. Mêmes paramètres, pas de valeurs approchées')
print(' ')
print('   x=[alpha')
print('      beta')
print('      gamma]')
print(' ')

# On désigne les termes constants des équations (modèles des
# observations).
a0 = np.array([0, 0, 0, 0, 0, 400])

# calcul direct des paramètres, sans passer par les incréments
Qxx = np.linalg.inv(A.transpose() @ P @ A)
print("Qxx =")
print(Qxx)
print(' ')

x = Qxx @ A.transpose() @ P @ (ang-a0)
print("x =")
print(x)
print(' ')

# calcul direct des résidus compensés
v = (ang-a0) - A @ x
print("v =")
print(v)
print(' ')

# observations compensées comme avant
angcomp = ang - v
print("angcomp =")
print(angcomp)
print(' ')

print("> On s'amuse à la cuisine, mais rien ne change à table!")