public float EstimateHeight(Vector3 position, Vector3 lastPosition) { if (!enableFootHeight) { transform.position = position; return(0); } // TODO: Get rid of this hack! position.y = lastPosition.y = 0; // get speed and clamp it to max speed speed = Mathf.Clamp((position - lastPosition).magnitude / Time.fixedDeltaTime, 0, maxSpeed); // scale speed to 0 - 1 speed01 = speed / maxSpeed; // apply filter speed01 = (float)kalman.UseFilter(speed01);// CalcKalman(speed01); // take height from curve height = curve.Evaluate(speed01); // apply height transform.position = new Vector3(position.x, height, position.z); return(height); }
private void FootAndHipHeight() { // get heights leftFootPosition.y = leftFoot.EstimateHeight(leftFootPosition, lastLeftFootPosition); rightFootPosition.y = rightFoot.EstimateHeight(rightFootPosition, lastRightFootPosition); distanceBetweenFeet = Vector3.Distance(leftFootPosition, rightFootPosition); centerPosition.y = -(float)hipHeightFilter.UseFilter(Mathf.Clamp(((distanceBetweenFeet - 0.4f) * 0.5f), 0.0f, 1.0f) - 0.02f); currenHipHeight = centerPosition.y; }
private void UseSimpleKalman() { // get speed and clamp it to max speed speed = Mathf.Clamp((transform.position - lastPosition).magnitude / Time.deltaTime, 0, maxSpeed); // store last foot position lastPosition = transform.position; // scale speed to 0 - 1 speed01 = speed / maxSpeed; // apply filter speed01 = (float)kalman.UseFilter(speed01);// CalcKalman(speed01); // take height from curve height = curve.Evaluate(speed01); // apply height transform.position = new Vector3(transform.position.x, height, transform.position.z); }
//private void FootFiltering() { // if (!applyFilterOnFeet) // return; // foreach (SimpleKalman item in kalmanFilters) { // item.Q = kalmanQ; // item.R = kalmanR; // } // leftFootPosition = new Vector3((float)kalmanFilters[0].UseFilter(leftFootPosition.x), leftFootPosition.y, (float)kalmanFilters[1].UseFilter(leftFootPosition.z)); // rightFootPosition = new Vector3((float)kalmanFilters[2].UseFilter(rightFootPosition.x), rightFootPosition.y, (float)kalmanFilters[3].UseFilter(rightFootPosition.z)); //} //private void OrientationFiltering() { // Vector3 tempFast = (centerPosition - lastCenterPosition); // fastOrientation = new Vector2(tempFast.x, tempFast.z).normalized; // //fastOrientation = (Orientation + fastOrientation) / 2; // fastOrientation = new Vector2((float)orientationFilter[0].UseFilter(fastOrientation.x), (float)orientationFilter[1].UseFilter(fastOrientation.y)); //} private void FootAndHipHeight() { distanceBetweenFeet = Vector3.Distance(leftFootPosition, rightFootPosition); centerPosition.y = -(float)hipHeightFilter.UseFilter(Mathf.Clamp(((distanceBetweenFeet - 0.5f) * 0.5f), 0.0f, 1.0f) - 0.0f); }