/// <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; }
protected override void OnInitialized(EventArgs e) { if (Reg.Size == 1) { Qubits.Content = "Qubits: " + FirstQubitIndex.ToString(); } else { Qubits.Content = "Qubits: " + FirstQubitIndex.ToString() + "-" + (FirstQubitIndex + Reg.Size - 1).ToString(); } int matrixRows, matrixCols; if (Reg.Size % 2 == 0) { matrixRows = matrixCols = (int)Math.Pow(2, Reg.Size / 2); } else { matrixRows = (int)Math.Pow(2, (Reg.Size - 1) / 2); matrixCols = (int)Math.Pow(2, (Reg.Size + 1) / 2); } StateMatrix.ResizeGrid(matrixRows, matrixCols); for (int i = 0; i < matrixRows; i++) { for (int j = 0; j < matrixCols; j++) { StateMatrix[i, j] = ComplexParser.ToString(Reg.Amplitude(i * matrixCols + j), 4).ToString(); } } if (Reg.Size == 1) { Examine.IsEnabled = false; } base.OnInitialized(e); }
public static void Main(string[] args) { /* * XorShift160 lol = new XorShift160(); * double runningSum = 0; * * while (!Console.KeyAvailable) * { * Console.WriteLine(lol.NextDouble()); * } * return; */ IStateMatrix A = new StateMatrix(2, 2); A.SetValue(0, 0, 0); A.SetValue(0, 1, 1); A.SetValue(1, 0, 0); A.SetValue(1, 0, 0); /* * var c = new CompiledMatrix<IStateMatrix>(A); * c.Compile(); */ IInputMatrix B = new InputMatrix(2, 1); B.SetValue(0, 0, 0); B.SetValue(1, 0, 1); IOutputMatrix C = new OutputMatrix(2, 2); C.SetValue(0, 0, 1); C.SetValue(0, 1, 0); C.SetValue(1, 0, 0); C.SetValue(1, 1, 1); IFeedthroughMatrix D = new FeedthroughMatrix(2, 1); D.SetValue(0, 0, 0); D.SetValue(1, 0, 0); ControlVector u = new ControlVector(1); u.SetValue(0, 1); // TODO: Construct vectors and matrices by factory and let the simulation driver compile them after every external change IStateVector x = new StateVector(2); /* * IStateVector dx = new StateVector(x.Length); * IStateVector dxu = new StateVector(x.Length); * IOutputVector y = new OutputVector(C.Rows); * IOutputVector yu = new OutputVector(C.Rows); */ CancellationTokenSource cts = new CancellationTokenSource(); CancellationToken ct = cts.Token; Semaphore startCalculation = new Semaphore(0, 2); Semaphore calculationDone = new Semaphore(0, 2); SimulationTime simulationTime = new SimulationTime(); IStateVector dx = new StateVector(x.Length); Task inputToState = new Task(() => { IStateVector dxu = new StateVector(x.Length); while (!ct.IsCancellationRequested) { startCalculation.WaitOne(); Thread.MemoryBarrier(); A.Transform(x, ref dx); B.Transform(u, ref dxu); // TODO: TransformAndAdd() dx.AddInPlace(dxu); Thread.MemoryBarrier(); calculationDone.Release(); } }); IOutputVector y = new OutputVector(C.Rows); Task stateToOutput = new Task(() => { IOutputVector yu = new OutputVector(C.Rows); while (!ct.IsCancellationRequested) { startCalculation.WaitOne(); Thread.MemoryBarrier(); C.Transform(x, ref y); D.Transform(u, ref yu); // TODO: TransformAndAdd() y.AddInPlace(yu); Thread.MemoryBarrier(); calculationDone.Release(); } }); Task control = new Task(() => { Stopwatch watch = Stopwatch.StartNew(); int steps = 0; while (!ct.IsCancellationRequested) { // wait for a new u to be applied // TODO: apply control vector startCalculation.Release(2); // wait for y calculationDone.WaitOne(); calculationDone.WaitOne(); Thread.MemoryBarrier(); // wait for state vector to be changeable // TODO: perform real transformation x.AddInPlace(dx); // discrete integration, T=1 // video killed the radio star if (steps % 1000 == 0) { var localY = y; double thingy = steps / watch.Elapsed.TotalSeconds; Trace.WriteLine(simulationTime.Time + " Position: " + localY.GetValue(0) + ", Velocity: " + localY.GetValue(1) + ", Acceleration: " + u.GetValue(0) + ", throughput: " + thingy); } // cancel out acceleration if (steps++ == 10) { u.SetValue(0, 0); } // advance simulation time simulationTime.Add(TimeSpan.FromSeconds(1)); } }); inputToState.Start(); stateToOutput.Start(); control.Start(); Console.ReadKey(true); cts.Cancel(); /* * while (!Console.KeyAvailable) * { * A.Transform(x, ref dx); * B.Transform(u, ref dxu); // TODO: TransformAndAdd() * dx.AddInPlace(dxu); * * // TODO: perform transformation * x.AddInPlace(dx); // discrete integration, T=1 * * C.Transform(x, ref y); * D.Transform(u, ref yu); // TODO: TransformAndAdd() * y.AddInPlace(yu); * * // video killed the radio star * if (steps%1000 == 0) * { * Trace.WriteLine("Position: " + y[0] + ", Velocity: " + y[1] + ", Acceleration: " + u[0] + ", throughput: " + steps/watch.Elapsed.TotalSeconds); * } * * // cancel out acceleration * if (steps++ == 10) * { * u[0] = 0; * } * } */ }
internal TriStateMatrix(bool[,] internalArray) : base(internalArray) { StateMatrix = new StateMatrix(internalArray.GetLength(0)); }
public TriStateMatrix(int width) : base(width, new bool[width, width]) { StateMatrix = new StateMatrix(width); }