/// <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 **********/ }
private double[] CalculateDelta(double[] measurement) { //innovation vector (measurement error) var stateMeasurement = MeasurementMatrix.Dot(state); var measurementError = measurement.Subtract(stateMeasurement); return(measurementError); }
/// <summary> /// Calculates Mahalanobis distance between the provided measurement and the predicted state. /// <para>Covariance matrix is the <see cref="ResidualCovariance"/>.</para> /// </summary> /// <param name="measurement">Measurement.</param> /// <param name="delta">The residual from the measurement and the current state.</param> /// <returns>Mahalanobis distance.</returns> public double CalculateMahalanobisDistance(TMeasurement measurement, out double[] delta) { CheckPrerequisites(); var m = measurementConvertFunc(measurement); var stateMeasurement = MeasurementMatrix.Dot(state); delta = m.Subtract(stateMeasurement); var distance = Distance.Mahalanobis(m, stateMeasurement, ResidualCovarianceInv); return(distance); }
/// <summary> /// Checks pre-conditions: matrix sizes. /// </summary> private void CheckPrerequisites() { /************************** TRANSITION MATRIX ***************************/ if (TransitionMatrix is null) { throw new Exception("Transition matrix cannot be null!"); } if (TransitionMatrix.GetLength(0) != StateVectorDimension || TransitionMatrix.GetLength(1) != StateVectorDimension) { throw new Exception("Transition matrix dimensions are not valid!"); } /************************** TRANSITION MATRIX ***************************/ /************************** CONTROL MATRIX ***************************/ if (ControlMatrix is null && ControlVectorDimension != 0) { throw new Exception("Control matrix can be null only if control vector dimension is set to 0!"); } if (ControlMatrix is not null && (ControlMatrix.GetLength(0) != StateVectorDimension || ControlMatrix.GetLength(1) != ControlVectorDimension)) { throw new Exception("Control matrix dimensions are not valid!"); } /************************** CONTROL MATRIX ***************************/ /************************** MEASUREMENT MATRIX ***************************/ if (MeasurementMatrix is null) { throw new Exception("Measurement matrix cannot be null!"); } if (MeasurementMatrix.GetLength(0) != MeasurementVectorDimension || MeasurementMatrix.GetLength(1) != StateVectorDimension) { throw new Exception("Measurement matrix dimesnions are not valid!"); } /************************** MEASUREMENT MATRIX ***************************/ /************************** PROCES NOISE COV. MATRIX ***************************/ if (ProcessNoise is null) { throw new Exception("Process noise covariance matrix cannot be null!"); } if (ProcessNoise.GetLength(0) != StateVectorDimension || ProcessNoise.GetLength(1) != StateVectorDimension) { throw new Exception("Process noise covariance matrix dimensions are not valid!"); } /************************** PROCES NOISE COV. MATRIX ***************************/ /************************** MEASUREMENT NOISE COV. MATRIX ***************************/ if (MeasurementNoise is null) { throw new Exception("Measurement noise covariance matrix cannot be null!"); } if (MeasurementNoise.GetLength(0) != MeasurementVectorDimension || MeasurementNoise.GetLength(1) != MeasurementVectorDimension) { throw new Exception("Measurement noise covariance matrix dimensions are not valid!"); } /************************** MEASUREMENT NOISE COV. MATRIX ***************************/ }