void EKFTask(Quaternion u, float error, IDictionary <int, Vector3> z)
{
ArrayMat uStateMatrix = g(_stateMatrix, u);
ArrayMat uVarianceCovarianceMatrix = _varianceCovarianceMatrix;
uVarianceCovarianceMatrix.At(0, 0, error);
uVarianceCovarianceMatrix.At(1, 1, error);
uVarianceCovarianceMatrix.At(2, 2, error);
uVarianceCovarianceMatrix.At(3, 3, error);
foreach (KeyValuePair <int, Vector3> zit in z)
{
int ObservingKey = zit.Key;
Vector3 ObservingValue = zit.Value;
if (InitList[ObservingKey])
{
Vector3 newVector = u * ObservingValue;
uStateMatrix.At(4 + 3 * ObservingKey, 0, (double)newVector.x);
uStateMatrix.At(5 + 3 * ObservingKey, 0, (double)newVector.y);
uStateMatrix.At(6 + 3 * ObservingKey, 0, (double)newVector.z);
uVarianceCovarianceMatrix.At(4 + 3 * ObservingKey, 4 + 3 * ObservingKey, (double)Mathf.Infinity);
uVarianceCovarianceMatrix.At(5 + 3 * ObservingKey, 5 + 3 * ObservingKey, (double)Mathf.Infinity);
uVarianceCovarianceMatrix.At(6 + 3 * ObservingKey, 6 + 3 * ObservingKey, (double)Mathf.Infinity);
InitList[ObservingKey] = false;
}
Vector3 j = new Vector3((float)uStateMatrix.At(4 + 3 * ObservingKey, 0), (float)uStateMatrix.At(5 + 3 * ObservingKey, 0), (float)uStateMatrix.At(6 + 3 * ObservingKey, 0));
Vector3 hObservingValue = new Quaternion(-u.x, -u.y, -u.z, u.w) * j;
ArrayMat hit = H(j, u) * FxJ(ObservingKey, MaxMapSize);
ArrayMat kit = uVarianceCovarianceMatrix * hit.t() * (hit * uVarianceCovarianceMatrix * hit.t() + Q).inv();
Vector3 Difference = ObservingValue - hObservingValue;
ArrayMat DifferenceMat = new ArrayMat(3, 1);
DifferenceMat.At(0, 0, Difference.x);
DifferenceMat.At(1, 0, Difference.y);
DifferenceMat.At(2, 0, Difference.z);
uStateMatrix = uStateMatrix + kit * DifferenceMat;
}
}
