// Here define A,B, and H Matrice private Joint kalmanFilterFull(int pos) { // Prepare //GeneralMatrix r = GeneralMatrix.Identity(3, 3); //R = r.MultiplyEquals(0.01); // Set r from Deviation Standard GeneralMatrix r = GeneralMatrix.Identity(3, 3); r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y R = r; // Set estimation from coordinate average in frame buffer GeneralMatrix Z = GeneralMatrix.Random(3, 1); Z.SetElement(0, 0, jointAveragePart[pos, 0]); Z.SetElement(1, 0, jointAveragePart[pos, 1]); Z.SetElement(2, 0, jointAveragePart[pos, 2]); // Predict GeneralMatrix Xp = F * Xk[pos]; GeneralMatrix Pp = F * P[pos] * F.Transpose() + Q; // Measurement update ( Correction ) K = Pp * H.Transpose() * (H * Pp * H.Transpose() + R).Inverse(); Xk[pos] = Xp + (K * (Z - (H * Xp))); GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension); P[pos] = (I - (K * H)) * Pp; return(convertMatrixTojoint(Xk[pos])); }
public void Inverse() { GeneralMatrix r = GeneralMatrix.Random(4, 4); GeneralMatrix iR = r.Inverse(); Assert.IsTrue(GeneralTests.Check(r.Multiply(iR), GeneralMatrix.Identity(4, 4))); }
public void SubstractAndAdd() { GeneralMatrix B = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension); GeneralMatrix C = A.Subtract(B); Assert.IsTrue(GeneralTests.Check(C.Add(B), A)); }
public void InitData() { A = new GeneralMatrix(columnwise, validld); R = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension); S = new GeneralMatrix(columnwise, nonconformld); O = new GeneralMatrix(A.RowDimension, A.ColumnDimension, 1.0); }
public void ArrayMultiplyEquals() { A = R.Copy(); GeneralMatrix B = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension); A.ArrayMultiplyEquals(B); Assert.IsTrue(GeneralTests.Check(A.ArrayRightDivideEquals(B), R)); }
// 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. */ }
void OnFrameArrived(object sender, BodyFrameArrivedEventArgs e) { using (var frame = e.FrameReference.AcquireFrame()) { if ((frame != null) && (frame.BodyCount > 0)) { if ((this.bodies == null) || (this.bodies.Length != frame.BodyCount)) { this.bodies = new Body[frame.BodyCount]; } frame.GetAndRefreshBodyData(this.bodies); // Sensor will reveive 6 body -> this mean for 6 people for (int i = 0; i < brushes.Length; i++) { if (this.bodies[i].IsTracked) // For this testing will only get 1 body that is tracked { this.bodyDrawers[i].DrawFrame(this.bodies[i]); if (count < dt) { rBody[count] = this.bodies[i]; } else { // Calculate R Variance every 30 Frame Body calcRVariance(); // Set the counter back to 0 (restart the frame tracked count) // Set body hist count = 0; rBody[count] = this.bodies[i]; } if ((count + 1) % dt == 0 && count != 0) // Calculate every dt frame { // Hitung sudut dari sensor kamera RULA this.calculateAngle(this.bodies[i]); // Lakukan perhitungan RULA this.calculateRulaEngine(); } // Initialize Xk-1 if (first) { if (count + 1 == dt - 1) { for (int c = 0; c < Xk.Length; c++) { GeneralMatrix StateMatrix = GeneralMatrix.Random(6, 1); StateMatrix.SetElement(0, 0, rBody[count].Joints[GlobalVal.BodyPart[c]].Position.X); StateMatrix.SetElement(1, 0, rBody[count].Joints[GlobalVal.BodyPart[c]].Position.Y); StateMatrix.SetElement(2, 0, rBody[count].Joints[GlobalVal.BodyPart[c]].Position.Z); StateMatrix.SetElement(3, 0, 1); StateMatrix.SetElement(4, 0, 1); StateMatrix.SetElement(5, 0, 1); Xk[c] = StateMatrix; } first = false; } } // Count the frame count += 1; } else { this.bodyDrawers[i].ClearFrame(); } } } } }