Предсказание и коррекция Калмана такие же, как начальные значения

Я бы не стал писать ваш фильтр так. Я бы использовал следующий контракт для всех фильтров типа Калмана.

public interface IKalmanFilter
{
    /// <summary>
    /// The current observation vector being used.
    /// </summary>
    Vector<double> Observation { get; }

    /// <summary>
    /// The best estimate of the current state of the system.
    /// </summary>
    Vector<double> State { get; }

    /// <summary>
    /// The covariance of the current state of the filter. Higher covariances
    /// indicate a lower confidence in the state estimate.
    /// </summary>
    Matrix<double> StateVariance { get; }

    /// <summary>
    /// The one-step-ahead forecast error of y given the previous measurement.
    /// </summary>
    Vector<double> ForecastError { get; }

    /// <summary>
    /// The one-step ahead forecast error variance.
    /// </summary>
    Matrix<double> ForecastErrorVariance { get; }

    /// <summary>
    /// The Kalman Gain matrix.
    /// </summary>
    Matrix<double> KalmanGain { get; }

    /// <summary>
    /// Performs a prediction of the next state of the system.
    /// </summary>
    /// <param name="T">The state transition matrix.</param>
    void Predict(Matrix<double> T);

    /// <summary>
    /// Perform a prediction of the next state of the system.
    /// </summary>
    /// <param name="T">The state transition matrix.</param>
    /// <param name="R">The linear equations to describe the effect of the noise
    /// on the system.</param>
    /// <param name="Q">The covariance of the noise acting on the system.</param>
    void Predict(Matrix<double> T, Matrix<double> R, Matrix<double> Q);

    /// <summary>
    /// Updates the state estimate and covariance of the system based on the
    /// given measurement.
    /// </summary>
    /// <param name="y">The measurements of the system.</param>
    /// <param name="T">The state transition matrix.</param>
    /// <param name="Z">Linear equations to describe relationship between
    /// measurements and state variables.</param>
    /// <param name="H">The covariance matrix of the measurements.</param>
    void Update(Vector<double> y, Matrix<double> T, 
        Matrix<double> Z, Matrix<double> H, Matrix<double> Q);
}

Где Vector<T> и Matrix<T> для моей собственной реализации — это значения из MathNet.Numerics. Причина, по которой я показал это, заключается в том, что эта структура позволит вам выполнять сглаживающие рекурсии для отфильтрованных наборов данных и выполнять оценку параметра максимального правдоподобия (если вам это потребуется).

После того, как у вас есть реализация для линейного гауссовского фильтра Калмана с шаблоном выше, вы можете вызвать его для некоторого набора данных в цикле для каждой точки данных во временном ряду (обратите внимание, что цикл не выполняется в коде фильтра). Для одномерного вектора состояния/наблюдения мы могли бы написать:

// Set default initial state and variance.
a = Vector<double>.Build.Dense(1, 0.0d);
P = Matrix<double>.Build.Dense(1, 1, Math.Pow(10, 7));

// Run the filter.
List<double> filteredData = new List<double>();
IKalmanFilter filter = new KalmanFilter(a, P);
for (int i = 0; i < Data.Length; i++)
{
    filter.Predict(T, R, Q);
    Vector<double> v = DenseVector.Create(1, k => Convert.ToDouble(Data[i]));
    filter.Update(v, T, Z, H, Q);

    // Now to get the filtered state values, you use. (0 as it is one-dimensional data)
    filteredData.Add(filter.State[0]);
}

Я знаю, что это не использует ваш код, но я надеюсь, что это поможет. Если вы решите пойти по этому пути, я помогу вам с фактическим кодом фильтра Калмана...