// 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. */ }