code:
#IF JAVA
public class KalmanFilter {
private double x_hat; // Stato stimato
private double P_hat; // Matrice di varianza dello stato stimato
private double Q; // Matrice di rumore del processo
private double R; // Matrice di rumore del misuratore
public KalmanFilter(double x0, double P0, double Q, double R) {
x_hat = x0;
P_hat = P0;
this.Q = Q;
this.R = R;
}
public double update(double z) {
// Calcola la predizione
double x_hat_pred = x_hat;
double P_hat_pred = P_hat + Q;
// Calcola il guadagno del filtro
double K = P_hat_pred * R / (P_hat_pred * R + Q);
// Aggiorna lo stato
x_hat = x_hat_pred + K * (z - x_hat_pred);
// Aggiorna la matrice di varianza dello stato
P_hat = (1 - K) * P_hat_pred;
return x_hat;
}
public double filterrssi (double rssi) {
// Crea un filtro Kalman
KalmanFilter filter = new KalmanFilter(0, 1, 0.5d,0.125d);
// Applica il filtro al valore RSSI
double x = filter.update(rssi);
// Stampa la stima dello stato corrente
return x;
}
}
#END IF