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