public void Update(Vector <double> measurement, MeasurementFunc measurementFunction, MeasurementJacobianFunc measurementFunctionJacobian, Matrix <double> noise)
{
var jacobian = measurementFunctionJacobian(State);
var kalmanGain = Uncertainty * jacobian.Transpose() * (jacobian * Uncertainty * jacobian.Transpose() + noise).Inverse();
State = State + kalmanGain * (measurement - measurementFunction(State));
Uncertainty = (DenseMatrix.CreateIdentity(State.Count) - kalmanGain * jacobian) * Uncertainty;
}
public void Update(Vector <double> measurement, MeasurementFunc measurementFunction, MeasurementJacobianFunc measurementFunctionJacobian, Matrix <double> noise)
{
var jacobian = measurementFunctionJacobian(Mean);
var kalmanGain = Covariance * jacobian.Transpose() * (jacobian * Covariance * jacobian.Transpose() + noise).Inverse();
Mean = Mean + kalmanGain * (measurement - measurementFunction(Mean));
Covariance = (DenseMatrix.Identity(Mean.Count) - kalmanGain * jacobian) * Covariance;
}