public void Predict(Vector <double> control, StateTransitionFunc stateTransition, StateTransitionJacobianFunc stateTransitionJacobian, Matrix <double> noise)
{
var jacobian = stateTransitionJacobian(control, State);
State = stateTransition(control, State);
Uncertainty = jacobian * Uncertainty * jacobian.Transpose() + noise;
}
public void Predict(Vector <double> control, StateTransitionFunc stateTransition, StateTransitionJacobianFunc stateTransitionJacobian, Matrix <double> noise)
{
var jacobian = stateTransitionJacobian(control, Mean);
Mean = stateTransition(control, Mean);
Covariance = jacobian * Covariance * jacobian.Transpose() + noise;
}
