예제 #1
0
    //Kalman-Filter Schritt Definition
    private string FilterStep(int n, string msg)
    {
        setVariables();
        string[] values = msg.Split(fieldSeperater);
        float[,] xlast, x_priori, Plast, P_priori, S, Kn, x_post, yn, P_post;

        //Bei erstmaligem anwenden des Filters werden Startwerte genutzt
        if (n == 1)
        {
            xlast = x0; Plast = P0;
        }
        //Nutzen der gefilterten Werte aus dem letzten Schritt
        else
        {
            xlast = x[n - 2]; Plast = P[n - 2];
        }

        float angle = FlexSensorToRad(float.Parse(values[1], System.Globalization.CultureInfo.InvariantCulture));

        yn = new float[, ] {
            { angle },
            { float.Parse(values[2], System.Globalization.CultureInfo.InvariantCulture) }
        };

        //Ausführung der Formeln für den Kalman-Filter

        //x_priori = Ad * xlast; //+ Bd * yn[1]
        x_priori = mCalc.MultiplyMatrix(Ad, xlast);
        //P_priori = Ad * Plast * Ad.T + Gd * Q * Gd.T
        P_priori = mCalc.AddMatrix(
            mCalc.MultiplyMatrix(mCalc.MultiplyMatrix(Ad, Plast), mCalc.Transpose(Ad)),
            mCalc.MultiplyMatrix(mCalc.MultiplyMatrix(Gd, Q), mCalc.Transpose(Gd))
            );
        //S = C * P_priori * C.T + R
        S = mCalc.AddMatrix(
            mCalc.MultiplyMatrix(mCalc.MultiplyMatrix(C, P_priori), mCalc.Transpose(C)),
            R
            );
        //Kn = P_priori * C.T * linalg.pinv(S)
        Kn = mCalc.MultiplyMatrix(
            mCalc.MultiplyMatrix(P_priori, mCalc.Transpose(C)), mCalc.InvertMatrix(S)
            );

        //x_post = x_priori + Kn * (yn - C * x_priori         // - D * y[n, 1])
        float[,] C_mul_xpriori = mCalc.MultiplyMatrix(C, x_priori);
        x_post = mCalc.AddMatrix(
            x_priori,
            mCalc.MultiplyMatrix(Kn,
                                 (mCalc.SubMatrix(yn, C_mul_xpriori))
                                 )
            );

        //P_post = (np.eye(2) - Kn * C) * P_priori
        P_post = mCalc.MultiplyMatrix(mCalc.SubMatrix(Identity, mCalc.MultiplyMatrix(Kn, C)), P_priori);

        x.Add(x_post);
        P.Add(P_post);
        K.Add(Kn);
        xGyro.Add(yn[1, 0]); // ZU EDELER !!! RAUSCHEN NACH VORNE ZU GROß

        //Setzen der Winkel der Gameobjects zur Darstellung der eben gefilterten Werte
        unfilteredBiege.transform.eulerAngles = new Vector3(angle, 0, 0);
        unfilteredGyro.transform.eulerAngles  = new Vector3(cumSum(xGyro) * Ts, 0, 0);
        filtered.transform.eulerAngles        = new Vector3(x_post[0, 0], 0, 0);

        return(angle.ToString("0.00", System.Globalization.CultureInfo.InvariantCulture)
               + fieldSeperater
               + x_post[0, 0].ToString("0.00", System.Globalization.CultureInfo.InvariantCulture)
               + fieldSeperater
               + x_post[2, 0].ToString("0.00", System.Globalization.CultureInfo.InvariantCulture)
               + fieldSeperater
               + P_post[0, 0].ToString("0.00", System.Globalization.CultureInfo.InvariantCulture));
    }