/* Interpole une valeur pour la position, en supposant aucun input du joueur, dans nbFrames frames */
/// <summary>
/// Tries to predict the object's position in nbFrames units of time.<br/>
/// Currently does a pretty poor job at it.
/// </summary>
/// <param name="nbFrames">The number of units of time ahead we want to know the object's position</param>
/// <returns>A position vector</returns>
public Vector2 interpolation(int nbFrames)
{
Vector temp = new DenseVector(estimation.ToArray());
for (int i = 0; i < nbFrames; i++)
{
temp = (Vector)(F * temp);
}
return(new Vector2((float)temp.At(0), (float)temp.At(2)));
}
/// <summary>
/// Returns two correlated randomly generated values according to a gauss(0,1)
/// </summary>
/// <param name="var">The stddev of the correlation</param>
/// <returns>A vector2 made of two correlated randomly generated values</returns>
public static Vector2 gaussCorrelated(double stddev = 1)
{
double t1 = Laws.gauss();
double t2 = Laws.gauss();
Vector vect = new DenseVector(new double[] { t1, t2, });
Matrix R = new DenseMatrix(2, 2);
R.SetRow(0, new double[] { 1, 0, });
R.SetRow(1, new double[] { 0, 1, });
R = (Matrix)R.Multiply(Math.Pow(stddev, 2));
Matrix sqrtR = sqrtm(R);
vect = (Vector)(sqrtR * vect);
return(new Vector2((float)vect.At(0), (float)vect.At(1)));
}
/* Prends une observation (pos.x, speed.x, pos.y, speed.y) non bruitée, la bruite et estime les nouveaux paramètres */
/// <summary>
/// Takes a sensor-perfect observation as entry for a Kalman filter iteration, adds realistic sensor imperfection and updates the internal state accordingly
/// </summary>
/// <param name="obs">A noised sensor-perfect position+speed observation</param>
public void addObservation(Vector4 obs)
{
Vector tempVector = new DenseVector(4);
double[] storage = new double[4];
storage[0] = obs.x; storage[1] = obs.y; storage[2] = obs.z; storage[3] = obs.w;
tempVector.SetValues(storage);
//tempVector = addNoise(tempVector);
// public noised values
posBruit.x = (float)tempVector.At(0);
posBruit.y = (float)tempVector.At(2);
/****************** Prédiction ******************/
// n(k|k-1) = F * n(k-1|k-1)
Vector nkminus1 = (Vector)(F * estimation);
// P(k|k-1) = F * P(k-1|k-1) * F^T + Q
Matrix Pkminus1 = (Matrix)(F * P * F.Transpose() + Q);
// Mise à jour
// K = P(k|k-1) * H^T * (R + H * P(k|k-1) * H^T)^-1
Matrix S = (Matrix)(R + H * Pkminus1 * H.Transpose());
K = (Matrix)(Pkminus1 * H.Transpose() * S.Inverse());
// P(k|k) = (I - KH) * P(k| k-1)
P = (Matrix)((I - K * H) * Pkminus1);
// n(k|k) = n(k|k-1) + K(y - H*n(k|k-1))
Vector y = (Vector)(H * tempVector);
y = addImprecision(y);
posImprecise = new Vector2((float)y.At(0), (float)y.At(1));
estimation = (Vector)(nkminus1 + K * (y - (H * nkminus1)));
posInterp.x = (float)estimation.At(0);
posInterp.y = (float)estimation.At(2);
}
private void AddPatterns(IList <DenseVector> chaoticSeries, int firstIndex)
{
for (int i = firstIndex; i < chaoticSeries.Count; i++)
{
Pattern adding = new Pattern(startTime + i);
DenseVector input = new DenseVector(historyLength);
for (int j = 1; j <= historyLength; j++)
{
input.At(j - 1, chaoticSeries[i - j].At(0)); // At(i) is likely faster than [i] in Math.NET
}
adding.Input = input;
adding.IdealOutput = chaoticSeries[i];
patterns.Add(adding);
}
}
private bool BayesTrain(List <List <double> > feature, List <int> label)
{
var featNum = feature.Count;
var featDim = feature[0].Count;
var labNum = label.Count;
if (labNum != featNum)
{
return(false);
}
_mClassLabel.Clear();
for (var i = 0; i < GestureNumber; i++)
{
_mClassLabel.Add(i);
}
Matrix <double> featMat = new DenseMatrix(featNum, featDim);
for (var i = 0; i < featNum; i++)
{
for (var j = 0; j < featDim; j++)
{
featMat[i, j] = feature[i][j];
}
}
var cNum = _mClassLabel.Count;
Matrix <double> meanMat = new DenseMatrix(cNum, featDim);
Matrix <double> covMat = new DenseMatrix(featDim * cNum, featDim);
Matrix <double> poolCovMat = new DenseMatrix(featDim, featDim);
Vector <double> numPerClass = new DenseVector(cNum);
// compute the mean vector for each class
for (var i = 0; i < featNum; i++)
{
for (var j = 0; j < cNum; j++)
{
if (label[i] != _mClassLabel[j])
{
continue;
}
meanMat.SetRow(j, meanMat.Row(j) + featMat.Row(i));
numPerClass.At(j, numPerClass.At(j) + 1);
}
}
for (var i = 0; i < cNum; i++)
{
meanMat.SetRow(i, meanMat.Row(i) / numPerClass.At(i));
}
//compute the covariance matrix for each class and pool covariance matrix
for (var i = 0; i < featNum; i++)
{
for (var j = 0; j < cNum; j++)
{
if (label[i] == _mClassLabel[j])
{
covMat.SetSubMatrix(j * featDim, featDim, 0, featDim,
covMat.SubMatrix(j * featDim, featDim, 0, featDim) +
(featMat.Row(i) - meanMat.Row(j)).OuterProduct(featMat.Row(i) - meanMat.Row(j)));
}
}
}
for (var i = 0; i < cNum; i++)
{
poolCovMat += covMat.SubMatrix(i * featDim, featDim, 0, featDim);
//PoolCovMat += CovMat.block(i* feat_dim,0,feat_dim,feat_dim);
covMat.SetSubMatrix(i * featDim, featDim, 0, featDim,
covMat.SubMatrix(i * featDim, featDim, 0, featDim) / (numPerClass.At(i) - 1));
//CovMat.block(i* feat_dim,0,feat_dim,feat_dim)=CovMat.block(i* feat_dim,0,feat_dim,feat_dim)/(feat_num_perclass(i)-1);
}
poolCovMat /= featNum - cNum;
poolCovMat = poolCovMat.Inverse();
//transform the data format from Eigen to member vectors
ModelMean?.Clear();
ModelCov?.Clear();
List <double> temp;
for (var i = 0; i < cNum; i++)
{
temp = new List <double>();
for (var j = 0; j < featDim; j++)
{
temp.Add(meanMat[i, j]);
}
ModelMean?.Add(temp);
}
for (var i = 0; i < featDim; i++)
{
temp = new List <double>();
for (var j = 0; j < featDim; j++)
{
temp.Add(poolCovMat[i, j]);
}
ModelCov?.Add(temp);
}
return(true);
}
