/// <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 **********/ }
/// <summary> /// Calculates entropy from the error covariance matrix. /// </summary> public double CalculateEntropy() { if (EstimateCovariance is null || EstimateCovariance.GetLength(0) != EstimateCovariance.GetLength(1)) { throw new ArgumentException("Error covariance matrix (P) must have the same number of rows and columns."); } var stateVectorDim = EstimateCovariance.GetLength(0); var errorCovDet = EstimateCovariance.Determinant(); return(0.5f * ((stateVectorDim * Log(4 * PI)) + Log(errorCovDet))); }
/// <summary> /// Corrects the state error covariance based on innovation vector and Kalman update. /// </summary> /// <param name="innovationVector">The difference between predicted state and measurement.</param> public void Correct(double[] innovationVector) { if (innovationVector is null) { throw new ArgumentNullException(nameof(innovationVector)); } CheckPrerequisites(); if (innovationVector.Length != MeasurementVectorDimension) { throw new Exception("PredicitionError error vector (innovation vector) must have the same length as measurement."); } //correct state using Kalman gain state = state.Add(KalmanGain.Dot(innovationVector)); var identity = Matrix.Identity(StateVectorDimension); EstimateCovariance = (identity.Subtract(KalmanGain.Multiply(MeasurementMatrix))).Multiply(EstimateCovariance.Transpose()); }