/// <summary>
/// Wrapper for CalculateCriterionValue function.
/// The sample vector is transformed to the form unscented transform parameters (the way it is done depends on the length of the sample vector, see UTParams and its constructors for details).
/// Then the criterion value given the provided unscented transform parameters is calculated.
/// </summary>
/// <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="P">Sample vector to be transformed ro unscented transfrom parameters</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>
/// <returns>The criterion value for the unscented transfrom parameters obtained from the sample vactor</returns>
public static double CalculateSampleCriterion(Func <Vector <double>, Vector <double> > Phi, Func <Matrix <double>, double> Crit, Vector <double> P, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu)
{
int L = mX.Count;
UTParams p = new UTParams(mX.Count, P.AsArray());
double crit = CalculateCriterionValue(Phi, Crit, p, X, Y, mX, KX, KNu);
return(crit);
}
/// <summary>
/// Calculates the criterion value for the estimate given the particular unscented transform parameters
/// </summary>
/// <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="p1">Unscented transfrom parameters for the forecast phase</param>
/// <param name="p2">Unscented transfrom parameters for the correction phase</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>
/// <returns>The criterion value for the particular unscented transform parameters</returns>
public static double CalculateCriterionValue(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,
UTParams p1,
UTParams p2,
int T,
DiscreteVectorModel[] models,
Vector <double> xhat0,
Matrix <double> DX0Hat
)
{
double crit = 0;
try
{
int N = models.Count();
Vector <double>[] xHatU = models.Select(x => xhat0).ToArray();
Matrix <double>[] PHatU = models.Select(x => DX0Hat).ToArray();
//Vector<double> PHatU = Vector<double>.Build.Dense(N, DX0Hat);
for (int t = 1; t < T; t++)
{
for (int i = 0; i < N; i++)
{
(xHatU[i], PHatU[i]) = Step(Phi1, Phi2, Psi1, Psi2, Mw, Rw, Mnu, Rnu, p1, p2, t, models[i].Trajectory[t][1], xHatU[i], PHatU[i]);
}
Vector <double>[] states = models.Select(x => (x.Trajectory[t][0])).ToArray();
Matrix <double> errorUPow2 = Exts.Cov(states.Subtract(xHatU), states.Subtract(xHatU));
crit = Crit(errorUPow2);
}
}
catch (Exception e)
{
crit = double.MaxValue;
}
return(crit);
}
public static (Vector <double>, Matrix <double>) Step(Func <int, Vector <double>, Vector <double> > Phi,
Func <int, Vector <double>, Vector <double> > Psi,
Matrix <double> Rw,
Matrix <double> Rnu,
UTParams p1,
UTParams p2,
int t,
Vector <double> y,
Vector <double> xHat_,
Matrix <double> P_)
{
UnscentedTransform.Transform(x => Phi(t, x), xHat_, P_, Rw, p1, out Vector <double> Xtilde, out _, out Matrix <double> Ptilde);
UnscentedTransform.Transform(x => Psi(t, x), Xtilde, Ptilde, Rnu, p2, out Vector <double> Ytilde, out Matrix <double> PXY, out Matrix <double> PYtilde);
Matrix <double> K = PXY * PYtilde.Inverse();
return(Xtilde + K * (y - Ytilde), Ptilde - K * PYtilde * K.Transpose());
}
/// <summary>
/// The sample vector is transformed to a couple of the unscented transform parameters (the way it is done depends on the length of the sample vector, see UTParams and its constructors for details).
/// </summary>
/// <param name="P">Sample vector to be transformed to a couple of the unscented transfrom parameters</param>
/// <param name="dim">The state vector dimention</param>
/// <returns></returns>
public static (UTParams, UTParams) SampleVectorToUTParams(Vector <double> P, int dim)
{
int n = P.Count();
if (n % 2 != 0)
{
new ArgumentException("Sample vector size must be even", "Sample vector");
}
int L = dim;
double[] s1 = P.Take(n / 2).ToArray();
UTParams p1 = new UTParams(L, s1);
double[] s2 = P.Skip(n / 2).ToArray();
UTParams p2 = new UTParams(L, s2);
return(p1, p2);
}
/// <summary>
/// Performs a step of Unscented Kalman Filter given the particular unscented transform parameters
/// for forecast and correction phases
/// </summary>
/// <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="p1">Unscented transfrom parameters for the forecast phase</param>
/// <param name="p2">Unscented transfrom parameters for the correction phase</param>
/// <param name="t">Current step time instant</param>
/// <param name="y">Observations on the current step</param>
/// <param name="xHat_">Estimate on the previous step</param>
/// <param name="P_">Approximated previous step error covariance</param>
/// <param name="xHat">Returns: current step estimate</param>
/// <param name="P">Returns: approximated current step error covariance</param>
public static (Vector <double>, Matrix <double>) Step(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,
UTParams p1,
UTParams p2,
int t,
Vector <double> y,
Vector <double> xHat_,
Matrix <double> P_)
{
try
{
UnscentedTransform.Transform(x => Phi1(t, x) + Phi2(t, x) * Mw, xHat_, P_, Phi2(t, xHat_) * Rw * Phi2(t, xHat_).Transpose(), p1, out Vector <double> Xtilde, out _, out Matrix <double> Ptilde);
//UnscentedTransform.Transform(x => Phi1(t, x), xHat_, P_, Rw, p1, out Vector<double> Xtilde2, out _, out Matrix<double> Ptilde2);
UnscentedTransform.Transform(x => Psi1(t, x) + Psi2(t, x) * Mnu, Xtilde, Ptilde, Psi2(t, Xtilde) * Rnu * Psi2(t, Xtilde).Transpose(), p2, out Vector <double> Ytilde, out Matrix <double> PXY, out Matrix <double> PYtilde);
//UnscentedTransform.Transform(x => Psi1(t, x), Xtilde, Ptilde, Rnu, p2, out Vector<double> Ytilde2, out Matrix<double> PXY2, out Matrix<double> PYtilde2);
Matrix <double> K = PXY * PYtilde.Inverse();
return(Xtilde + K * (y - Ytilde), Ptilde - K * PYtilde * K.Transpose());
}
catch (Exception e)
{
Console.WriteLine(e.Message);
return(xHat_, P_);
}
//Matrix<double> K2 = PXY2 * PYtilde2.Inverse();
//Vector<double> xHat2 = Xtilde2 + K2 * (y - Ytilde2);
//Matrix<double> PHat2 = Ptilde2 - K2 * PYtilde2 * K2.Transpose();
//Console.WriteLine(xHat - xHat2);
//Console.WriteLine(PHat - PHat2);
}
/// <summary>
/// Calculates the criterion value for the estimate given the particular unscented transform parameters
/// </summary>
/// <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="p">Unscented transform parameters</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>
/// <returns>The criterion value for the particular unscented transform parameters</returns>
public static double CalculateCriterionValue(Func <Vector <double>, Vector <double> > Phi, Func <Matrix <double>, double> Crit, UTParams p, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu)
{
double crit = 0;
try
{
UnscentedTransform.Transform(x => Phi(x), mX, KX, KNu, p, out Vector <double> M_UT, out Matrix <double> Kxy_UT, out Matrix <double> Kyy_UT);
Matrix <double> P_UT = Kxy_UT * ((Kyy_UT).PseudoInverse());
Matrix <double> KErrTh_UT = KX - P_UT * Kyy_UT * P_UT.Transpose();
Vector <double>[] Xhat_UT = Y.Select(y => mX + P_UT * (y - M_UT)).ToArray();
Vector <double>[] Err_UT = Xhat_UT.Subtract(X);
Vector <double> mErr_UT = Err_UT.Average();
Matrix <double> KErr_UT = Exts.Cov(Err_UT, Err_UT);
crit = Crit(KErr_UT);//.Trace();
}
catch { crit = double.MaxValue; }
return(crit);
}
/// <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);
}
/// <summary>
/// The unscented transform for y = Phi(x) + nu
/// </summary>
/// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</param>
/// <param name="mX">Mean of the transformed random variable</param>
/// <param name="dX">Cov of the transformed random variable</param>
/// <param name="dNu">Cov of the additive random variable</param>
/// <param name="p">Parameters of the unscented transform</param>
/// <param name="y">Returns: approximated mean of the transformed variable</param>
/// <param name="Kxy">Returns: approximated cross-covariance of the initial and the transformed variable</param>
/// <param name="Kyy">Returns: approximated covariance of the transormed variable</param>
public static void Transform(Func <Vector <double>, Vector <double> > Phi, Vector <double> mX, Matrix <double> dX, Matrix <double> dNu, UTParams p, out Vector <double> y, out Matrix <double> Kxy, out Matrix <double> Kyy)
{
int L = mX.Count;
Matrix <double> Xi = SigmaPoints.Generate(mX, dX, p.Lambda);
Matrix <double> Upsilon = Phi(Xi.Column(0)).ToColumnMatrix();
for (int i = 1; i < 2 * L + 1; i++)
{
Upsilon = Upsilon.Append(Phi(Xi.Column(i)).ToColumnMatrix());
}
y = p.Wm[0] * Upsilon.Column(0);
for (int i = 1; i < 2 * L + 1; i++)
{
y = y + p.Wm[i] * Upsilon.Column(i);
}
Kxy = p.Wc[0] * (Xi.Column(0) - mX).ToColumnMatrix() * (Upsilon.Column(0) - y).ToRowMatrix();
Kyy = p.Wc[0] * (Upsilon.Column(0) - y).ToColumnMatrix() * (Upsilon.Column(0) - y).ToRowMatrix();
Matrix <double> Z = Xi.Stack(Upsilon);
Vector <double> MZ = mX.Stack(y);
Matrix <double> PFull = p.Wc[0] * (Z.Column(0) - MZ).ToColumnMatrix() * (Z.Column(0) - MZ).ToRowMatrix();
for (int i = 1; i < 2 * L + 1; i++)
{
PFull = PFull + p.Wc[i] * (Z.Column(i) - MZ).ToColumnMatrix() * (Z.Column(i) - MZ).ToRowMatrix();
Kxy = Kxy + p.Wc[i] * (Xi.Column(i) - mX).ToColumnMatrix() * (Upsilon.Column(i) - y).ToRowMatrix();
Kyy = Kyy + p.Wc[i] * (Upsilon.Column(i) - y).ToColumnMatrix() * (Upsilon.Column(i) - y).ToRowMatrix();
}
Kyy = Kyy + dNu;
//Py = Py + R;
//return PFull;
}
