/// <summary>
/// <para>Unscented transform parameters optimization procedure.</para>
/// <para>The OptimizationMethod param determines the optimization method:</para>
/// <para>- OptimizationMethod.RandomShoot - parameters are randomly sampled and the best sample is chosen as optimal;
/// <para>- OptimizationMethod.NelderMeed - parameters are optimized with non-gradient Nelder-Meed method.</para>
/// <para>The UTOptimizationType type param determines the relation between the optimized variable and the unscented tranform params (see UTParams and its constructors for details). </para>
/// <para>- If type is UTOptimizationType.ImplicitAlpha, then the optimized variable is saclar [alpha0];</para>
/// <para>- If type is UTOptimizationType.ImplicitAlphaBetaKappa, then optimized variable is a vector [alpha, beta, kappa];</para>
/// <para>- If type is UTOptimizationType.Explicit, then then optimized variable is a vector [lambda, wm0, wc0, wi]. ///TODO it is not correct to define the parameters of the unsctnted transform arbitraty, they have to be interdependent, so that the mean and cov would be transformed correctly.</para>
/// </summary>
/// <param name="method">Unscented transform parameters optimization method</param>
/// <param name="type">Unscented transform parameters definition type</param>
/// <param name="Phi1">State transformation: a nonlinear function which determines the dynamics: x_{t+1} = Phi_1(x_t) + Phi_2(x_t) W_t</param>
/// <param name="Phi2">Noise multiplicator in the dynamics equation: x_{t+1} = Phi(x_t) + W_t</param>
/// <param name="Psi1">Observations transformation: a nonlinear function which determines the relation between the state and the observations: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
/// <param name="Psi2">Noise multiplicator in the observations equation: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
/// <param name="Mw">Mean of the noise in the dynamics equation </param>
/// <param name="Rw">Covariance matrix of the state disturbances</param>
/// <param name="Mnu">Mean of the noise in the obseration equation </param>
/// <param name="Rnu">Convariance matrix of the observation noise</param>
/// <param name="Crit">Criterion: a function which determines the quality of the unscented Kalman filter. Depends on the sample covariance of the estimation error on the last step: val = Crit(Cov(X_T-Xhat_T,X_T-Xhat_T)) </param>
/// <param name="T">The upper bound of the observation interval</param>
/// <param name="models">Discrete vector model samples</param>
/// <param name="xhat0">Initial condition</param>
/// <param name="DX0Hat">Initial condition covariance</param>
/// <param name="outputFolder">The results are saved to this folder in file "UT_optimization_{type}.txt"</param>
static (double, UTParams, UTParams) UTParmsOptimize(OptimizationMethod method, UTDefinitionType type,
Func <int, Vector <double>, Vector <double> > Phi1,
Func <int, Vector <double>, Matrix <double> > Phi2,
Func <int, Vector <double>, Vector <double> > Psi1,
Func <int, Vector <double>, Matrix <double> > Psi2,
Vector <double> Mw,
Matrix <double> Rw,
Vector <double> Mnu,
Matrix <double> Rnu,
Func <Matrix <double>, double> Crit,
int T,
DiscreteVectorModel[] models,
Vector <double> xhat0,
Matrix <double> DX0Hat,
string outputFolder)
{
(int n, Vector <double> lowerBound, Vector <double> upperBound, Vector <double> initialGuess, string filename) = DefineOptimizationParameters(type, xhat0, string.IsNullOrWhiteSpace(outputFolder) ? null : Path.Combine(outputFolder, "UT_optimization_{type}.txt"));
double min = double.MaxValue;
Vector <double> argmin = Exts.Stack(initialGuess, initialGuess);
switch (method)
{
case OptimizationMethod.RandomShoot:
var OptimumRandom = RandomOptimizer.Minimize((x) => CalculateSampleCriterion(Phi1, Phi2, Psi1, Psi2, Mw, Rw, Mnu, Rnu, Crit, x, T, models, xhat0, DX0Hat), Exts.Stack(lowerBound, lowerBound), Exts.Stack(upperBound, upperBound), 100, 100, filename);
min = OptimumRandom.min;
argmin = OptimumRandom.argmin;
break;
case OptimizationMethod.NelderMeed:
NelderMeadSimplex optimizer = new NelderMeadSimplex(1e-3, 100);
var objective = ObjectiveFunction.Value((x) => CalculateSampleCriterion(Phi1, Phi2, Psi1, Psi2, Mw, Rw, Mnu, Rnu, Crit, x, T, models, xhat0, DX0Hat));
try
{
var optimumNM = optimizer.FindMinimum(objective, Exts.Stack(initialGuess, initialGuess));
min = optimumNM.FunctionInfoAtMinimum.Value;
argmin = optimumNM.MinimizingPoint;
}
catch (Exception e)
{
Console.WriteLine($"Optimizer faild, using the initail guess ({e.Message})");
argmin = Exts.Stack(initialGuess, initialGuess);
}
break;
default: // no optimization by default
break;
}
return(min, new UTParams(xhat0.Count, argmin.Take(n).ToArray()), new UTParams(xhat0.Count, argmin.Skip(n).Take(n).ToArray()));
}
/// <summary>
/// <para>Unscented transform parameters optimization procedure.</para>
/// <para>The OptimizationMethod param determines the optimization method:</para>
/// <para>- OptimizationMethod.RandomShoot - parameters are randomly sampled and the best sample is chosen as optimal;
/// <para>- OptimizationMethod.NelderMeed - parameters are optimized with non-gradient Nelder-Meed method.</para>
/// <para>The UTOptimizationType type param determines the relation between the optimized variable and the unscented tranform params (see UTParams and its constructors for details). </para>
/// <para>- If type is UTOptimizationType.ImplicitAlpha, then the optimized variable is saclar [alpha0];</para>
/// <para>- If type is UTOptimizationType.ImplicitAlphaBetaKappa, then optimized variable is a vector [alpha, beta, kappa];</para>
/// <para>- If type is UTOptimizationType.Explicit, then then optimized variable is a vector [lambda, wm0, wc0, wi]. ///TODO it is not correct to define the parameters of the unsctnted transform arbitraty, they have to be interdependent, so that the mean and cov would be transformed correctly.</para>
/// </summary>
/// <param name="method">Unscented transform parameters optimization method</param>
/// <param name="type">Unscented transform parameters definition type</param>
/// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</param>
/// <param name="Crit">Criterion: a function which determines the quality of the unscented transform estimate. Depends on the sample covariance of the estimation error: val = Crit(Cov(X-Xhat,X-Xhat)) </param>
/// <param name="X">Array of initial variable x samples</param>
/// <param name="Y">Array of transformed variable y = Phi(x) + nu samples</param>
/// <param name="mX">Mean of x</param>
/// <param name="KX">Cov of x</param>
/// <param name="KNu">Cov of the noize nu</param>
/// <param name="outputFolder">If needed, the results or the random sampling (OptimizationMethod.RandomShoot) are saved to this folder in file "UT_optimization_{type}.txt"</param>
/// <returns>Returns touple (the best criteron value, the parameters of the unscented transform with best estimation quality)</returns>
static (double, UTParams) UTParmsOptimize(OptimizationMethod method, UTDefinitionType type,
Func <Vector <double>, Vector <double> > Phi,
Func <Matrix <double>, double> Crit,
Vector <double>[] X,
Vector <double>[] Y,
Vector <double> mX,
Matrix <double> KX,
Matrix <double> KNu,
string outputFolder = null
)
{
int n;
string filename = string.IsNullOrWhiteSpace(outputFolder) ? null : Path.Combine(outputFolder, "UT_optimization_{type}.txt");
Vector <double> lowerBound;
Vector <double> upperBound;
Vector <double> initialGuess;
switch (type)
{
case UTDefinitionType.ImplicitAlpha:
n = 1;
lowerBound = Exts.Vector(1 - 2 / mX.Count);
upperBound = Exts.Vector(1);
initialGuess = Exts.Vector(0.5);
filename = filename.Replace("{type}", "ImplicitAlpha");
break;
case UTDefinitionType.ImplicitAlphaBetaKappa:
n = 3;
lowerBound = Exts.Vector(0, 0, 3.0 - mX.Count - 2.0);
upperBound = Exts.Vector(5, 5, 3.0 - mX.Count + 2.0);
initialGuess = Exts.Vector(0.5, 2.0, 3.0 - mX.Count);
filename = filename.Replace("{type}", "ImplicitABK"); break;
case UTDefinitionType.Explicit:
n = 4;
lowerBound = Exts.Vector(-10, -10, -10, -10);
upperBound = Exts.Vector(10, 10, 10, 10);
initialGuess = Exts.Vector((new UTParams(mX.Count, 0.5, 2.0, 3.0 - mX.Count)).Params);
filename = filename.Replace("{type}", "Explicit"); break;
default:
n = 0;
lowerBound = null;
upperBound = null;
initialGuess = null;
break;
}
double min = double.MaxValue;
Vector <double> argmin = initialGuess;
switch (method)
{
case OptimizationMethod.RandomShoot:
var OptimumRandom = RandomOptimizer.Minimize((p) => CalculateSampleCriterion(Phi, Crit, p, X, Y, mX, KX, KNu), lowerBound, upperBound, 1000, 1000, filename);
min = OptimumRandom.min;
argmin = OptimumRandom.argmin;
break;
case OptimizationMethod.NelderMeed:
NelderMeadSimplex optimizer = new NelderMeadSimplex(1e-3, 100);
var objective = ObjectiveFunction.Value((p) => CalculateSampleCriterion(Phi, Crit, p, X, Y, mX, KX, KNu));
var optimumNM = optimizer.FindMinimum(objective, initialGuess);
min = optimumNM.FunctionInfoAtMinimum.Value;
argmin = optimumNM.MinimizingPoint;
break;
}
return(min, new UTParams(mX.Count, argmin.AsArray()));
}
/// <summary>
/// <para>Unscented transform parameters stepwize optimization procedure.</para>
/// <para>The OptimizationMethod param determines the optimization method:</para>
/// <para>- OptimizationMethod.RandomShoot - parameters are randomly sampled and the best sample is chosen as optimal;
/// <para>- OptimizationMethod.NelderMeed - parameters are optimized with non-gradient Nelder-Meed method.</para>
/// <para>The UTOptimizationType type param determines the relation between the optimized variable and the unscented tranform params (see UTParams and its constructors for details). </para>
/// <para>- If type is UTOptimizationType.ImplicitAlpha, then the optimized variable is saclar [alpha0];</para>
/// <para>- If type is UTOptimizationType.ImplicitAlphaBetaKappa, then optimized variable is a vector [alpha, beta, kappa];</para>
/// <para>- If type is UTOptimizationType.Explicit, then then optimized variable is a vector [lambda, wm0, wc0, wi]. ///TODO it is not correct to define the parameters of the unsctnted transform arbitraty, they have to be interdependent, so that the mean and cov would be transformed correctly.</para>
/// </summary>
/// <param name="method">Unscented transform parameters optimization method</param>
/// <param name="type">Unscented transform parameters definition type</param>
/// <param name="Phi1">State transformation: a nonlinear function which determines the dynamics: x_{t+1} = Phi_1(x_t) + Phi_2(x_t) W_t</param>
/// <param name="Phi2">Noise multiplicator in the dynamics equation: x_{t+1} = Phi(x_t) + W_t</param>
/// <param name="Psi1">Observations transformation: a nonlinear function which determines the relation between the state and the observations: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
/// <param name="Psi2">Noise multiplicator in the observations equation: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
/// <param name="Mw">Mean of the noise in the dynamics equation </param>
/// <param name="Rw">Covariance matrix of the state disturbances</param>
/// <param name="Mnu">Mean of the noise in the obseration equation </param>
/// <param name="Rnu">Convariance matrix of the observation noise</param>
/// <param name="Crit">Criterion: a function which determines the quality of the unscented Kalman filter. Depends on the sample covariance of the estimation error on the last step: val = Crit(Cov(X_T-Xhat_T,X_T-Xhat_T)) </param>
/// <param name="T">The upper bound of the observation interval</param>
/// <param name="models">Discrete vector model samples</param>
/// <param name="xhat0">Initial condition</param>
/// <param name="DX0Hat">Initial condition covariance</param>
/// <param name="outputFolder">The results are saved to this folder in file "UT_optimization_{type}.txt"</param>
static (double, UTParams[], UTParams[]) UTParmsOptimizeStepwise(OptimizationMethod method, UTDefinitionType type,
Func <int, Vector <double>, Vector <double> > Phi1,
Func <int, Vector <double>, Matrix <double> > Phi2,
Func <int, Vector <double>, Vector <double> > Psi1,
Func <int, Vector <double>, Matrix <double> > Psi2,
Vector <double> Mw,
Matrix <double> Rw,
Vector <double> Mnu,
Matrix <double> Rnu,
Func <Matrix <double>, double> Crit,
int T,
DiscreteVectorModel[] models,
Vector <double> xhat0,
Matrix <double> DX0Hat,
string outputFolder)
{
UTParams[] pForecast = new UTParams[T];
UTParams[] pCorrect = new UTParams[T];
(int n, Vector <double> lowerBound, Vector <double> upperBound, Vector <double> initialGuess, string filename) = DefineOptimizationParameters(type, xhat0, string.IsNullOrWhiteSpace(outputFolder) ? null : Path.Combine(outputFolder, "UT_stepwise_ptimization_{type}.txt"));
Vector <double>[] xHatU = models.Select(x => xhat0).ToArray();
Matrix <double>[] PHatU = models.Select(x => DX0Hat).ToArray();
double min = double.MaxValue;
Console.WriteLine($"UKF estimate parameters start");
DateTime start = DateTime.Now;
for (int t = 1; t < T; t++)
//Parallel.For(0, T, new ParallelOptions() { MaxDegreeOfParallelism = System.Environment.ProcessorCount }, t =>
{
DateTime startiteration = DateTime.Now;
min = double.MaxValue;
Vector <double> argmin = initialGuess;
switch (method)
{
case OptimizationMethod.RandomShoot:
var OptimumRandom = RandomOptimizer.Minimize((x) => CalculateSampleStepwiseCriterion(Phi1, Phi2, Psi1, Psi2, Mw, Rw, Mnu, Rnu, Crit, x, t, models, xHatU, PHatU), Exts.Stack(lowerBound, lowerBound), Exts.Stack(upperBound, upperBound), 100, 100, filename);
min = OptimumRandom.min;
argmin = OptimumRandom.argmin;
break;
case OptimizationMethod.NelderMeed:
NelderMeadSimplex optimizer = new NelderMeadSimplex(1e-3, 100);
var objective = ObjectiveFunction.Value((x) => CalculateSampleStepwiseCriterion(Phi1, Phi2, Psi1, Psi2, Mw, Rw, Mnu, Rnu, Crit, x, t, models, xHatU, PHatU));
try
{
var optimumNM = optimizer.FindMinimum(objective, Exts.Stack(initialGuess, initialGuess));
min = optimumNM.FunctionInfoAtMinimum.Value;
argmin = optimumNM.MinimizingPoint;
}
catch (Exception e)
{
Console.WriteLine($"Optimizer faild, using the initail guess ({e.Message})");
argmin = Exts.Stack(initialGuess, initialGuess);
}
break;
}
pForecast[t] = new UTParams(xhat0.Count, argmin.Take(n).ToArray());
pCorrect[t] = new UTParams(xhat0.Count, argmin.Skip(n).Take(n).ToArray());
for (int i = 0; i < models.Count(); i++)
{
(xHatU[i], PHatU[i]) = Step(Phi1, Phi2, Psi1, Psi2, Mw, Rw, Mnu, Rnu, pForecast[t], pCorrect[t], t, models[i].Trajectory[t][1], xHatU[i], PHatU[i]);
}
Console.WriteLine($"UKF estimate parameters for t={t}, done in {(DateTime.Now - startiteration).ToString(@"hh\:mm\:ss\.fff")}");
}
// });
DateTime finish = DateTime.Now;
Console.WriteLine($"UKF estimate parameters finished in {(finish - start).ToString(@"hh\:mm\:ss\.fff")}");
return(min, pForecast, pCorrect);
}
