ArrayMat g(ArrayMat stateMatrix, Quaternion q) { if (_stateMatrix.Rows < 4 || _stateMatrix.Cols != 1) { throw new OutLookARException("ArrayMat mu is not rows >= 4 & cols() = 1 ."); } stateMatrix.At(0, 0, q.x); stateMatrix.At(1, 0, q.y); stateMatrix.At(2, 0, q.z); stateMatrix.At(3, 0, q.w); return(stateMatrix); }
/// <summary> /// Cvms the mul. /// 非同期による行列の掛け算 /// </summary> /// <returns>The mul.</returns> /// <param name="z">The z coordinate.</param> /// <param name="w">The width.</param> /// <param name="callback">Callback.</param> public static System.Collections.IEnumerator cvmMul(ArrayMat z, ArrayMat w, Action <ArrayMat> callback) { if (z.Cols != w.Rows) { throw new OutLookARException("Mat type must bu equal to the number of w.rows and z.cols."); } System.Diagnostics.Stopwatch sw = System.Diagnostics.Stopwatch.StartNew(); ArrayMat v = ArrayMat.zeros(z.Rows, w.Cols); for (int r = 0; r < v.Rows; r++) { for (int c = 0; c < v.Cols; c++) { double pt = 0; for (int b = 0; b < z.Cols; b++) { pt += z.At(r, b) * w.At(b, c); if (sw.Elapsed.Milliseconds > 100) { sw.Reset(); Debug.Log(string.Format("cvmMul :... {0}/{1} ... {2} %", r, v.Rows, r * 100 / v.Rows)); yield return(null); } sw.Start(); } v.At(r, c, pt); } } callback(v); }
void MappingTask(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); var NoStateIDs = from i in MMatchList where i.StateID < 0 orderby i.MapID select i.MapID; int NoStateCount = 0; foreach (KeyValuePair <int, Vector3> j in z) { if (j.Key < 0) { int MatID; if (NoStateIDs.Count() >= NoStateCount) { var MaxStateIDs = from i in MMatchList where i.Error > 0 orderby i.OutOfCount descending select new { i.MapID, i.Error } into countObje orderby countObje.Error descending select countObje.MapID; MatID = MaxStateIDs.ElementAt(0); } else { MatID = NoStateIDs.ElementAt(NoStateCount); NoStateCount++; } Vector3 newVector = u * j.Value; uStateMatrix.At(MatID * 3 + 4, 0, newVector.x); uStateMatrix.At(MatID * 3 + 4, 0, newVector.y); uStateMatrix.At(MatID * 3 + 4, 0, newVector.z); } else { } } }
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; } }
ArrayMat FxJ(int j, int n) { if (j < 0 & n >= j) { throw new OutLookARException("引数が不正です。引数は0以上の整数である必要が有ります。またn>j。"); } ArrayMat mj = new ArrayMat(7, 4 + 3 * n); mj.At(0, 0, 1); mj.At(1, 1, 1); mj.At(2, 2, 1); mj.At(3, 3, 1); mj.At(4, 3 * j + 1, 1); mj.At(5, 3 * j + 2, 1); mj.At(6, 3 * j + 3, 1); return(mj); }
ArrayMat H(Vector3 v, Quaternion q) { ArrayMat h = new ArrayMat(3, 7); //{{2 (qy Y + qz Z)}, {2 (qy X - 2 qx Y + qw Z)}, {2 qz X - 2 qw Y - 4 qx Z}} h.At(0, 0, (double)(2 * (q.y * v.y - q.z * v.z))); h.At(1, 0, (double)(2 * (q.y * v.x - 2 * q.x * v.y + q.w * v.z))); h.At(2, 0, (double)(2 * (q.z * v.x - q.w * v.y - 2 * q.x * v.z))); //{{-4 qy X + 2 qx Y - 2 qw Z}, {2 (qx X + qz Z)}, {-2 qw X + 2 qz Y - 4 qy Z}} h.At(0, 1, (double)(2 * (-2 * q.y * v.x + q.x * v.y - q.w * v.z))); h.At(1, 1, (double)(2 * (q.x * v.x + q.z * v.z))); h.At(2, 1, (double)(2 * (-q.w * v.x + q.z * v.y - 2 * q.y * v.z))); //{{2 (-2 qz X + qw Y + qx Z)}, {-2 qw X - 4 qz Y + 2 qy Z}, {2 (qx X + qy Y)}} h.At(0, 2, (double)(2 * (-2 * q.z * v.x + q.w * v.y + q.x * v.z))); h.At(1, 2, (double)(2 * (-q.w * v.x - 2 * q.z * v.y + q.y * v.z))); h.At(2, 2, (double)(2 * (q.x * v.x + q.y * v.y))); //{{2 qz Y - 2 qy Z}, {-2 qz X + 2 qx Z}, {-2 (qy X + qx Y)}} h.At(0, 3, (double)(2 * (q.z * v.y - q.y * v.z))); h.At(1, 3, (double)(2 * (-q.z * v.x + q.x * v.z))); h.At(2, 3, (double)(-2 * (q.y * v.x + q.x * v.y))); //{{1 - 2 qy^2 - 2 qz^2}, {2 qx qy - 2 qz qw}, {2 qx qz - 2 qy qw}} h.At(0, 4, (double)(1 - 2 * Mathf.Pow(q.y, 2f) - 2 * Mathf.Pow(q.z, 2f))); h.At(1, 4, (double)(2 * (q.x * q.y - q.z * q.w))); h.At(2, 4, (double)(2 * (q.x * q.z - q.y * q.w))); //{{2 (qx qy + qz qw)}, {1 - 2 qx^2 - 2 qz^2}, {2 qy qz - 2 qx qw}} h.At(0, 5, (double)(2 * (q.x * q.y - q.z * q.w))); h.At(1, 5, (double)(1 - 2 * Mathf.Pow(q.x, 2f) - 2 * Mathf.Pow(q.z, 2f))); h.At(2, 5, (double)(2 * (q.y * q.z - q.x * q.w))); //{{2 qx qz - 2 qy qw}, {2 (qy qz + qx qw)}, {1 - 2 qx^2 - 2 qy^2}} h.At(0, 6, (double)(2 * (q.x * q.z - q.y * q.w))); h.At(1, 6, (double)(2 * (q.y * q.z + q.x * q.w))); h.At(2, 6, (double)(1 - 2 * Mathf.Pow(q.x, 2f) - 2 * Mathf.Pow(q.y, 2f))); return(h); }