/// <summary> /// Predicts the next state using the current state and <paramref name="controlVector"/>. /// </summary> /// <param name="controlVector">Set of data for external system control.</param> /// <summary> /// <para>Estimates the subsequent model state.</para> /// <para> /// x'(k) = A * x(k-1) + B * u(k). /// P'(k) = A * P(k-1) * At + Q /// K(k) = P'(k) * Ht * (H * P'(k) * Ht + R)^(-1) /// </para> /// </summary> public void Predict(double[] controlVector) { CheckPrerequisites(); //x'(k) = A * x(k-1) //this.state = this.state.Multiply(this.TransitionMatrix); state = TransitionMatrix.Dot(state); //x'(k) = x'(k) + B * u(k) if (controlVector is not null) { state = state.Add(ControlMatrix.Dot(controlVector)); } //P'(k) = A * P(k-1) * At + Q EstimateCovariance = TransitionMatrix.Multiply(EstimateCovariance).Multiply(TransitionMatrix.Transpose()).Add(ProcessNoise); /******* calculate Kalman gain **********/ var measurementMatrixTransponsed = MeasurementMatrix.Transpose(); //S(k) = H * P'(k) * Ht + R ResidualCovariance = MeasurementMatrix.Multiply(EstimateCovariance).Multiply(measurementMatrixTransponsed).Add(MeasurementNoise); ResidualCovarianceInv = ResidualCovariance.Inverse(); //K(k) = P'(k) * Ht * S(k)^(-1) KalmanGain = EstimateCovariance.Multiply(measurementMatrixTransponsed).Multiply(ResidualCovarianceInv); /******* calculate Kalman gain **********/ }