//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));
}