Aruco Tag e filtri Kalman

Usando gli Aruco tag in ambiente esterno con illuminazione solare a diverse ore e in diversi periodi dell'anno ho trovato un errore nella stima della distanza tra due tag compresi tra 0.7% e 1.3% della distanza misurata. La domanda e' stata: e' possibile ridurre l'errore ?

Ho provato ad applicare il filtro Kalman per condizioni statiche (le distanze tra i tag nel tempo non sono variate) usando usando il codice a questo indirizzo 

Nel  grafico sottostante i punti blu sono i dati non filtrati, in linea continua blu i dati filtrati, in linea rossa il valore reale della misura di distanza

questo il codice impiegato con modestissime modifiche rispetto a quello di esempio

 

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd



def kalman_1d(x, P, measurement, R_est, Q_est):
    x_pred = x
    P_pred = P + Q_est
    K = P_pred / (P_pred + R_est)
    x_est = x_pred + K * (measurement - x_pred)
    P_est = (1 - K) * P_pred
    return x_est, P_est


def plot_1d_comparison(measurements_made, estimate, true_value, axis):
    axis.plot(measurements_made ,'k+' ,label='measurements' ,alpha=0.3)
    axis.plot(estimate ,'-' ,label='KF estimate')
    if not isinstance(true_value, (list, tuple, np.ndarray)):
        # plot line for a constant value
        axis.axhline(true_value ,color='r' ,label='true value', alpha=0.5)
    else:
        # for a list, tuple or array, plot the points
        axis.plot(true_value ,color='r' ,label='true value', alpha=0.5)
    axis.legend(loc = 'lower right')
    axis.set_title('Estimated position vs. time step')
    axis.set_xlabel('Time')
    axis.set_ylabel('$x_t$')

def plot_1d_error(estimated_error, lower_limit, upper_limit, axis):
    # lower_limit and upper_limit are the lower and upper limits of the vertical axis
    axis.plot(estimated_error, label='KF estimate for $P$')
    axis.legend(loc = 'upper right')
    axis.set_title('Estimated error vs. time step')
    axis.set_xlabel('Time')
    axis.set_ylabel('$P_t$')
    plt.setp(axis ,'ylim' ,[lower_limit, upper_limit])


nome_file = "20_40"
df = pd.read_csv(nome_file+'.csv',header=0,sep=";")

#11 5.38
#15 3.39
#25 7.46
#40 4.26

mu = 4.26 # Actual position
R = 0.1    # Actual standard deviation of actual measurements (R)

# Generate measurements
#n_measurements = 1000 # Change the number of points to see how the convergence changes
#Z = np.random.normal(mu, np.sqrt(R), size=n_measurements)
Z = df[df.columns[2]].to_numpy()
# Estimated covariances
Q_est = 1e-4
R_est = 2e-2

# initial guesses
x = 5.1 # Use an integer (imagine the initial guess is determined with a meter stick)
P = 0.04 # error covariance P

KF_estimate=[] # To store the position estimate at each time point
KF_error=[] # To store estimated error at each time point
for z in Z:
    x, P = kalman_1d(x, P, z, R_est, Q_est)
    KF_estimate.append(x)
    KF_error.append(P)

fig, axes = plt.subplots(1 ,2, figsize=(12, 5))
plot_1d_comparison(Z, KF_estimate, mu, axes[0])
np.savetxt(nome_file+"_stima.csv",KF_estimate)
plot_1d_error(KF_error, 0, 0.015, axes[1])
plt.tight_layout()
plt.savefig(nome_file+'.png')