public (Vector <double>, Matrix <double>) Step(int t, Vector <double> y, Vector <double> xHat_, Matrix <double> kHat_, int n)
{
Vector <double>[] x_mod = new Vector <double> [n];
Vector <double>[] y_mod = new Vector <double> [n];
//Parallel.For(0, n, new ParallelOptions() {MaxDegreeOfParallelism = System.Environment.ProcessorCount }, i =>
RandomVector <Normal> xHatDistr = new RandomVector <Normal>(xHat_, kHat_);
for (int i = 0; i < n; i++)
{
x_mod[i] = xHatDistr.Sample();
if (t > 0)
{
x_mod[i] = Phi1(t, x_mod[i]) + Phi2(t, x_mod[i]) * W(t);
}
y_mod[i] = Psi1(t, x_mod[i]) + Psi2(t, x_mod[i]) * Nu(t);
}
//});
Vector <double> f = x_mod.Average();
Matrix <double> kTilde = Exts.Cov(x_mod, x_mod);
Vector <double>[] zetaTilde = new Vector <double> [n];
for (int i = 0; i < n; i++)
{
zetaTilde[i] = Zeta(t, f, y_mod[i], kTilde);
}
Matrix <double> CovZetaTilde = Exts.Cov(zetaTilde, zetaTilde);
Matrix <double> InvCovZetaTilde = Matrix <double> .Build.Dense(CovZetaTilde.RowCount, CovZetaTilde.ColumnCount, 0.0);
try
{
InvCovZetaTilde = CovZetaTilde.PseudoInverse();
}
catch (Exception e)
{
Console.WriteLine("Can't inverse ZetaTilde");
Console.WriteLine(CovZetaTilde.ToString());
Console.WriteLine(e.Message);
}
Matrix <double> H = Exts.Cov(x_mod.Subtract(f), zetaTilde) * InvCovZetaTilde;
Vector <double> h = -H *zetaTilde.Average();
Matrix <double> kHat = kTilde - Exts.Cov(x_mod.Subtract(f), zetaTilde) * H.Transpose();
Vector <double> xHat__ = f + H * Zeta(t, f, y, kTilde) + h;
return(xHat__, kHat);
}
/// <summary>
/// Provides the unscented transform estimate for the array X = [x_0,...,x_N] of random variable x samples
/// given the array of observations Y = [y_0,...,y_N], where y_i = Phi(x_i) + Nu_i.
/// Parameters of the unscented transform utParams should be initialized.
/// </summary>
/// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</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="mErr_UT">Returns: estimation error mean vector</param>
/// <param name="KErr_UT">Returns: estimation error covariance marix</param>
/// <param name="KErrTh_UT">Returns: estimation error theoretical covariance marix</param>
/// <returns>Array of estimates \hat{X} = [\hat{x}_0,...,\hat{x}_N]</returns>
public Vector <double>[] Estimate(Func <Vector <double>, Vector <double> > Phi, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu,
out Vector <double> mErr_UT, out Matrix <double> KErr_UT, out Matrix <double> KErrTh_UT)
{
UnscentedTransform.Transform(x => Phi(x), mX, KX, KNu, utParams, out Vector <double> M_UT, out Matrix <double> Kxy_UT, out Matrix <double> Kyy_UT);
Matrix <double> P_UT = Kxy_UT * ((Kyy_UT).PseudoInverse());
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);
mErr_UT = Err_UT.Average();
KErr_UT = Exts.Cov(Err_UT, Err_UT);
return(Xhat_UT);
}
/// <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 void Initialize(int n, string outputFolder)
{
provider = new NumberFormatInfo();
provider.NumberDecimalSeparator = ".";
StaticVectorModel[] models = new StaticVectorModel[n];
Vector <double>[] X = new Vector <double> [n];
Vector <double>[] Y = new Vector <double> [n];
Vector <double>[] Xinv = new Vector <double> [n];
Vector <double>[] YXinv = new Vector <double> [n];
for (int i = 0; i < n; i++)
{
models[i] = new StaticVectorModel(Phi, InvPhi, W, Nu);
X[i] = models[i].X;
Y[i] = models[i].Y;
Xinv[i] = models[i].Xinv;
YXinv[i] = models[i].YXinv;
}
Kxx = Exts.Cov(X, X);
Kxy = Exts.Cov(X, YXinv);
Kyy = Exts.Cov(YXinv, YXinv);
My = YXinv.Average();
P = Kxy * (Kyy.PseudoInverse());
Kxy_inv = Exts.Cov(X, Xinv);
Kyy_inv = Exts.Cov(Xinv, Xinv);
My_inv = Xinv.Average();
P_inv = Kxy_inv * (Kyy_inv.PseudoInverse());
Kxy_lin = Exts.Cov(X, Y);
Kyy_lin = Exts.Cov(Y, Y);
My_lin = Y.Average();
P_lin = Kxy_lin * (Kyy_lin.PseudoInverse());
utStaticEstimate = new UTStaticEstimate(UTDefinitionType.ImplicitAlphaBetaKappa, OptimizationMethod.NelderMeed);
utStaticEstimate.EstimateParameters(Phi, x => x.Trace(), X, Y, MX, KX, KNu, outputFolder);
//utStaticEstimate.utParams = new UTParams(2, 0.5, 2.0, 1.0);
using (System.IO.StreamWriter outputfile = new System.IO.StreamWriter(Path.Combine(outputFolder, "UTStaticEstimateParams.txt")))
{
outputfile.WriteLine(utStaticEstimate.utParams.ToString());
outputfile.Close();
}
}
/// <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);
}
public void GenerateBundle(int n,
out Vector <double> mErr, out Matrix <double> KErr, out Matrix <double> KErrTh,
out Vector <double> mErr_inv, out Matrix <double> KErr_inv, out Matrix <double> KErrTh_inv,
out Vector <double> mErr_lin, out Matrix <double> KErr_lin, out Matrix <double> KErrTh_lin,
out Vector <double> mErr_UT, out Matrix <double> KErr_UT, out Matrix <double> KErrTh_UT,
string fileName = null
)
{
StaticVectorModel[] models = new StaticVectorModel[n];
Vector <double>[] X = new Vector <double> [n];
Vector <double>[] Y = new Vector <double> [n];
Vector <double>[] Xinv = new Vector <double> [n];
Vector <double>[] YXinv = new Vector <double> [n];
for (int i = 0; i < n; i++)
{
models[i] = new StaticVectorModel(Phi, InvPhi, W, Nu);
X[i] = models[i].X;
Y[i] = models[i].Y;
Xinv[i] = models[i].Xinv;
YXinv[i] = models[i].YXinv;
}
Vector <double>[] Xhat = YXinv.Select(y => MX + P * (y - My)).ToArray();
Vector <double>[] Err = Xhat.Subtract(X);
mErr = Err.Average();
KErr = Exts.Cov(Err, Err);
//KErrTh = KX - P * Kyy * P.Transpose();
KErrTh = Kxx - P * Kyy * P.Transpose();
Vector <double>[] Xhat_inv = Xinv.Select(y => MX + P_inv * (y - My_inv)).ToArray();
Vector <double>[] Err_inv = Xhat_inv.Subtract(X);
mErr_inv = Err_inv.Average();
KErr_inv = Exts.Cov(Err_inv, Err_inv);
//KErrTh_inv = KX - P_inv * Kyy_inv * P_inv.Transpose();
KErrTh_inv = Kxx - P_inv * Kyy_inv * P_inv.Transpose();
Vector <double>[] Xhat_lin = Y.Select(y => MX + P_lin * (y - My_lin)).ToArray();
Vector <double>[] Err_lin = Xhat_lin.Subtract(X);
mErr_lin = Err_lin.Average();
KErr_lin = Exts.Cov(Err_lin, Err_lin);
//KErrTh_lin = KX - P_lin * Kyy_lin * P_lin.Transpose();
KErrTh_lin = Kxx - P_lin * Kyy_lin * P_lin.Transpose();
Vector <double>[] Xhat_UT = utStaticEstimate.Estimate(Phi, X, Y, MX, KX, KNu, out mErr_UT, out KErr_UT, out KErrTh_UT);
if (!string.IsNullOrWhiteSpace(fileName))
{
string X_head = string.Join("; ", Enumerable.Range(0, X[0].Count).Select(i => $"X_{i}"));
string Y_head = string.Join("; ", Enumerable.Range(0, Y[0].Count).Select(i => $"Y_{i}"));
string Xinv_head = string.Join("; ", Enumerable.Range(0, Xinv[0].Count).Select(i => $"Xinv_{i}"));
string Xhat_head = string.Join("; ", Enumerable.Range(0, Xhat[0].Count).Select(i => $"Xhat_{i}"));
string Xhat_inv_head = string.Join("; ", Enumerable.Range(0, Xhat_inv[0].Count).Select(i => $"Xhat_inv_{i}"));
string Xhat_lin_head = string.Join("; ", Enumerable.Range(0, Xhat_lin[0].Count).Select(i => $"Xhat_lin_{i}"));
string Xhat_UT_head = string.Join("; ", Enumerable.Range(0, Xhat_lin[0].Count).Select(i => $"Xhat_UT_{i}"));
using (System.IO.StreamWriter outputfile = new System.IO.StreamWriter(fileName))
{
outputfile.WriteLine($"{X_head}; {Y_head}; {Xinv_head}; {Xhat_head}; {Xhat_inv_head}; {Xhat_lin_head}; {Xhat_UT_head}");
for (int i = 0; i < n; i++)
{
outputfile.WriteLine($"{X[i].ToLine()}; {Y[i].ToLine()}; {Xinv[i].ToLine()}; {Xhat[i].ToLine()}; {Xhat_inv[i].ToLine()}; {Xhat_lin[i].ToLine()}; {Xhat_UT[i].ToLine()}");
}
outputfile.Close();
}
}
}
public void EstimateParameters(DiscreteVectorModel[] models, Vector <double> xhat0, int T)
{
int n_total = models.Count();
Vector <double>[] xHat = Enumerable.Repeat(xhat0, n_total).ToArray();
Console.WriteLine($"BCMNF estimate parameters start");
DateTime start = DateTime.Now;
for (int t = 1; t < T; t++) // start from 1 because for 0 we do not have observations
{
DateTime startiteration = DateTime.Now;
Vector <double>[] x = new Vector <double> [n_total];
Vector <double>[] y = new Vector <double> [n_total];
Vector <double>[] alpha = new Vector <double> [n_total];
Vector <double>[] gamma = new Vector <double> [n_total];
for (int i = 0; i < n_total; i++)
{
x[i] = models[i].Trajectory[t][0];
y[i] = models[i].Trajectory[t][1];
alpha[i] = Alpha(t, xHat[i]);
gamma[i] = Gamma(t, xHat[i], y[i]);
}
Vector <double> mX = x.Average();
Vector <double> mAlpha = alpha.Average();
Vector <double> mGamma = gamma.Average();
Matrix <double> covXX = Exts.Cov(x, x);
Matrix <double> covAlphaAlpha = Exts.Cov(alpha, alpha);
Matrix <double> covGammaGamma = Exts.Cov(gamma, gamma);
Matrix <double> covXAlpha = Exts.Cov(x, alpha);
Matrix <double> covXGamma = Exts.Cov(x, gamma);
Matrix <double> covGammaAlpha = Exts.Cov(gamma, alpha);
Matrix <double> invCovAlphaAlpha = covAlphaAlpha.Inverse(1e-32, 1e-32);
//Matrix<double> invCovAlphaAlpha = covAlphaAlpha.PseudoInverse();
Matrix <double> F = covXAlpha * invCovAlphaAlpha;
Vector <double> f = mX - F * mAlpha;
Matrix <double> kTildeXX = covXX - F * covXAlpha.Transpose();
Matrix <double> H = covGammaAlpha * invCovAlphaAlpha;
Vector <double> h = mGamma - H * mAlpha;
Matrix <double> kTildeXGamma = covXGamma - F * covGammaAlpha.Transpose();
Matrix <double> kTildeGammaGamma = covGammaGamma - H * covGammaAlpha.Transpose();
Matrix <double> invKTildeGammaGamma = kTildeGammaGamma.Inverse(1e-32, 1e-32);
//Matrix<double> invKTildeGammaGamma = kTildeGammaGamma.PseudoInverse();
Matrix <double> Gain = kTildeXGamma * invKTildeGammaGamma;
Matrix <double> kHat = kTildeXX - Gain * kTildeXGamma.Transpose();
for (int i = 0; i < n_total; i++)
{
xHat[i] = F * alpha[i] + f + Gain * (gamma[i] - H * alpha[i] - h);
}
FHat.Add(t, F);
fHat.Add(t, f);
HHat.Add(t, H);
hHat.Add(t, h);
GainHat.Add(t, Gain);
KTilde.Add(t, kTildeXX);
KHat.Add(t, kHat);
Console.WriteLine($"BCMNF estimate parameters for t={t}, done in {(DateTime.Now - startiteration).ToString(@"hh\:mm\:ss\.fff")}");
x = null;
y = null;
alpha = null;
gamma = null;
}
DateTime finish = DateTime.Now;
Console.WriteLine($"BCMNF estimate parameters finished in {(finish - start).ToString(@"hh\:mm\:ss\.fff")}");
}
public void EstimateParameters(DiscreteVectorModel[] models, Vector <double> xhat0, int T)
{
//Func<Vector<double>, bool> Sifter = (x) => Math.Sqrt(x[0] * x[0] + x[1] * x[1]) > 1000.0;
int n_total = models.Count();
Vector <double>[] xHat = Enumerable.Repeat(xhat0, n_total).ToArray();
//bool[] inUse = Enumerable.Repeat(true, n_total).ToArray();
Console.WriteLine($"CMNF estimate parameters start");
DateTime start = DateTime.Now;
for (int t = 1; t < T; t++) // start from 1 because for 0 we do not have observations
{
//int n = inUse.Where(e => e).Count();
DateTime startiteration = DateTime.Now;
Vector <double>[] x = new Vector <double> [n_total];
Vector <double>[] y = new Vector <double> [n_total];
Vector <double>[] xiHat = new Vector <double> [n_total];
//int k = 0;
for (int i = 0; i < n_total; i++)
{
//if (inUse[i])
//{
x[i] = models[i].Trajectory[t][0];
y[i] = models[i].Trajectory[t][1];
xiHat[i] = Xi(t, xHat[i]);
// k++;
//}
}
Matrix <double> CovXiHat = Exts.Cov(xiHat, xiHat);
Matrix <double> InvCovXiHat = Matrix <double> .Build.Dense(CovXiHat.RowCount, CovXiHat.ColumnCount, 0.0);
if (CovXiHat.FrobeniusNorm() > 0)
{
try
{
InvCovXiHat = CovXiHat.PseudoInverse();
//InvCovXiHat = CovXiHat.Inverse(1e-32, 1e-32);
}
catch (Exception e)
{
Console.WriteLine("Can't inverse XiHat");
Console.WriteLine(CovXiHat.ToString());
Console.WriteLine(e.Message);
}
}
Matrix <double> F = Exts.Cov(x, xiHat) * InvCovXiHat;
Vector <double> f = x.Average() - F * xiHat.Average();
Matrix <double> kTilde = Exts.Cov(x, x) - F * Exts.Cov(x, xiHat).Transpose();
Vector <double>[] xTilde = new Vector <double> [n_total];
Vector <double>[] zetaTilde = new Vector <double> [n_total];
Vector <double>[] delta_x_xTilde = new Vector <double> [n_total];
Matrix <double>[] delta_by_zetaTilde = new Matrix <double> [n_total];
for (int i = 0; i < n_total; i++)
{
xTilde[i] = F * xiHat[i] + f;
zetaTilde[i] = Zeta(t, xTilde[i], y[i], kTilde);
delta_x_xTilde[i] = x[i] - xTilde[i];
delta_by_zetaTilde[i] = delta_x_xTilde[i].ToColumnMatrix() * zetaTilde[i].ToRowMatrix();
}
Matrix <double> CovZetaTilde = Exts.Cov(zetaTilde, zetaTilde);
Matrix <double> InvCovZetaTilde = Matrix <double> .Build.Dense(CovZetaTilde.RowCount, CovZetaTilde.ColumnCount, 0.0);
try
{
InvCovZetaTilde = CovZetaTilde.PseudoInverse();
//InvCovZetaTilde = CovZetaTilde.Inverse(1e-32, 1e-32);
}
catch (Exception e)
{
Console.WriteLine("Can't inverse ZetaTilde");
Console.WriteLine(CovZetaTilde.ToString());
Console.WriteLine(e.Message);
}
var delta_x = x.Subtract(xTilde);
Matrix <double> H = delta_by_zetaTilde.Average() * InvCovZetaTilde;
Vector <double> h = -H *zetaTilde.Average();
Matrix <double> kHat = kTilde - Exts.Cov(delta_x, zetaTilde) * H.Transpose();
//k = 0;
for (int i = 0; i < n_total; i++)
{
//if (inUse[i])
//{
xHat[i] = xTilde[i] + H * zetaTilde[i] + h;
// inUse[i] = !Sifter(x[k] - xHat[k]);
// k++;
//}
}
FHat.Add(t, F);
fHat.Add(t, f);
HHat.Add(t, H);
hHat.Add(t, h);
KTilde.Add(t, kTilde);
KHat.Add(t, kHat);
//KHat.Add(t, Exts.Cov(x, x) - Exts.Cov(x, xiHat) * F - Exts.Cov(x.Subtract(xTilde), zetaTilde) * H);
// KHat.Add(t, cov(x, x) - cov(x, xiHat) * F - cov(x - xTilde, zetaTilde) * H);
Matrix <double> I = Matrix <double> .Build.DenseIdentity(CovXiHat.RowCount);
//Console.WriteLine();
//Console.WriteLine($"cov_xi_{t} = {CovXiHat.ToMatlab()}");
//Console.WriteLine();
//Console.WriteLine($"inv_by_cov_xi_{t} = {(InvCovXiHat * CovXiHat).ToMatlab()}");
//Console.WriteLine();
//Console.WriteLine($"cov_zeta_{t} = {CovZetaTilde.ToMatlab()}");
//Console.WriteLine();
//Console.WriteLine($"inv_by_cov_zeta_{t} = {(InvCovZetaTilde * CovZetaTilde).ToMatlab()}");
//Console.WriteLine();
//Console.WriteLine();
//Console.WriteLine($"$cov(\\hat{{\\xi}}_{{{t}}}, \\hat{{\\xi}}_{{{t}}}) = {CovXiHat.ToLatex("E3")}$");
//Console.WriteLine();
//Console.WriteLine($"$cov(\\hat{{\\xi}}_{{{t}}}, \\hat{{\\xi}}_{{{t}}}) \\cdot (cov(\\hat{{\\xi}}_{{{t}}}, \\hat{{\\xi}}_{{{t}}}))^{{-1}} = {(InvCovXiHat * CovXiHat).ToLatex("F3")}$");
//Console.WriteLine();
//double diff_xi = (I - InvCovXiHat * CovXiHat).L2Norm();
//Console.WriteLine($"$|| I - cov(\\hat{{\\xi}}_{{{t}}}, \\hat{{\\xi}}_{{{t}}}) \\cdot (cov(\\hat{{\\xi}}_{{{t}}}, \\hat{{\\xi}}_{{{t}}}))^{{-1}}|| = {diff_xi}$");
//Console.WriteLine();
//if (diff_xi > 0.001)
// Console.WriteLine("!!!");
//Matrix<double> II = Matrix<double>.Build.DenseIdentity(CovZetaTilde.RowCount);
//Console.WriteLine($"$cov(\\tilde{{\\zeta}}_{{{t}}}, \\tilde{{\\zeta}}_{{{t}}}) = {CovZetaTilde.ToLatex("E3")}$");
//Console.WriteLine();
//Console.WriteLine($"$cov(\\tilde{{\\zeta}}_{{{t}}}, \\tilde{{\\zeta}}_{{{t}}}) \\cdot (cov(\\tilde{{\\zeta}}_{{{t}}}, \\tilde{{\\zeta}}_{{{t}}}))^{{-1}} = {(InvCovZetaTilde * CovZetaTilde).ToLatex("F3")}$");
//Console.WriteLine();
//double diff_zeta = (II - InvCovZetaTilde * CovZetaTilde).L2Norm();
//Console.WriteLine($"$|| I - cov(\\tilde{{\\zeta}}_{{{t}}}, \\tilde{{\\zeta}}_{{{t}}}) \\cdot (cov(\\tilde{{\\zeta}}_{{{t}}}, \\tilde{{\\zeta}}_{{{t}}}))^{{-1}}|| = {diff_zeta}$");
//Console.WriteLine();
//if (diff_zeta > 0.001)
// Console.WriteLine("!!!");
//Console.WriteLine($"$F_{{{t}}} = {F.ToLatex()}$");
//Console.WriteLine($"$f_{{{t}}} = {f.ToLatex()}$");
//Console.WriteLine($"$H_{{{t}}} = {H.ToLatex()}$");
//Console.WriteLine($"$h_{{{t}}} = {h.ToLatex()}$");
//Console.WriteLine($"$\\tilde{{K}}_{{{t}}} = {kTilde.ToLatex()}$");
//Console.WriteLine($"$\\hat{{K}}_{{{t}}} = {kHat.ToLatex()}$");
Console.WriteLine($"CMNF estimate parameters for t={t}, done in {(DateTime.Now - startiteration).ToString(@"hh\:mm\:ss\.fff")}");
x = null;
y = null;
xiHat = null;
}
DateTime finish = DateTime.Now;
Console.WriteLine($"CMNF estimate parameters finished in {(finish - start).ToString(@"hh\:mm\:ss\.fff")}");
}
public (Vector <double>, Matrix <double>) Step(int t, Vector <double> y, Vector <double> xHat_, Matrix <double> kHat_)
{
Vector <double>[] x_mod = new Vector <double> [Models.Count()];
Vector <double>[] y_mod = new Vector <double> [Models.Count()];
Vector <double>[] xiHat_mod = new Vector <double> [Models.Count()];
RandomVector <Normal> xHatDistr = new RandomVector <Normal>(xHat_, kHat_);
int selected = 0;
for (int i = 0; i < Models.Count(); i++)
{
//Models[i].Step();
//if (Models[i].Trajectory[t][1].InBounds(y - SigmaNu, y + SigmaNu))
//{
// x_mod[i] = Models[i].Trajectory[t][0];
// y_mod[i] = Models[i].Trajectory[t][1];
// xiHat_mod[i] = Xi(t, xHat[i]);
// selected++;
//}
//else
//{
x_mod[i] = xHatDistr.Sample();
if (t > 0)
{
x_mod[i] = Phi1(t, x_mod[i]) + Phi2(t, x_mod[i]) * W(t);
}
y_mod[i] = Psi1(t, x_mod[i]) + Psi2(t, x_mod[i]) * Nu(t);
xiHat_mod[i] = Xi(t, xHatDistr.Sample());
// Models[i].Trajectory[t][0] = x_mod[i];
// Models[i].Trajectory[t][1] = y_mod[i];
//}
}
int good = 0;
for (int i = 0; i < Models.Count(); i++)
{
if (y_mod[i].InBounds(y - SigmaNu, y + SigmaNu))
{
good++;
}
}
//Console.WriteLine($"selected: {selected}, total good: {good}");
Matrix <double> CovXiHat = Exts.Cov(xiHat_mod, xiHat_mod);
Matrix <double> InvCovXiHat = Matrix <double> .Build.Dense(CovXiHat.RowCount, CovXiHat.ColumnCount, 0.0);
if (CovXiHat.FrobeniusNorm() > 0)
{
try
{
InvCovXiHat = CovXiHat.PseudoInverse();
}
catch (Exception e)
{
Console.WriteLine("Can't inverse XiHat");
Console.WriteLine(CovXiHat.ToString());
Console.WriteLine(e.Message);
}
}
Matrix <double> F = Exts.Cov(x_mod, xiHat_mod) * InvCovXiHat;
Vector <double> f = x_mod.Average() - F * xiHat_mod.Average();
Matrix <double> kTilde = Exts.Cov(x_mod, x_mod) - Exts.Cov(x_mod, xiHat_mod) * F.Transpose();
Vector <double>[] xTilde = new Vector <double> [Models.Count()];
Vector <double>[] zetaTilde = new Vector <double> [Models.Count()];
for (int i = 0; i < Models.Count(); i++)
{
xTilde[i] = F * xiHat_mod[i] + f;
zetaTilde[i] = Zeta(t, xTilde[i], y_mod[i], kTilde);
}
Matrix <double> CovZetaTilde = Exts.Cov(zetaTilde, zetaTilde);
Matrix <double> InvCovZetaTilde = Matrix <double> .Build.Dense(CovZetaTilde.RowCount, CovZetaTilde.ColumnCount, 0.0);
try
{
InvCovZetaTilde = CovZetaTilde.PseudoInverse();
}
catch (Exception e)
{
Console.WriteLine("Can't inverse ZetaTilde");
Console.WriteLine(CovZetaTilde.ToString());
Console.WriteLine(e.Message);
}
Matrix <double> H = Exts.Cov(x_mod.Subtract(f), zetaTilde) * InvCovZetaTilde;
Vector <double> h = -H *zetaTilde.Average();
Matrix <double> kHat = kTilde - Exts.Cov(x_mod.Subtract(f), zetaTilde) * H.Transpose();
Vector <double> xTilde__ = F * Xi(t, xHat_) + f;
Vector <double> xHat__ = xTilde__ + H * Zeta(t, xTilde__, y, kTilde) + h;
for (int i = 0; i < Models.Count(); i++)
{
xHat[i] = xTilde[i] + H * zetaTilde[i] + h;
}
return(xHat__, kHat);
}