/// <summary>
/// Update the EKF.
/// 更新
/// </summary>
/// <param name="q">Q.</param>
/// <param name="error">Error.</param>
/// <param name="points">Points.</param>
/// <param name="callback">Callback.</param>
public IEnumerator UpdateEnumerator(Quaternion q, float error, Dictionary <int, Vector3> points)
{
_updateStatus = true;
int nowSize = StateMatrix.rows();
int N = (nowSize - 4) / 3;
Mat uStateMatrix = g(StateMatrix, q);
Mat uVarianceCovarianceMatrix = G(N) * VarianceCovarianceMatrix * GT(N) + FxT(N) * R(error) * Fx(N);
yield return(null);
Mat I = Mat.eye(nowSize, nowSize, Type);
Quaternion uStateMatrix_q = new Quaternion((float)uStateMatrix.get(0, 0)[0], (float)uStateMatrix.get(1, 0)[0], (float)uStateMatrix.get(2, 0)[0], (float)uStateMatrix.get(3, 0)[0]);
foreach (int pointID in points.Keys)
{
Vector3 ZT = points[pointID];
Vector3 uStateMatrix_j = new Vector3((float)uStateMatrix.get(4 + pointID * 3, 0)[0], (float)uStateMatrix.get(4 + pointID * 3 + 1, 0)[0], (float)uStateMatrix.get(4 + pointID * 3 + 2, 0)[0]);
Quaternion qi = new Quaternion(-uStateMatrix_q.x, -uStateMatrix_q.y, -uStateMatrix_q.z, uStateMatrix_q.w);
Vector3 ZTH = qi * uStateMatrix_j;
Mat h = H(uStateMatrix_j, uStateMatrix_q) * FxJ(pointID, N);
Mat K = uVarianceCovarianceMatrix * h.t() * (h * uVarianceCovarianceMatrix * h.t() + Q).inv();
Mat deltaZ = Mat.zeros(3, 3, Type);
deltaZ = deltaZ.resize(3, 1);
deltaZ.put(0, 0, (double)(ZT.x - ZTH.x));
deltaZ.put(1, 0, (double)(ZT.y - ZTH.y));
deltaZ.put(2, 0, (double)(ZT.z - ZTH.z));
uStateMatrix = uStateMatrix + K * deltaZ;
uVarianceCovarianceMatrix = (I - K * h) * uVarianceCovarianceMatrix;
yield return(null);
}
_stateMatrix = uStateMatrix;
_varianceCovarianceMatrix = uVarianceCovarianceMatrix;
_updateStatus = false;
}
/// <summary>
/// Add the EKF.
/// 追加
/// </summary>
/// <param name="q">Q.</param>
/// <param name="error">Error.</param>
/// <param name="points">Points.</param>
/// <param name="Descriptors">Descriptors.</param>
public IEnumerator AddEnumerator(Quaternion q, float error, IList <Vector3> points, MatOfFloat Descriptors)
{
_updateStatus = true;
if (points.Count != Descriptors.rows())
{
throw new OutLookARException("KeyVectorとDescriptorsの数が合いません");
}
int nowSize = StateMatrix.rows();
int N = (nowSize - 4) / 3;
int pointsCount = points.Count;
int newSize = nowSize + pointsCount * 3;
Mat uStateMatrix = g(StateMatrix, q);
uStateMatrix = uStateMatrix.resize(newSize);
yield return(null);
Mat uVarianceCovarianceMatrix = G(N) * VarianceCovarianceMatrix * GT(N) + FxT(N) * R(error) * Fx(N);
uVarianceCovarianceMatrix = uVarianceCovarianceMatrix.resize(newSize, newSize);
for (int i = 0; i < pointsCount; i++)
{
for (int c = 0; c < 3; c++)
{
uVarianceCovarianceMatrix.put(nowSize + c + 3 * i, nowSize + c + 3 * i, Infinite);
}
yield return(null);
}
Mat I = Mat.eye(newSize, newSize, Type);
Quaternion uStateMatrix_q = new Quaternion((float)uStateMatrix.get(0, 0)[0], (float)uStateMatrix.get(1, 0)[0], (float)uStateMatrix.get(2, 0)[0], (float)uStateMatrix.get(3, 0)[0]);
int j = N + 1;
foreach (Vector3 point in points)
{
Vector3 uStateMatrix_j = uStateMatrix_q * point;
uStateMatrix.put(j * 3 + 1, 0, uStateMatrix_j.x);
uStateMatrix.put(j * 3 + 2, 0, uStateMatrix_j.y);
uStateMatrix.put(j * 3 + 3, 0, uStateMatrix_j.z);
Mat h = H(uStateMatrix_j, uStateMatrix_q) * FxJ(j, N + pointsCount);
Mat K = uVarianceCovarianceMatrix * h.t() * (h * uVarianceCovarianceMatrix * h.t() + Q).inv();
uVarianceCovarianceMatrix = (I - K * h) * uVarianceCovarianceMatrix;
yield return(null);
j++;
}
_stateMatrix = uStateMatrix;
_varianceCovarianceMatrix = uVarianceCovarianceMatrix;
_stateDescriptors.Add(Descriptors);
_updateStatus = false;
}
/// <summary>
/// Add the EKF.
/// 追加
/// </summary>
/// <param name="q">Q.</param>
/// <param name="error">Error.</param>
/// <param name="points">Points.</param>
public void Add(Quaternion q, float error, IList <Vector3> points)
{
_updateStatus = true;
int nowSize = StateMatrix.rows();
int N = (nowSize - 4) / 3;
int pointsCount = points.Count;
int newSize = nowSize + pointsCount * 3;
Mat uStateMatrix = g(StateMatrix, q);
uStateMatrix = uStateMatrix.resize(newSize);
Mat uVarianceCovarianceMatrix = G(N) * VarianceCovarianceMatrix * GT(N) + FxT(N) * R(error) * Fx(N);
uVarianceCovarianceMatrix = uVarianceCovarianceMatrix.resize(newSize, newSize);
for (int i = 0; i < pointsCount; i++)
{
for (int c = 0; c < 3; c++)
{
uVarianceCovarianceMatrix.put(nowSize + c + 3 * i, nowSize + c + 3 * i, Infinite);
}
}
Mat I = Mat.eye(newSize, newSize, Type);
Quaternion uStateMatrix_q = new Quaternion((float)uStateMatrix.get(0, 0)[0], (float)uStateMatrix.get(1, 0)[0], (float)uStateMatrix.get(2, 0)[0], (float)uStateMatrix.get(3, 0)[0]);
int j = N + 1;
foreach (Vector3 point in points)
{
Vector3 uStateMatrix_j = uStateMatrix_q * point;
uStateMatrix.put(j * 3 + 1, 0, uStateMatrix_j.x);
uStateMatrix.put(j * 3 + 2, 0, uStateMatrix_j.y);
uStateMatrix.put(j * 3 + 3, 0, uStateMatrix_j.z);
Mat h = H(uStateMatrix_j, uStateMatrix_q) * FxJ(j, N + pointsCount);
Mat K = uVarianceCovarianceMatrix * h.t() * (h * uVarianceCovarianceMatrix * h.t() + Q).inv();
uVarianceCovarianceMatrix = (I - K * h) * uVarianceCovarianceMatrix;
j++;
}
_stateMatrix = uStateMatrix;
_varianceCovarianceMatrix = uVarianceCovarianceMatrix;
_updateStatus = false;
}