# lecture fichier CSV
import pandas as p
exp1 = p.read_csv("Experience2.csv")
X_m = exp1['X_mesure'].tolist()
Y_m = exp1['Y_mesure'].tolist()
V = exp1['Vitesse_mesure'].tolist()

#parametres
deltaT = 0.1
sigmaAcc = 0.1

#matrices caractéristiques
import numpy as np
F = np.array([[1, 0, deltaT, 0], [0, 1, 0, deltaT], [0, 0, 1, 0], [0, 0, 0, 1]])
Q = np.array([[deltaT*deltaT*deltaT*deltaT/4, 0, deltaT*deltaT*deltaT/2, 0],
     [0, deltaT*deltaT*deltaT*deltaT/4, 0, deltaT*deltaT*deltaT/2],
     [deltaT*deltaT*deltaT/2, 0, deltaT*deltaT, 0],
     [0, deltaT*deltaT*deltaT/2, 0, deltaT*deltaT]])*sigmaAcc
R = np.diag([.8, .6, .3])

#iniialisation
StateKalman = np.zeros((4,len(V)))
StateKalman[:,0] = np.array([X_m[0], Y_m[0], 0.1, 0.1])#valeurs aleatoires pour Dx et Dy initiaux
CovKalman = np.zeros((4,4,len(V)))
CovKalman[:,:, 0] = np.diag([.1,.1,.1,.1])


Y = np.zeros((3,len(V)))
XKal = np.zeros(len(V))
YKal = np.zeros(len(V))
XKal[0] = StateKalman[0, 0]
YKal[0] = StateKalman[1, 0]

#estimation avec filtre de Kalman
for i in range(1, len(V)):
    #prediction 
    StateKalman[:, i] = F @ StateKalman[:, i-1];
    CovKalman[:,:, i] = F @ CovKalman[:,:, i-1] @ np.transpose(F) + Q
    #mesure
    Y[:, i] = [X_m[i], Y_m[i], V[i]]
    #mise a jour
    import math
    V[i] = math.sqrt(StateKalman[2, i]*StateKalman[2, i] + StateKalman[3, i]*StateKalman[3, i]);
    resY = Y[:, i] - np.array([StateKalman[0, i], StateKalman[1, i], V[i]])
    H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, StateKalman[2, i]/V[i], StateKalman[3, i]/V[i]]]);
    S = H @ CovKalman[:, :, i] @ np.transpose(H) + R
    K = CovKalman[:, :, i] @ np.transpose(H) @ np.linalg.inv(S) #attention: nul si CovKalman=0 
    StateKalman[:, i] = StateKalman[:, i] + K @ resY
    CovKalman[:,:, i] = (np.identity(4) - K@H) @ CovKalman[:,:, i]
    XKal[i] = StateKalman[0, i]
    YKal[i] = StateKalman[1, i]
    
#affichage avec filtre de Kalman
import matplotlib.pyplot as plt
plt.plot(XKal, YKal)
plt.show()