/// <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);
}
