Esempio n. 1
0
    public Vector3 getFilteredAcceleration(out MovementStepData md)
    {
        MovementStepData msd = getRawData();

        md = msd;
        int N = dataQueue.Count;

        if (N == 0)
        {
            N++;
        }
        Vector3 outputVec = new Vector3(msd.linearAccAverage.x, msd.linearAccAverage.y, msd.linearAccAverage.z);

        float diffOfRotAndMovEnergies = 0.4f;

        if ((msd.gyroRotationEnergy - msd.linearAccEnergy) > diffOfRotAndMovEnergies)
        {
            return(Vector3.zero);
        }

        float z = outputVec.z;
        float deltaToDistinguishFrontAndBack = 0.026f;

        if (z > 0)                                  //if moving back
        {
            if (z < deltaToDistinguishFrontAndBack) //not strongly - forbid back movement
            {
                outputVec.z = 0;
            }
            else //strongly - enforce
            {
                outputVec.z *= 1.5f;
            }
        }

        return(outputVec);
    }
Esempio n. 2
0
    public void insertData(GyroControl gc)
    {
        Gyroscope gyro              = gc.gyro;
        Vector3   linearAcc         = gyro.userAcceleration;
        Vector3   gyroRotationSpeed = gyro.rotationRateUnbiased;
        //Vector3 linearAcc = new Vector3(0.1f, 0.1f, 0.2f);
        //Vector3 gyroRotationSpeed = new Vector3(0.1f, 0.1f, 0.2f);
        float linearAccMagnitude         = linearAcc.magnitude;
        float gyroRotationSpeedMagnitude = gyroRotationSpeed.magnitude;

        Vector3 linearAccClearedFromConstant         = new Vector3(0, 0, 0);
        Vector3 gyroRotationSpeedClearedFromConstant = new Vector3(0, 0, 0);
        Vector3 linearAccAverage = new Vector3(0, 0, 0);
        float   linearAccMagnitudeClearedFromConstant         = 0;
        float   gyroRotationSpeedMagnitudeClearedFromConstant = 0;
        float   linearAccEnergy    = 0;
        float   gyroRotationEnergy = 0;

        int N = dataQueue.Count;

        if (N == 0)
        {
            N++;
        }

        foreach (MovementStepData md in dataQueue)
        {
            linearAccClearedFromConstant                  += md.linearAcc;
            linearAccAverage                              += md.linearAcc;
            gyroRotationSpeedClearedFromConstant          += md.gyroRotationSpeed;
            linearAccMagnitudeClearedFromConstant         += md.linearAccMagnitude;
            gyroRotationSpeedMagnitudeClearedFromConstant += md.gyroRotationSpeedMagnitude;
            linearAccEnergy    += md.linearAccMagnitudeClearedFromConstant;
            gyroRotationEnergy += md.gyroRotationSpeedMagnitudeClearedFromConstant;
        }
        linearAccClearedFromConstant                  = linearAcc - 1 / N * linearAccClearedFromConstant;
        linearAccAverage                             /= N;
        gyroRotationSpeedClearedFromConstant          = gyroRotationSpeed - 1 / N * gyroRotationSpeedClearedFromConstant;
        linearAccMagnitudeClearedFromConstant         = linearAccMagnitude - 1 / N * linearAccMagnitudeClearedFromConstant;
        gyroRotationSpeedMagnitudeClearedFromConstant = gyroRotationSpeedMagnitude - 1 / N * gyroRotationSpeedMagnitudeClearedFromConstant;
        linearAccEnergy    /= N;
        gyroRotationEnergy /= N;


        float linearAccVariance    = 0;
        float gyroRotationVariance = 0;

        foreach (MovementStepData md in dataQueue)
        {
            float curLinVarMember = md.linearAccMagnitudeClearedFromConstant + linearAccEnergy;
            linearAccVariance += curLinVarMember * curLinVarMember;
            float curRotVarMember = md.gyroRotationSpeedMagnitudeClearedFromConstant + gyroRotationEnergy;
            gyroRotationVariance += curRotVarMember * curRotVarMember;
        }
        linearAccVariance    /= N;
        gyroRotationVariance /= N;

        MovementStepData mdNew = new MovementStepData();

        mdNew.gyroRotationEnergy = gyroRotationEnergy;
        mdNew.gyroRotationSpeed  = gyroRotationSpeed;
        mdNew.gyroRotationSpeedClearedFromConstant          = gyroRotationSpeedClearedFromConstant;
        mdNew.gyroRotationSpeedMagnitude                    = gyroRotationSpeedMagnitude;
        mdNew.gyroRotationSpeedMagnitudeClearedFromConstant = gyroRotationSpeedMagnitudeClearedFromConstant;
        mdNew.gyroRotationVariance                  = gyroRotationVariance;
        mdNew.linearAcc                             = linearAcc;
        mdNew.linearAccClearedFromConstant          = linearAccClearedFromConstant;
        mdNew.linearAccEnergy                       = linearAccEnergy;
        mdNew.linearAccMagnitude                    = linearAccMagnitude;
        mdNew.linearAccMagnitudeClearedFromConstant = linearAccMagnitudeClearedFromConstant;
        mdNew.linearAccVariance                     = linearAccVariance;
        mdNew.linearAccAverage                      = linearAccAverage;

        if (N >= maxDepth)
        {
            dataQueue.Dequeue();
        }
        dataQueue.Enqueue(mdNew);
        lastMSD = mdNew;
    }