<jupyter_start><jupyter_text>Extended Kalman Filter algorithmLoan Sarazin & <NAME><jupyter_code>import numpy as np
import numpy.linalg as la
from math import *
import matplotlib.pyplot as plt

import pandas as pd
#Importation des données 
data = pd.read_excel("donnee.xlsx")
print(data.head())
signalReel = np.array(data.loc[:, ('signalReel')])
signalBruite = np.array(data.loc[:, ('signalBruite')])

Temps = np.array(data.loc[:, ('Temps')])
N = signalReel.shape[0]
X = Temps

plt.figure(figsize = (15, 8))
plt.plot(X, signalBruite, '.', label = "Noisy signal")
plt.plot(X, signalReel, '.', label = "Real signal")
plt.legend()
plt.show()
#restriction au 300 premières valeurs
plt.figure(figsize = (15, 8))
plt.plot(X[:300], signalBruite[:300], label = "Noisy signal", linestyle = "dashed")
plt.plot(X[:300], signalReel[:300], label = "Real signal")
plt.legend()
plt.xlabel("Time")
plt.ylabel("Signal values")
plt.title("Plot of both the noisy signal and the real signal through the time evolution")

plt.show()
#Implémentation du filtre de Kalman étendu 

def KalmanF_extended(Z, Q, R, A):
    #Initialisation de x0 et P00
    x0 = np.random.normal(size = 2)
    P00 = np.identity(2)
    
    k=0
    x_evol = []
    
    nu0 = 12
    Te = 1/193.28
    
    x = x0
    P = P00
    N = Z.shape[0]
    
    #Boucle d'estimation/prediction
    while (k < N ):
        new_x = x.copy().reshape((2, 1))
    
        newP =  P + Q
        
        H = np.array([sin(2*pi*nu0*Te*(k+1) + new_x[1, 0]), 
                      new_x[0, 0]*cos(2*pi*nu0*Te*(k+1) + new_x[1, 0])]).reshape((2, 1))
        S = np.array(H.T@newP@H + R)
        K = (newP@H /S).reshape((2, 1))
        
        epsilon = Z[k] - new_x[0]*sin(2*pi*nu0*(k+1)*Te + new_x[1])
        x = new_x + epsilon*K
        P = newP - K@H.T@newP
        
        x_evol.append(x)
        k += 1
    return np.array(x_evol).reshape((len(x_evol), 2))
Q = np.array([2*10**(-5), 0, 0, 2*10**(-1)])
Q = Q.reshape((2, 2))

R = 3

A = np.eye(2)

Q, R, A

x_estim = KalmanF_extended(signalBruite, Q, R, A)
plt.figure(figsize = (8, 5))
iter = np.arange(0, 20000)
plt.plot(iter, x_estim[:, 0], 'b.')
plt.xlabel("Iterations of the EKF algorithm")
plt.ylabel("Value of alpha")
plt.title("Plot of amplitude alpha along the iterations")
plt.show()
plt.figure(figsize = (8, 5))
iter = np.arange(0, 20000)
plt.plot(iter, x_estim[:, 1], 'b.')
plt.xlabel("Iterations of the EKF algorithm")
plt.ylabel("Value of $\phi$")
plt.title("Plot of the phase $\phi$ along the iterations")
plt.show()
nu0 = 12
Te = 1/193.28

nmin = 19850
nmax = nmin + 150

plt.figure(figsize = (10, 8))
iter = np.arange(nmin, nmax)
plt.plot(iter, x_estim[nmin:nmax, 0]*np.sin(2*pi*nu0*(iter)*Te + x_estim[nmin:nmax, 1]), label = "Estimated signal")
plt.plot(iter, signalReel[nmin:nmax], label = "Real signal")
plt.plot(iter, signalBruite[nmin:nmax], 'r.', label = "Noised signal")
plt.xlabel("Time")
plt.ylabel("Value of the signal")
plt.title("Real and estimated signals (19 000, 20 000)")
plt.legend()
plt.show()<jupyter_output><empty_output><jupyter_text>We can see that we have a quite accurate estimation of the signal. The amplitude alpha is converging to a value close to 5 and the phase $\phi$ is also close to the real phase. Note that the plot is for the last periods, we can see below that the estimated signal is less accurate at the beginning of the time period.<jupyter_code>nmin = 0
nmax = nmin + 150

plt.figure(figsize = (10, 8))
iter = np.arange(nmin, nmax)
plt.plot(iter, x_estim[nmin:nmax, 0]*np.sin(2*pi*nu0*(iter)*Te + x_estim[nmin:nmax, 1]), label = "Estimated signal")
plt.plot(iter, signalReel[nmin:nmax], label = "Real signal")
plt.plot(iter, signalBruite[nmin:nmax], 'r.', label = "Noised signal")
plt.xlabel("Time")
plt.ylabel("Value of the signal")
plt.title("Real and estimated signals (0, 300)")
plt.legend()
plt.show()<jupyter_output><empty_output>
StarcoderdataJupyterStructuredCleanDedup