private void MeasurementUpdateCorrection(Vector3 data)
{
float val = (FACTOR_MEASURED_VALUE_TO_PREVIOUS_VALUE * this.predictedCovariance) / (((FACTOR_MEASURED_VALUE_TO_PREVIOUS_VALUE * this.predictedCovariance) * FACTOR_MEASURED_VALUE_TO_PREVIOUS_VALUE) + ENVIRONMENT_NOISE);
if (!PhysicsUtil.IsValidFloat(val))
{
this.Reset(this.State);
}
else
{
this.State = this.predictedState + (val * (data - (FACTOR_MEASURED_VALUE_TO_PREVIOUS_VALUE * this.predictedState)));
this.covariance = (1f - (val * FACTOR_MEASURED_VALUE_TO_PREVIOUS_VALUE)) * this.predictedCovariance;
}
}