public TestSimpleIdentification()
{
TestName = "Простая модель с идентификацией";
TestFileName = "TestSimpleIdentification";
Vector <double> mW = Exts.Vector(0, 0); Matrix <double> dW = Exts.Diag(0, 1.0);
Vector <double> mNu = Exts.Vector(0); Matrix <double> dNu = Exts.Diag(1.0);
Vector <double> mEta = Exts.Vector(0, 0.0); Matrix <double> dEta = Exts.Diag(0.27, 0);
Func <int, Vector <double>, Vector <double> > phi1 = (s, x) => Exts.Vector(x[0], x[0] * x[1]);
Func <int, Vector <double>, Matrix <double> > phi2 = (s, x) => Exts.Diag(0.0, 1.0);
Func <int, Vector <double>, Vector <double> > psi1 = (s, x) => Exts.Vector(x[1]);
Func <int, Vector <double>, Matrix <double> > psi2 = (s, x) => Exts.Matrix(0.1);
RandomVector <Normal> NormalW = new RandomVector <Normal>(mW, dW);
RandomVector <Normal> NormalNu = new RandomVector <Normal>(mNu, dNu);
ContinuousUniform UniformW = new ContinuousUniform(-0.9, 0.9);
Phi1 = phi1;
Phi2 = phi2;
Psi1 = psi1;
Psi2 = psi2;
Xi = (s, x) => phi1(s, x) + phi2(s, x) * mW;
Zeta = (s, x, y, k) => y - psi1(s, x) - psi2(s, x) * mNu;
W = (s) => NormalW.Sample();
Nu = (s) => NormalNu.Sample();
DW = dW;
DNu = dNu;
X0 = () => Exts.Vector(UniformW.Sample(), 0.0);
X0Hat = mEta;
DX0Hat = dEta;
}
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);
}
//////////////////////////////////////////////////////////////////////////
#endregion
#region Handlers
//////////////////////////////////////////////////////////////////////////
//HANDLERS
//////////////////////////////////////////////////////////////////////////
void Start()
{
GameObject _ArcContainer = new GameObject("Arc_Container");
_ArcContainer.transform.SetParent(transform);
for (int i = 0; i < m_Arcs; i++)
{
GameObject _Arc = new GameObject("Lightning_Arc_" + i.ToString());
_Arc.transform.SetParent(_ArcContainer.transform);
_Arc.AddComponent <LineRenderer>();
_Arc.AddComponent <LightningArc>();
_Arc.GetComponent <LightningArc>().SetTransforms(m_StartPoint, m_EndPoint);
_Arc.GetComponent <LightningArc>().SetMaterialColor(m_Material, new Color(
Mathf.Abs(Random.Range(m_MeanColor.r * (1.0f - m_ColorOffsetPercentage), m_MeanColor.r * (1.0f + m_ColorOffsetPercentage))),
Mathf.Abs(Random.Range(m_MeanColor.g * (1.0f - m_ColorOffsetPercentage), m_MeanColor.g * (1.0f + m_ColorOffsetPercentage))),
Mathf.Abs(Random.Range(m_MeanColor.b * (1.0f - m_ColorOffsetPercentage), m_MeanColor.b * (1.0f + m_ColorOffsetPercentage)))));
_Arc.GetComponent <LightningArc>().SetMode(m_Mode);
_Arc.GetComponent <LightningArc>().SetParameters(
Random.Range(m_MinSegments, m_MaxSegments + 1),
Random.Range(m_MeanWidth * (1.0f - m_WidthOffsetPercentage), m_MeanWidth * (1.0f + m_WidthOffsetPercentage)),
RandomVector.Range <Vector3>(m_MeanAmplitude * (1.0f - m_AmplitudeOffsetPercentage), m_MeanAmplitude * (1.0f + m_AmplitudeOffsetPercentage)),
Random.Range(m_MeanFrequency * (1.0f - m_FrequencyOffsetPercentage), m_MeanFrequency * (1.0f + m_FrequencyOffsetPercentage)));
_Arc.GetComponent <LightningArc>().Begin();
}
GameObject _AudioContainer = new GameObject("Audio_Container");
_AudioContainer.transform.SetParent(transform);
for (int i = 0; i < m_Sounds.Count; i++)
{
if (m_Sounds[i] && m_Volumes[i] > 0.0f)
{
GameObject _AS = new GameObject("AudioSource_" + i.ToString());
_AS.transform.SetParent(_AudioContainer.transform);
_AS.AddComponent <AudioSource>();
_AS.GetComponent <AudioSource>().clip = m_Sounds[i];
_AS.GetComponent <AudioSource>().volume = m_Volumes[i];
_AS.GetComponent <AudioSource>().pitch = m_Pitches[i];
_AS.GetComponent <AudioSource>().loop = true;
_AS.GetComponent <AudioSource>().maxDistance = Vector3.Distance(m_StartPoint.position, m_EndPoint.position) * 0.5f;
_AS.GetComponent <AudioSource>().Play();
}
}
}
static void Run(Options o, string[] args)
{
if (string.IsNullOrWhiteSpace(o.ScriptsFolder))
{
o.ScriptsFolder = "..\\..\\..\\OutputScripts\\";
}
Directory.CreateDirectory(o.OutputFolder);
using (System.IO.StreamWriter outputfile = new System.IO.StreamWriter(Path.Combine(o.OutputFolder, "parameters.txt"), true))
{
outputfile.WriteLine($"{DateTime.Now}\t{string.Join(" ", args)}");
outputfile.Close();
}
// original continuous model
// x_t = x_0 + \int_0^t Phi_1(x_t) dt + \int_0^t Phi_2(x_t) dW_t
// discrete model:
// x_{t+1} = Phi_1(x_t) + Phi_2(x_t) W_t
//observations
// y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t
#region 3d model Ienkaran Arasaratnam, Simon Haykin, and Tom R. Hurd
//mpdel params
//double sigma1 = Math.Sqrt(0.2);
//double sigma2 = 7.0 * 1e-3;
//double sigma_r = 50;
//double sigma_th = 0.1;
//double sigma_ph = 0.1;
// starting point
//Vector<double> mEta = Exts.Vector(1000, 0, 2650, 150, 200, 0, 1.0);
//Matrix<double> dEta = Exts.Diag(1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0);
//RandomVector<Normal> NormalEta = new RandomVector<Normal>(mEta, dEta);
//Func<Vector<double>> X0;
////X0 = () => NormalEta.Sample();
//X0 = () => mEta;
// dynamics
//Func<Vector<double>, Vector<double>> Phi1 = (x) => Exts.Vector(x[1], -x[6] * x[3], x[3], x[6] * x[1], x[5], 0, 0);
//Func<Matrix<double>> Phi2 = () => Exts.Diag(1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0);
//Vector<double> mW = Exts.Vector(0, 0, 0, 0, 0, 0, 0);
//Matrix<double> dW = Exts.Diag(1e1, Math.Pow(sigma1, 2), 1e1, Math.Pow(sigma1, 2), 1e1, Math.Pow(sigma1, 2), Math.Pow(sigma2, 2));
//RandomVector<Normal> NormalW = new RandomVector<Normal>(mW, dW);
//Func<Vector<double>> W;
//W = () => NormalW.Sample();
// observations
//Func<Vector<double>, Vector<double>> Psi1 = (x) => Utils.cart2sphere(Exts.Vector(x[0], x[2], x[4]));
//Func<Matrix<double>> Psi2 = () => Exts.Diag(1.0, 1.0, 1.0);
//Vector<double> mNu = Exts.Vector(0, 0, 0);
//Matrix<double> dNu = Exts.Diag(Math.Pow(sigma_r, 2), Math.Pow(sigma_th, 2), Math.Pow(sigma_ph, 2));
////Vector<double> X_R2 = Exts.Vector(0, 0);
////Vector<double> mNu = Exts.Vector(0, 0, 0, 0);
////Matrix<double> dNu = Exts.Diag(Math.Pow(0.1 * Math.PI / 180, 2), Math.Pow(50, 2), Math.Pow(0.1 * Math.PI / 180, 2), Math.Pow(50, 2));
////Func<double, Vector<double>, Vector<double>> Psi1 = (s, x) => Exts.Stack(Utils.cart2pol(Exts.Vector(x[0], x[1]) - X_R1), Utils.cart2pol(Exts.Vector(x[0], x[1]) - X_R2));
////Func<double, Vector<double>, Matrix<double>> Psi2 = (s, x) => Exts.Diag(1.0, 1.0, 1.0, 1.0);
//RandomVector<Normal> NormalNu = new RandomVector<Normal>(mNu, dNu);
//Func<Vector<double>> Nu;
//Nu = () => NormalNu.Sample();
//double h_state = 0.01;
//double h_obs = 0.01;
//double T = 1.0 + h_state / 2;
#endregion
#region 2d model with acceleration
// model params
double Alpha_n = o.alpha; // 0.05;
double Beta_n = o.beta; //1.0;
double Gamma_n = o.gamma; //0.5;
// starting point
Vector <double> mEta = Exts.Vector(0, 25000, 400, 10 * Math.PI / 180, Gamma_n / Alpha_n);
Matrix <double> dEta = Exts.Diag(Math.Pow(o.DX0, 2), Math.Pow(o.DX0, 2), Math.Pow(115, 2), Math.Pow(15 * Math.PI / 180, 2), Math.Pow(Beta_n, 2) / 2 / Alpha_n);
RandomVector <Normal> NormalEta = new RandomVector <Normal>(mEta, dEta);
ContinuousUniform UniformEtaV = new ContinuousUniform(200, 600);
Func <Vector <double> > X0;
X0 = () =>
{
var x = NormalEta.Sample();
x[2] = UniformEtaV.Sample();
return(x);
};
// dynamics
Func <Vector <double>, Vector <double> > Phi1 = (x) => Exts.Vector(x[2] * Math.Cos(x[3]), x[2] * Math.Sin(x[3]), 0, x[4] / x[2], -Alpha_n * x[4] + Gamma_n);
Func <Matrix <double> > Phi2 = () => Exts.Diag(0, 0, 0, 0, 1.0);
Vector <double> mW = Exts.Vector(0, 0, 0, 0, 0);
Matrix <double> dW = Exts.Diag(0, 0, 0, 0, Math.Pow(Beta_n, 2));
Normal NormalW = new Normal(mW[4], Math.Sqrt(dW[4, 4]));
Func <Vector <double> > W;
W = () => Exts.Vector(0, 0, 0, 0, NormalW.Sample());
// observations
Vector <double> X_R1 = Exts.Vector(-10000, 10000); // first radar location
Vector <double> X_R2 = Exts.Vector(0, 0); // second radar location
Func <Vector <double>, Vector <double> > Psi1 = (x) => Exts.Stack(Utils.cart2pol(Exts.Vector(x[0], x[1]) - X_R1), Utils.cart2pol(Exts.Vector(x[0], x[1]) - X_R2));
Func <Matrix <double> > Psi2 = () => Exts.Diag(1.0, 1.0, 1.0, 1.0);
Vector <double> mNu = Exts.Vector(0, 0, 0, 0);
Matrix <double> dNu = Exts.Diag(Math.Pow(0.1 * Math.PI / 180, 2), Math.Pow(50, 2), Math.Pow(0.1 * Math.PI / 180, 2), Math.Pow(50, 2));
RandomVector <Normal> NormalNu = new RandomVector <Normal>(mNu, dNu);
Func <Vector <double> > Nu;
Nu = () => NormalNu.Sample();
//discretization
double h_state = o.h_state; //0.01;
double h_obs = o.h_obs; //1.0;
double T = o.T + h_state / 2;
Func <int, Vector <double>, Vector <double> > Phi1_discr = (i, x) =>
{
Vector <double> xx = x;
for (double s = h_state; s < h_obs + h_state / 2; s += h_state)
{
xx += h_state * Phi1(xx);
}
return(xx);
};
Func <int, Vector <double>, Matrix <double> > Phi2_discr = (i, x) => Math.Sqrt(h_obs) * Phi2();
Func <int, Vector <double>, Vector <double> > Psi1_discr = (i, x) => Psi1(x);
Func <int, Vector <double>, Matrix <double> > Psi2_discr = (i, x) => Psi2();
// derivatives for the Extended Kalman Filter
Func <Vector <double>, Matrix <double> > dPhi = (x) => Matrix <double> .Build.Dense(5, 5, new double[5 * 5] {
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
Math.Cos(x[3]), Math.Sin(x[3]), 0, -x[4] / Math.Pow(x[2], 2), 0,
-x[2] * Math.Sin(x[3]), x[2] * Math.Cos(x[3]), 0, 0, 0,
0, 0, 0, 1.0 / x[2], -Alpha_n
}); // Column-major order
Func <Vector <double>, Matrix <double> > dPsi = (x) =>
{
var x1 = x - X_R1.Stack(Exts.Vector(0, 0, 0));
var r1r1 = x1[0] * x1[0] + x1[1] * x1[1];
var r1 = Math.Sqrt(r1r1);
var x2 = x - X_R2.Stack(Exts.Vector(0, 0, 0));
var r2r2 = x2[0] * x2[0] + x2[1] * x2[1];
var r2 = Math.Sqrt(r2r2);
return(Matrix <double> .Build.Dense(4, 5, new double[5 * 4] {
-x1[1] / r1r1, x1[0] / r1, -x2[1] / r2r2, x2[0] / r2,
x1[0] / r1r1, x1[1] / r1, x2[0] / r2r2, x2[1] / r2,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0
})); // Column-major order
};
Func <int, Vector <double>, Matrix <double>, (Vector <double>, Matrix <double>)> Ricatti = (i, x, P) =>
{
Vector <double> xx = x;
Matrix <double> PP = P;
for (double s = h_state; s < h_obs + h_state / 2; s += h_state)
{
PP += h_state * (dPhi(xx) * PP + PP * dPhi(xx).Transpose() + Phi2() * dW * Phi2().Transpose());
xx += h_state * (Phi1(xx) + Phi2() * mW);
}
return(xx, PP);
};
//trivial estimate
Func <int, Vector <double>, Vector <double>, Matrix <double>, (Vector <double>, Matrix <double>)> DummyEstimate = (i, y, x, P) =>
{
Vector <double> xx = x;
Matrix <double> PP = P;
for (double s = h_state; s < h_obs + h_state / 2; s += h_state)
{
PP += h_state * (dPhi(xx) * PP + PP * dPhi(xx).Transpose() + Phi2() * dW * Phi2().Transpose());
xx += h_state * (Phi1(xx) + Phi2() * mW);
}
return((0.5 * (Utils.pol2cart(Exts.Vector(y[0], y[1])) + X_R1 + Utils.pol2cart(Exts.Vector(y[2], y[3])) + X_R2)).Stack(Exts.Vector(xx[2], xx[3], xx[4])), PP);
};
#endregion
int N = (int)(T / h_obs);
Func <DiscreteVectorModel> ModelGenerator = () =>
{
DiscreteVectorModel model = null;
int n = 0;
double h_tolerance = h_state / 2.0;
double t_nextobservation = h_obs;
Vector <double> State = X0();
Vector <double> Obs;
model = new DiscreteVectorModel(Phi1_discr, Phi2_discr, Psi1_discr, Psi2_discr, (i) => W(), (i) => Nu(), X0(), true);
//for (double s = 0; s < T; s += h_obs)
//{
// model.Step();
//}
for (double s = h_state; s < T; s += h_state)
{
if (s > 0)
{
State = State + h_state * Phi1(State) + Math.Sqrt(h_state) * Phi2() * W();
}
if (Math.Abs(s - t_nextobservation) < h_tolerance)
{
Obs = Psi1(State) + Psi2() * Nu();
t_nextobservation += h_obs;
n++;
model.Trajectory.Add(n, new Vector <double>[] { State, Obs });
}
}
return(model);
};
// filter params file names
string CMNFFileName = Path.Combine(o.OutputFolder, "cmnf.params");
if (!string.IsNullOrWhiteSpace(o.CMNFFileName))
{
CMNFFileName = o.CMNFFileName;
}
string BCMNFFileName = Path.Combine(o.OutputFolder, "bcmnf.params");
if (!string.IsNullOrWhiteSpace(o.BCMNFFileName))
{
BCMNFFileName = o.BCMNFFileName;
}
string UKFFileName = Path.Combine(o.OutputFolder, "ukf.params");
if (!string.IsNullOrWhiteSpace(o.UKFFileName))
{
UKFFileName = o.UKFFileName;
}
string UKFOptStepwiseNMFileName = Path.Combine(o.OutputFolder, "ukfoptstepwiseNM.params");
if (!string.IsNullOrWhiteSpace(o.UKFStepwiseNelderMeadFileName))
{
UKFOptStepwiseNMFileName = o.UKFStepwiseNelderMeadFileName;
}
string UKFOptIntegralNMFileName = Path.Combine(o.OutputFolder, "ukfoptintegralNM.params");
if (!string.IsNullOrWhiteSpace(o.UKFIntegralNelderMeadFileName))
{
UKFOptIntegralNMFileName = o.UKFIntegralNelderMeadFileName;
}
string UKFOptStepwiseRandFileName = Path.Combine(o.OutputFolder, "ukfoptstepwiserand.params");
if (!string.IsNullOrWhiteSpace(o.UKFStepwiseRandomShootFileName))
{
UKFOptStepwiseRandFileName = o.UKFStepwiseRandomShootFileName;
}
string UKFOptIntegralRandFileName = Path.Combine(o.OutputFolder, "ukfoptintegralrand.params");
if (!string.IsNullOrWhiteSpace(o.UKFIntegralRandomShootFileName))
{
UKFOptIntegralRandFileName = o.UKFIntegralRandomShootFileName;
}
// filters
List <(FilterType, string)> filters = new List <(FilterType, string)>();
if (o.CMNF)
{
filters.Add((FilterType.CMNF, CMNFFileName));
}
if (o.BCMNF)
{
filters.Add((FilterType.BCMNF, BCMNFFileName));
}
if (o.MCMNF)
{
filters.Add((FilterType.MCMNF, string.Empty));
}
if (o.UKF)
{
filters.Add((FilterType.UKFNoOptimization, UKFFileName));
}
if (o.UKFStepwiseNelderMead)
{
filters.Add((FilterType.UKFStepwise, UKFOptStepwiseNMFileName));
}
if (o.UKFIntegralNelderMead)
{
filters.Add((FilterType.UKFIntegral, UKFOptIntegralNMFileName));
}
if (o.UKFStepwiseRandomShoot)
{
filters.Add((FilterType.UKFStepwiseRandomShoot, UKFOptStepwiseRandFileName));
}
if (o.UKFIntegralRandomShoot)
{
filters.Add((FilterType.UKFIntegralRandomShoot, UKFOptIntegralRandFileName));
}
if (o.EKF)
{
filters.Add((FilterType.EKF, string.Empty));
}
if (o.Dummy)
{
filters.Add((FilterType.Dummy, string.Empty));
}
// test environment
TestEnvironmentVector testEnv = new TestEnvironmentVector()
{
TestName = "Target tracking",
TestFileName = "TargetTracking",
Phi1 = Phi1_discr,
Phi2 = Phi2_discr,
Psi1 = Psi1_discr,
Psi2 = Psi2_discr,
dPhi = (i, x) => dPhi(x),
dPsi = (i, x) => dPsi(x),
Xi = (i, x) => Phi1_discr(i, x) + Phi2_discr(i, x) * mW,
//Zeta = (i, x, y, k) => (y - Psi1_discr(i, x) - Psi2_discr(i, x) * mNu).Stack(Utils.pol2cart(Exts.Vector(y[0], y[1]))+X_R1).Stack(Utils.pol2cart(Exts.Vector(y[2], y[3]))+X_R2),
Zeta = (i, x, y, k) => (y - Psi1_discr(i, x) - Psi2_discr(i, x) * mNu),
Alpha = (i, x) => Phi1_discr(i, x) + Phi2_discr(i, x) * mW,
Gamma = (i, x, y) => (y).Stack(Utils.pol2cart(Exts.Vector(y[0], y[1])) + X_R1).Stack(Utils.pol2cart(Exts.Vector(y[2], y[3])) + X_R2),
//Gamma = (i, x, y) => y - Psi1_discr(i, x) - Psi2_discr(i, x) * mNu,
nMCMNF = o.MCMNFTrainCount,
W = (i) => W(),
Nu = (i) => Nu(),
DW = dW,
DNu = dNu,
X0 = () => X0(),
X0Hat = mEta,
DX0Hat = dEta,
Predict = Ricatti,
DummyEstimate = DummyEstimate,
ModelGenerator = ModelGenerator
};
if (o.Bulk)
{
testEnv.GenerateBundleSamples(o.T, o.TrainCount, o.OutputFolder);
}
else
{
testEnv.Initialize(o.T, o.TrainCount, o.OutputFolder, filters, o.Save, o.Load);
if (o.Sift)
{
testEnv.Sifter = (x) => Math.Sqrt(x[0] * x[0] + x[1] * x[1]) > o.SiftBound;
}
if (o.Aggregate)
{
testEnv.Aggregate(o.OutputFolder, o.OutputFolder, !o.NoBin, !o.NoText);
}
if (!o.Skip)
{
if (o.SamplesCount == 0)
{
testEnv.GenerateBundles(o.BundleCount, o.TestCount, o.OutputFolder, o.Parallel, o.ParallelismDegree, !o.NoBin, !o.NoText);
if (!o.NoPython)
{
testEnv.RunScript(Path.Combine(o.ScriptsFolder, "estimate_statistics.py"), o.OutputFolder);
}
}
else
{
if (o.SamplesCount == 1)
{
testEnv.GenerateOne(o.OutputFolder);
if (!o.NoPython)
{
testEnv.RunScript(Path.Combine(o.ScriptsFolder, "estimate_sample.py"), o.OutputFolder);
testEnv.RunScript(Path.Combine(o.ScriptsFolder, "trajectory.py"), o.OutputFolder);
}
}
else
{
for (int i = 0; i < o.SamplesCount; i++)
{
testEnv.GenerateOne(o.OutputFolder, i);
}
}
}
}
}
}
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);
}
