public void FindMinimum()
{
int i;
_xCurrent = FirstStep();
double [] xPrev = new double[_nDim];
for (i = 0; i < _nDim; i++)
{
xPrev[i] = _initial[i];
}
double [] xNext;
GeneralMatrix hPrev = GeneralMatrix.Identity(_nDim, _nDim);
double currEps = 1e5;
_curIter = 0;
while (currEps > _epsilon && _curIter < _itMax)
{
_hessian = CalculateNextHessianApproximation(hPrev, xPrev, _xCurrent, _f.GetGradient(xPrev), _f.GetGradient(_xCurrent));
xNext = CalculateNextPoint(_xCurrent, _f.GetGradient(_xCurrent), _hessian);
for (i = 0; i < _nDim; i++)
{
xPrev[i] = _xCurrent[i];
_xCurrent[i] = xNext[i];
}
currEps = Diff(_xCurrent, xPrev);
_curIter++;
}
}
// 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)));
}
/******************* Method Kalman Filter ******************/
// Variable A,B, and H we will asumtion this is a constant value
// Most probably is 1.
private Joint kalmanFilter(GeneralMatrix z, int pos)
{
// Prepare
Joint join = new Joint();
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;
// Predict
GeneralMatrix Xp = Xk[pos];
GeneralMatrix Pp = P[pos];
// Measurement Update (correction
GeneralMatrix s = Pp + R;
K = Pp * s.Inverse(); // Calculate Kalman Gain
Xk[pos] = Xp + (K * (z - Xp));
GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension);
P[pos] = (I - K) * Pp;
estimationX = Xk[pos].GetElement(0, 0);
estimationY = Xk[pos].GetElement(1, 0);
estimationZ = Xk[pos].GetElement(2, 0);
join.Position.X = (float)estimationX;
join.Position.Y = (float)estimationY;
join.Position.Z = (float)estimationZ;
return(join);
}
/// <summary>
/// Returns first point toward the minimum.
/// An auxiliiary method made public only
/// for NUnit testing
/// </summary>
/// <returns></returns>
private double[] FirstStep()
{
//first approximation of the Hessian is always the
//identity matrix
GeneralMatrix iMat = GeneralMatrix.Identity(_nDim, _nDim);
return(CalculateNextPoint(_initial, _f.GetGradient(_initial), iMat));
}
/******************* End Function for calculate Angle ******************/
#endregion
#region Support Function
private GeneralMatrix convertJoinToMatrix(Joint j)
{
GeneralMatrix matrix = GeneralMatrix.Identity(3, 1);
matrix.SetElement(0, 0, j.Position.X);
matrix.SetElement(1, 0, j.Position.Y);
matrix.SetElement(2, 0, j.Position.Z);
return(matrix);
}
public void CholeskyDecomposition2()
{
double[][] pvals = { new double[] { 1.0, 1.0, 1.0 }, new double[] { 1.0, 2.0, 3.0 }, new double[] { 1.0, 3.0, 6.0 } };
GeneralMatrix A = new GeneralMatrix(pvals);
CholeskyDecomposition chol = A.chol();
GeneralMatrix X = chol.Solve(GeneralMatrix.Identity(3, 3));
Assert.IsTrue(GeneralTests.Check(A.Multiply(X), GeneralMatrix.Identity(3, 3)));
}
public void Identity()
{
double[][] ivals = { new double[] { 1.0, 0.0, 0.0, 0.0 }, new double[] { 0.0, 1.0, 0.0, 0.0 }, new double[] { 0.0, 0.0, 1.0, 0.0 } };
GeneralMatrix I = new GeneralMatrix(ivals);
GeneralMatrix K = GeneralMatrix.Identity(3, 4);
Assert.IsTrue(I.Norm1() == K.Norm1() && I.Norm1() == 1);
}
// 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.
*/
}
public void Clear()
{
m_transform = GeneralMatrix.Identity(3, 3);
}
public CoordinateMapping()
{
m_transform = GeneralMatrix.Identity(3, 3);
}
