private (Matrix <double>, Matrix <double>) Filter(Matrix <double> A, Matrix <double> B)
{
var Ap = A.SubMatrix(0, 1, 0, A.ColumnCount);
var Bp = B.SubMatrix(0, 1, 0, B.ColumnCount);
for (int i = 1; i < A.RowCount; i++)
{
var At = SignalUtils.Stack(Ap, A.SubMatrix(i, 1, 0, A.ColumnCount));
var Bt = SignalUtils.Stack(Bp, B.SubMatrix(i, 1, 0, B.ColumnCount));
if (At.Rank() == At.RowCount)
{
Ap = At;
Bp = Bt;
}
else if (SignalUtils.Append(At, Bt).Rank() != At.RowCount)
{
logger.Debug($"Same constrains detected at NO.{i}, which will be ignored.");
}
else
{
logger.Debug(SignalUtils.SPrint(At, "At"));
logger.Debug(SignalUtils.SPrint(Bt, "Bt"));
throw logger.Error(new LigralException($"Conflict constrains detected at NO.{i}."));
}
}
return(Ap, Bp);
}
void Update()
{
if (Input.GetKeyDown(KeyCode.O))
{
Debug.Log("Testing over-hour: " + SignalUtils.GetSignalTimeOffset(new SignalTime(00, 59)));
}
}
public Beacon ToBeacon()
{
// See description for the +1 explanation.
int major = GetBeaconMajor() + 1;
int minor = GetBeaconMinor() + 1;
return(new Beacon(SignalUtils.GetSignaureUUID(signature), major, minor));
}
public override void FireEvent(Signal s)
{
player.PlayTrackEntry(trackEntry);
int offset = SignalUtils.GetSignalTimeOffset(s.GetSignalTime());
Diglbug.Log("Skipping track by " + offset + " seconds due to latency", PrintStream.AUDIO_PLAYBACK);
player.SetTrackTime(offset);
}
public void Digl_SetReceiveSignatures(Signature[] ss)
{
Diglbug.Log("Pipe_SetReceiveSignatures :" + ss.Length);
digl_receiveRegionArray = new iBeaconRegion[ss.Length];
for (int k = 0; k < digl_receiveRegionArray.Length; k++)
{
digl_receiveRegionArray[k] = new iBeaconRegion(digl_region, new Beacon(SignalUtils.GetSignaureUUID(ss[k]), 0, 0));
}
}
public override void ModeSelected()
{
if (!RecoveryManager.Instance.RunningRecovery())
{
Signature[] sigs = SignalUtils.GetAllSignatures();
BLE.Instance.Manager.SetReceivedSignatures(sigs);
BLE.Instance.Manager.StartReceiving();
BeginShow();
}
}
public void Send(byte[] id, params byte[] data)
{
if (this.client.IsConnected)
{
var bytesList = new List <byte>(id);
bytesList.AddRange(data);
var bytes = bytesList.ToArray();
this.client.Send(bytes.ToArray());
SignalLogs.Log($"[Sent] {SignalUtils.GetPlcSignalName(bytes)} 0x{Utils.ByteArrayToHexString(bytes)}");
}
}
private Matrix <double> Equal(Matrix <double> z)
{
var x = z.SubMatrix(0, n, 0, 1);
var u = z.SubMatrix(n, p, 0, 1);
var dx = problem.SystemDynamicFunction(x, u);
var y = problem.ObservationFunction();
var hx = Filter(x, x0, xConstrain);
var hu = Filter(u, u0, uConstrain);
var hy = Filter(y, y0, yConstrain);
var hdx = Filter(dx, dx0, dxConstrain);
return(SignalUtils.Stack(hx, hu, hy, hdx));
}
public void Trim(Problem problem)
{
modelName = problem.Name;
this.problem = problem;
if (optimizer == null)
{
optimizer = Optimizer.GetOptimizer(optimizerName);
}
var z = optimizer.Optimize(Cost, SignalUtils.Stack(x0, u0), Equal, Bound);
x = z.SubMatrix(0, n, 0, 1);
u = z.SubMatrix(n, p, 0, 1);
}
private Matrix <double> Bound(Matrix <double> z)
{
var x = z.SubMatrix(0, n, 0, 1);
var u = z.SubMatrix(n, p, 0, 1);
var dx = problem.SystemDynamicFunction(x, u);
var y = problem.ObservationFunction();
var gxUpper = Filter(x, xUpper, new Signal(xUpper).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var guUpper = Filter(u, uUpper, new Signal(uUpper).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var gyUpper = Filter(y, yUpper, new Signal(yUpper).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var gdxUpper = Filter(dx, dxUpper, new Signal(dxUpper).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var gxLower = Filter(xLower, x, new Signal(xLower).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var guLower = Filter(uLower, u, new Signal(uLower).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var gyLower = Filter(yLower, y, new Signal(yLower).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
var gdxLower = Filter(dxLower, dx, new Signal(dxLower).ToList().ConvertAll(bnd => !double.IsInfinity(bnd)));
return(SignalUtils.Stack(gxUpper, guUpper, gyUpper, gdxUpper, gxLower, guLower, gyLower, gdxLower));
}
private IEnumerator PlayActWhenLoaded(ActSignalBundle asb)
{
Act a = asb.act;
Signal s = asb.signal;
int timeToSkip = SignalUtils.GetSignalTimeOffset(s.GetSignalTime());
TracklistEntry toPlay = a.GetEntryAtActTime(timeToSkip);
LoadTrackIfNeeded(toPlay.GetTrack());
while (!toPlay.GetTrack().IsLoaded())
{
yield return(null);
}
// RecoveryManager.Instance.RecoveryComplete ();// TODO: Move this somewhere nicer. Maybe sub.
int postLoadTimeToSkip = SignalUtils.GetSignalTimeOffset(s.GetSignalTime());
float timeToPlayFrom = a.GetSpecificEntryTimeAtActTime(toPlay, postLoadTimeToSkip);
PlayTrackEntry(toPlay, timeToPlayFrom);
}
private (Matrix <double>, Matrix <double>) Filter(Matrix <double> A, Matrix <double> B, Matrix <double> A0, Matrix <double> B0)
{
if (A == null || B == null)
{
return(A0, B0);
}
if (A0 == null || B0 == null)
{
return(Filter(A, B));
}
var Ap = A0;
var Bp = B0;
if (Ap.ColumnCount != A.ColumnCount)
{
throw logger.Error(new LigralException($"Constrain dimensions do not agree in SQP(A) {Ap.ColumnCount}: {A.ColumnCount}"));
}
if (Bp.ColumnCount != B.ColumnCount)
{
throw logger.Error(new LigralException($"Constrain dimensions do not agree in SQP(B) {Bp.ColumnCount}: {B.ColumnCount}"));
}
for (int i = 0; i < A.RowCount; i++)
{
var At = SignalUtils.Stack(Ap, A.SubMatrix(i, 1, 0, A.ColumnCount));
var Bt = SignalUtils.Stack(Bp, B.SubMatrix(i, 1, 0, B.ColumnCount));
if (At.Rank() == At.RowCount)
{
Ap = At;
Bp = Bt;
}
else if (SignalUtils.Append(At, Bt).Rank() == At.RowCount)
{
logger.Debug($"Same constrains detected at NO.{i}, which will be ignored.");
}
else
{
logger.Debug($"Conflict constrains detected at NO.{i}, which are resulted from bound constrains and will be ignore.");
}
}
return(Ap, Bp);
}
public bool Send(byte[] id, params byte[] data)
{
var signal = new List <byte>(id);
if (data != null && data.Length > 0)
{
signal.AddRange(data);
}
var byteArr = signal.ToArray();
var sent = Services.PlcConnection.Send(byteArr);
if (sent)
{
Services.Messages.Info($"[Sent] {SignalUtils.GetGuiSignalName(byteArr)} 0x{Utils.ByteArrayToHexString(byteArr)}", MessageCategory.PLC);
}
else
{
Services.Messages.Error($"[Error Sending] {SignalUtils.GetGuiSignalName(byteArr)} 0x{Utils.ByteArrayToHexString(byteArr)}", MessageCategory.PLC);
}
return(sent);
}
void DataReceived(object sender, DataReceivedFromServerEventArgs e)
{
SignalLogs.Log($"[Recieved] {SignalUtils.GetGuiSignalName(e.Data)} 0x{Utils.ByteArrayToHexString(e.Data)}");
}
private void Awake()
{
digl_receiveRegionArray = new iBeaconRegion[] {
new iBeaconRegion(digl_region, new Beacon(SignalUtils.GetSignaureUUID(Signature.NONE), 0, 0))
};
}
public Signal(Signature s, Payload p)
{
SetSignature(s);
SetPayload(p);
time = SignalUtils.GetSignalTime();
}
public override Matrix <double> Optimize(MFunc cost, Matrix <double> x0, MFunc equal, MFunc bound)
{
int n = x0.RowCount;
if (x0.ColumnCount != 1)
{
throw logger.Error(new LigralException($"Only x with shape nx1 is supported, but we got {n}x{x0.ColumnCount}"));
}
var x = x0;
var c = cost(x);
for (int i = 0; i < MaximumIteration; i++)
{
var H = Algorithm.RoughHessian(cost, x);
var C = Algorithm.RoughPartial(cost, x);
var Ae = Algorithm.RoughPartial(equal, x);
var Be = equal(x);
if (Ae != null)
{
Ae = Ae.Transpose();
(Ae, Be) = Filter(Ae, Be);
if (Be.ColumnCount != 1)
{
throw logger.Error(new LigralException($"Only h(x) with shape mx1 is supported, but we got {Be.RowCount}x{Be.ColumnCount}"));
}
}
logger.Debug(SignalUtils.SPrint(Ae, "Ae"));
logger.Debug(SignalUtils.SPrint(Be, "Be"));
var Ab = Algorithm.RoughPartial(bound, x);
var Bb = bound(x);
if (Ab != null)
{
Ab = Ab.Transpose();
int bRowCount = Bb.RowCount;
for (int j = 0, k = 0; j < bRowCount; j++)
{
if (Bb[j - k, 0] < BoundaryConstrainTolerant)
{
Ab = Ab.RemoveRow(j - k);
Bb = Bb.RemoveRow(j - k);
k++;
}
}
}
logger.Debug(SignalUtils.SPrint(Ab, "Ab"));
logger.Debug(SignalUtils.SPrint(Bb, "Bb"));
(var A, var B) = Filter(Ab, Bb, Ae, Be);
logger.Debug(SignalUtils.SPrint(A, "A"));
logger.Debug(SignalUtils.SPrint(B, "B"));
if (B != null && B.ColumnCount != 1)
{
throw logger.Error(new LigralException($"Only g(x) with shape px1 is supported, but we got {B.RowCount-Be.RowCount}x{B.ColumnCount}"));
}
var build = Matrix <double> .Build;
int mp = B == null?0:B.RowCount;
var K = SignalUtils.Stack(
SignalUtils.Append(H, A == null?A:A.Transpose()),
SignalUtils.Append(A, mp == 0?null:build.Dense(mp, mp, 0))
);
if (K == null)
{
logger.Warn("No constrain is given, optimizer quit");
return(x);
}
logger.Debug(SignalUtils.SPrint(K, "K"));
var p = SignalUtils.Stack(-C, -B);
logger.Debug(SignalUtils.SPrint(p, "p"));
var r = K.Solve(p);
var s = r.SubMatrix(0, n, 0, 1);
logger.Debug(SignalUtils.SPrint(s, "s"));
var lambda = r.SubMatrix(n, mp, 0, 1);
logger.Debug(SignalUtils.SPrint(lambda, "lambda"));
x += s;
var cp = cost(x);
if (lambda.RowAbsoluteSums().ForAll(sum => sum < OptimizationStopLambdaTolerant))
{
logger.Info("SQP Optimizer stops as lambda is close to zero");
break;
}
else if ((cp - c).RowAbsoluteSums().ForAll(sum => sum < OptimizationStopCostTolerant))
{
logger.Info("SQP Optimizer stops as costs did not change.");
break;
}
else
{
c = cp;
}
}
return(x);
}
