// Segundo Filtro de Kalman (um pouco melhor) #region FiltroKalman2(double valor) private double FiltroKalman2(double valor) { double retorno; Kalman1D k = new Kalman1D(); k.Reset( (double)0.1, (double)0.1, (double)0.1, (double)400, 0); // Assume we get to see every other measurement we calculated, and use // the others as the points to compare for estimates. // Run the filter, note our time unit is 1. double[] kalman = new double[data_array_Y.Length]; double[] vel = new double[data_array_Y.Length]; double[] kGain = new double[data_array_Y.Length]; for (int i = 0; i < data_array_Y.Length; i += 2) { if (i == 0) { kalman[0] = 0; vel[0] = k.Velocity; kGain[0] = k.LastGain; kalman[1] = k.Predicition(1); vel[1] = k.Velocity; kGain[1] = k.LastGain; } else { kalman[i] = k.Update(data_array_Y[i], 1); kalman[i + 1] = kalman[i]; vel[i] = k.Velocity; kGain[i] = k.LastGain; vel[i + 1] = vel[i]; kGain[i + 1] = kGain[i]; // kalman[i + 1] = k.Predicition(1); } } retorno = kalman[kalman.Length - 1]; return(retorno); }
private double FiltroKalman2(double valor) { double retorno; Kalman1D k = new Kalman1D(); k.Reset( (double)0.1, (double)0.1, (double)0.1, (double)400, 0); // Assume we get to see every other measurement we calculated, and use // the others as the points to compare for estimates. // Run the filter, note our time unit is 1. double[] kalman = new double[data_array_Y.Length]; double[] vel = new double[data_array_Y.Length]; double[] kGain = new double[data_array_Y.Length]; for (int i = 0; i < data_array_Y.Length; i += 2) { if (i == 0) { kalman[0] = 0; vel[0] = k.Velocity; kGain[0] = k.LastGain; kalman[1] = k.Predicition(1); vel[1] = k.Velocity; kGain[1] = k.LastGain; } else { kalman[i] = k.Update(data_array_Y[i], 1); kalman[i + 1] = kalman[i]; vel[i] = k.Velocity; kGain[i] = k.LastGain; vel[i + 1] = vel[i]; kGain[i + 1] = kGain[i]; // kalman[i + 1] = k.Predicition(1); } } retorno = kalman[kalman.Length - 1]; return retorno; }