//Instanzieren aller für den Kalman-Filter notwendigen deklarierten Variabeln
private void setVariables()
{
Identity = new float[, ] {
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
};
//Erster x-Eintrag
x0 = new float[, ] {
{ 0 },
{ 0 },
{ 0 }
};
P0 = new float[, ] {
{ 100, 0, 0 },
{ 0, 100, 0 },
{ 0, 0, 100 }
};
A = new float[, ] {
{ 0, Ts, -1 },
{ 0, 0, 0 },
{ 0, 0, 0 }
};
Ad = mCalc.AddMatrix(A, Identity);
C = new float[, ] {
{ 1, 0, 0 },
{ 0, 1, 1 }
};
Gd = new float[, ] {
{ pow(0.5f * Ts, 2), -Ts },
{ Ts, 0 },
{ 0, Ts }
};
Q = new float[, ] {
{ pow(sigma_biege, 2), 0 },
{ 0, pow(sigma_gyro, 2) }
};
R = new float[, ] {
{ 12, 0 },
{ 0, 29 }
};
}