/// <summary>
/// Obtain the Taylor Model for the non-linear ODE after substituting the controller response at the current state.
/// </summary>
private TaylorModelVec GetODEExpanded(State current)
{
// extract current values
Dictionary <string, DoubleInterval> x0 = current.ToDictionary();
x0.Add("time", new DoubleInterval(current.step * period));
// add controlled variables
var controlled = Hold(current);
foreach (var kvp in controlled)
{
x0.Add(kvp.Key, kvp.Value);
}
// update ODEs with the controlled variables
TaylorModel[] taylorModels = new TaylorModel[ode.Count];
for (int i = 0; i < expandedOde.Count; ++i)
{
// calculate ODE
Polynomial p = TaylorExpansion.Expansion(expandedOde[i], tmVarNamesWithout0, x0, order);
// Should calculate the error bounds...
taylorModels[i] = new TaylorModel(p, new DoubleInterval(0));
Console.Write("Polynomial: {0} ->", ode[i]);
p.Dump(tmVarNames, true);
}
return(new TaylorModelVec(taylorModels));
}
/// <summary>
/// Obtain the Taylor Model for the non-linear ODE after substituting the controller response at the current state.
/// </summary>
private TaylorModelVec GetODE(State current)
{
var controlled = Hold(current);
string[] varNames = new string[current.continuousNames.Length + 1];
varNames[0] = "time";
current.continuousNames.CopyTo(varNames, 1);
// update ODEs with the controlled variables
TaylorModel[] taylorModels = new TaylorModel[ode.Count];
for (int i = 0; i < ode.Count; ++i)
{
var odeC = ode[i];
foreach (var kvp in controlled)
{
//Log.WriteLine("Controlled {0} = {1}", kvp.Key, kvp.Value);
odeC = odeC.Substitute(kvp.Key, new REAL(kvp.Value));
}
// calculate ODE
Polynomial p = TaylorExpansion.ConvertPolynomial(odeC, current.continuousNames);
taylorModels[i] = new TaylorModel(p, new DoubleInterval(0));
}
return(new TaylorModelVec(taylorModels));
}
public void Initialize(string[] continuousNames, DoubleInterval[] continuousInitialState, string[] discreteNames, FPIntegerInterval[] discreteInitialState, int order, double period)
{
this.initialState = new State(0, continuousNames, new DoubleBoundingBox(continuousInitialState), discreteNames, new FPIntegerBoundingBox(discreteInitialState));
this.period = period;
this.order = order;
this.discreteVariables = this.initialState.discreteNames;
IsPolynomial = true;
ContainsSqrt = false;
for (int i = 0; i < ode.Count; ++i)
{
if (!ode[i].IsPolynomial())
{
IsPolynomial = false;
}
if (ode[i].ContainsSqrt())
{
ContainsSqrt = true;
}
}
// since flow* doesn't handle sqrt, do taylor expansion
if (ContainsSqrt)
{
string[] varNames = new string[continuousNames.Length + 1];
varNames[0] = "time";
continuousNames.CopyTo(varNames, 1);
expandedOde = new List <List <TaylorExpansion.TaylorStructure> >();
for (int i = 0; i < ode.Count; ++i)
{
var e = TaylorExpansion.GetExpansionStruct(ode[i], varNames, order);
expandedOde.Add(e.Item1);
}
}
foreach (string s in continuousNames)
{
Flowstar.DeclareStateVariable(s);
}
Flowstar.SetCutoff(cutoffThreshold);
estimate = new double[initialState.continuousNames.Length];
for (int i = 0; i < estimate.Length; ++i)
{
estimate[i] = errorEstimate;
}
localVarNames = new string[continuousNames.Length];
continuousNames.CopyTo(localVarNames, 0);
stateVarNames = new string[continuousNames.Length];
for (int i = 0; i < continuousNames.Length; ++i)
{
stateVarNames[i] = continuousNames[i] + "'";
}
tmVarNames = new string[continuousNames.Length + 1];
tmVarNames[0] = "local_t";
for (int i = 0; i < continuousNames.Length; ++i)
{
tmVarNames[i + 1] = continuousNames[i] + "0";
}
tmVarNamesWithout0 = new string[continuousNames.Length + 1];
tmVarNamesWithout0[0] = "time";
continuousNames.CopyTo(tmVarNamesWithout0, 1);
}
