// Inisiasi Variabel
private void initializeKalmanVar()
{
// Return count for frame to 0
count = 0;
// Prepare Variable
int length = GlobalVal.BodyPart.Length;
P = new GeneralMatrix[length];
Xk = new GeneralMatrix[length];
// 3 -> x,y,z
Rv = new double[length, 3];
jointAveragePart = new double[length, 3];
estimationVector = new double[6];
rBody = new Body[dt]; // Save Body every frame for calculating Measurement Variance
first = true; // Initial State
// Inisialisasi matrik fungsi transisi (F / A)
double[][] matrikA = { new double[] { 1, 0, 0, dt, 0, 0 },
new double[] { 0, 1, 0, 0, dt, 0 },
new double[] { 0, 0, 1, 0, 0, dt },
new double[] { 0, 0, 0, 1, 0, 0 },
new double[] { 0, 0, 0, 0, 1, 0 },
new double[] { 0, 0, 0, 0, 0, 1 } };
F = new GeneralMatrix(matrikA); // Initialize A variable
// Initialize State (H)
H = GeneralMatrix.Random(3, 6);
H.SetElement(0, 0, 1); H.SetElement(0, 1, 0); H.SetElement(0, 2, 0); H.SetElement(0, 3, 0); H.SetElement(0, 4, 0); H.SetElement(0, 5, 0);
H.SetElement(1, 0, 0); H.SetElement(1, 1, 1); H.SetElement(1, 2, 0); H.SetElement(1, 3, 0); H.SetElement(1, 4, 0); H.SetElement(1, 5, 0);
H.SetElement(2, 0, 0); H.SetElement(2, 1, 0); H.SetElement(2, 2, 1); H.SetElement(2, 3, 0); H.SetElement(2, 4, 0); H.SetElement(2, 5, 0);
// Initialize Process Noise (Q) -> ini bisa diabaikan
// Sedikit lebih sulit karena harus dipas"kan berdasarkan pengujian serta proses yang berlangsung
GeneralMatrix Qq = GeneralMatrix.Identity(6, 6);
Q = Qq.MultiplyEquals(0.1);
GeneralMatrix i = GeneralMatrix.Identity(6, 6);
for (int c = 0; c < length; c++)
{
// Prior error covariance matrix (P)
P[c] = i;
// Initialize Rv -> init: 0.01
Rv[c, 0] = 0.01;
Rv[c, 1] = 0.01;
Rv[c, 2] = 0.01;
}
/*
* Note: Tunning kalman filter (Give better result)
* 1. Q = 0.1; R = 0.01
* 2.
*/
}
