public void FailSampleStatic(
[Values(2, 2, 2, 2, 5, 5, 5, 5)] int rowsOfM,
[Values(2, 2, 2, 2, 2, 2, 2, 2)] int columnsOfM,
[Values(3, 2, 2, 2, 6, 5, 5, 5)] int rowsOfV,
[Values(2, 3, 2, 2, 5, 6, 5, 5)] int columnsOfV,
[Values(2, 2, 3, 2, 2, 2, 3, 2)] int rowsOfK,
[Values(2, 2, 2, 3, 2, 2, 2, 3)] int columnsOfK)
{
Assert.Throws <ArgumentOutOfRangeException>(() => MatrixNormal.Sample(new Random(), MatrixLoader.GenerateRandomDenseMatrix(rowsOfM, columnsOfM), MatrixLoader.GenerateRandomDenseMatrix(rowsOfV, columnsOfV), MatrixLoader.GenerateRandomDenseMatrix(rowsOfK, columnsOfK)));
}
/// <summary>
/// Gets a random 2d error.
/// </summary>
/// <param name="sigmaSquared">The square of the sigma we want for our error.</param>
/// <returns></returns>
public (Vector3, Vector3) GetRandom2D(float sigmaSquared)
{
if (norm == null)
{
Matrix <double> m = Matrix <double> .Build.Dense(2, 1);
m[0, 0] = 0;
m[1, 0] = 0;
Matrix <double> v = Matrix <double> .Build.Dense(2, 2);
v[0, 0] = sigmaSquared;
v[0, 1] = 0;
v[1, 0] = 0;
v[1, 1] = sigmaSquared;
Matrix <double> k = Matrix <double> .Build.Dense(1, 1);
k[0, 0] = 1.0;
norm = new MatrixNormal(m, v, k, random);
}
Matrix <double> samp = norm.Sample();
Vector3 randVector = new Vector3((float)samp[0, 0], 0, (float)samp[1, 0]);
//We average the result of multiple subsequent points to smoothen the trajectories here.
Vector3 acc = Vector3.Zero;
for (int i = 0; i < interpolationRands.Length; i++)
{
if (i != interpolationRands.Length - 1)
{
interpolationRands[i] = interpolationRands[i + 1];
}
else
{
interpolationRands[i] = randVector;
}
acc += interpolationRands[i];
}
acc = acc / interpolationLength;
return(randVector, acc);
}
public void FailSampleStatic(int rowsOfM, int columnsOfM, int rowsOfV, int columnsOfV, int rowsOfK, int columnsOfK)
{
Assert.Throws <ArgumentOutOfRangeException>(() => MatrixNormal.Sample(new Random(), MatrixLoader.GenerateRandomDenseMatrix(rowsOfM, columnsOfM), MatrixLoader.GenerateRandomDenseMatrix(rowsOfV, columnsOfV), MatrixLoader.GenerateRandomDenseMatrix(rowsOfK, columnsOfK)));
}
public void CanSampleStatic(int n, int p)
{
MatrixNormal.Sample(new Random(), MatrixLoader.GenerateRandomDenseMatrix(n, p), MatrixLoader.GenerateRandomPositiveDefiniteDenseMatrix(n), MatrixLoader.GenerateRandomPositiveDefiniteDenseMatrix(p));
}
public void CanSample(int n, int p)
{
var d = new MatrixNormal(MatrixLoader.GenerateRandomDenseMatrix(n, p), MatrixLoader.GenerateRandomPositiveDefiniteDenseMatrix(n), MatrixLoader.GenerateRandomPositiveDefiniteDenseMatrix(p));
d.Sample();
}
public void FailSampleStatic(int rowsOfM, int columnsOfM, int rowsOfV, int columnsOfV, int rowsOfK, int columnsOfK)
{
Assert.That(() => MatrixNormal.Sample(new System.Random(0), Matrix <double> .Build.Random(rowsOfM, columnsOfM, 1), Matrix <double> .Build.Random(rowsOfV, columnsOfV, 1), Matrix <double> .Build.Random(rowsOfK, columnsOfK, 1)), Throws.ArgumentException);
}
public void CanSampleStatic(int n, int p)
{
MatrixNormal.Sample(new System.Random(0), Matrix <double> .Build.Random(n, p, 1), Matrix <double> .Build.RandomPositiveDefinite(n, 1), Matrix <double> .Build.RandomPositiveDefinite(p, 1));
}
public void CanSample(int n, int p)
{
var d = new MatrixNormal(Matrix <double> .Build.Random(n, p, 1), Matrix <double> .Build.RandomPositiveDefinite(n, 1), Matrix <double> .Build.RandomPositiveDefinite(p, 1));
d.Sample();
}
public void CanSample(int n, int p)
{
var d = new MatrixNormal(MatrixLoader.GenerateRandomDenseMatrix(n, p), MatrixLoader.GenerateRandomPositiveDefiniteDenseMatrix(n), MatrixLoader.GenerateRandomPositiveDefiniteDenseMatrix(p));
d.Sample();
}
//[Row(5, 2, 6, 5, 2, 2)]
//[Row(5, 2, 5, 6, 2, 2)]
//[Row(5, 2, 5, 5, 3, 2)]
//[Row(5, 2, 5, 5, 2, 3)]
public void FailSampleStatic(int mRows, int mCols, int vRows, int vCols, int kRows, int kCols)
{
var s = MatrixNormal.Sample(new Random(), MatrixLoader.GenerateRandomDenseMatrix(mRows, mCols), MatrixLoader.GenerateRandomDenseMatrix(vRows, vCols), MatrixLoader.GenerateRandomDenseMatrix(kRows, kCols));
}
/// <summary>
/// 测试
/// </summary>
void FormulaTest()//
{
List <Matrix <double> > C0 = new List <Matrix <double> >(), C1 = new List <Matrix <double> >();
List <Matrix <double> > Test0 = new List <Matrix <double> >(), Test1 = new List <Matrix <double> >();
var m2 = new DenseMatrix(new[, ] {
{ 1.0 }, { 0.5 }
});
var k2 = new DenseMatrix(new[, ] {
{ 1.0 }
});
var v2 = new DenseMatrix(new[, ] {
{ 3.0, 1.0 }, { 1.0, 2.0 }
});
var m1 = new DenseMatrix(new[, ] {
{ 0.0 }, { 0.0 }
});
var k1 = new DenseMatrix(new[, ] {
{ 1.0 }
});
var v1 = new DenseMatrix(new[, ] {
{ 1.0, 0.0 }, { 0.0, 1.0 }
});
ClassifiedData.Clear();
for (int i = 0; i < 50; i++)
{
C0.Add(MatrixNormal.Sample(new Random(), m2, v2, k2));
ClassifiedData.Add(new data(C0[0][0, 0], C0[0][1, 0], 0));
C1.Add(MatrixNormal.Sample(new Random(), m1, v1, k1));
ClassifiedData.Add(new data(C1[0][0, 0], C1[0][1, 0], 1));
}
for (int i = 0; i < 20; i++)
{
Test0.Add(MatrixNormal.Sample(new Random(), m2, v2, k2));
Test1.Add(MatrixNormal.Sample(new Random(), m1, v1, k1));
TestData.Add(new data(Test0[0][0, 0], Test0[0][1, 0], 0));
C1.Add(MatrixNormal.Sample(new Random(), m1, v1, k1));
TestData.Add(new data(Test1[0][0, 0], Test1[0][1, 0], 1));
}
int indicator = 0;
int endNetworks = 50;
int startNetworks = 1;
//int seed = 0;
do
{
indicator = 0;
for (int ii = startNetworks; ii <= endNetworks; ii++)
{
Jobs ajob = new Jobs(ii, 10);
/// 新建一个神经网络
NeuronNetworks nn = new NeuronNetworks(ajob.m);//, new Random(DateTime.Now.Millisecond));
/// 当前的迭代次数
nn.currentLearnRepeats = 0;
DateTime _start = DateTime.Now;
while (true)
//(++nn.currentLearnRepeats < nn.maxLearnRepeats)
{
++nn.currentLearnRepeats;
foreach (data d in ClassifiedData)
{
nn.Train(d.input1, d.input2, d.output);
}
//if (nn.currentLearnRepeats % ajob.testpoint == 0) // recheck whether it is converge.
{
double errorsum = 0;
for (int i = 0; i < ClassifiedData.Count; i++)
{
double estimateOut = nn.Display(ClassifiedData[i].input1, ClassifiedData[i].input2) - ClassifiedData[i].output;
errorsum += (estimateOut * estimateOut) / 2;
}
if (errorsum < nn.ε)
{
break;
}
if ((DateTime.Now - _start).Seconds > ajob.timeout)//timeout
{
Console.WriteLine(DateTime.Now - _start);
Console.WriteLine("Request timeout.");
break;
}
}
}
double sum = 0;
foreach (data d in TestData)
{
double t = (nn.Display(d.input1, d.input2) - d.output);
//Console.WriteLine("{0},{1}->{2}", d.input1, d.input2, nn.Display(d.input1, d.input2));
sum += t * t;
}
//Console.WriteLine(DateTime.Now - _start);未知样本的总差值=
if (sum > 10)
{
indicator++;
}
//Console.WriteLine("Task: m={0},testpoint={1};", ajob.m, ajob.testpoint);
//Console.WriteLine("开始计算差值");
//Console.WriteLine(nn.currentLearnRepeats);
sum = 0;
foreach (data d in ClassifiedData)
{
double t = (nn.Display(d.input1, d.input2) - d.output);
//Console.WriteLine("{0},{1}->{2}", d.input1, d.input2, nn.Display(d.input1, d.input2));
sum += t * t;
}
//Console.WriteLine(DateTime.Now - _start);样本训练后的总差值=
Console.Write("{0}\t", sum);
sum = 0;
foreach (data d in TestData)
{
double t = (nn.Display(d.input1, d.input2) - d.output);
//Console.WriteLine("{0},{1}->{2}", d.input1, d.input2, nn.Display(d.input1, d.input2));
//if (t > 0.7) t = 1;
//else t = 0;
sum += t * t;
}
//Console.WriteLine(DateTime.Now - _start);未知样本的总差值=
Console.WriteLine("{0}", sum);
//if (sum < 10) Console.ReadKey();
if (indicator >= (endNetworks - startNetworks) * 0.9)
{
Console.Clear();
}
}
} while (indicator > 5);
Console.WriteLine("Press any key to exit.");
if (indicator < 5)
{
Console.ReadKey();
}
}
void Fast_Update(double deltaDis, double deltaAng)
{
if (deltaDis == 0)
{
//Ground Truth with noise
StatesUpdate(ref X_rob_actual, deltaDis + deltaDis * transNoiseFactor * normal.Sample(),
deltaAng + deltaDis * Deg2Rad * angNoiseFactor * normal.Sample());
//Paticles are sampled from the proposal distribution
for (int i = 0; i < X_particles.Length; i++)
{
//Motion model
StatesUpdate(ref X_particles[i], deltaDis + deltaDis * transNoiseFactor * normal.Sample(),
deltaAng + deltaDis * Deg2Rad * angNoiseFactor * normal.Sample());
}
fastVisualizer.Visualize(X_particles, X_rob_actual);
return;
}
//Ground Truth with noise
StatesUpdate(ref X_rob_actual, deltaDis + deltaDis * transNoiseFactor * normal.Sample(),
deltaAng + deltaDis * Deg2Rad * angNoiseFactor * normal.Sample());
//Observation from ground truth
observedLandmarks = ObserveLandmarks(X_rob_actual, rangeNoiseFactor, bearingNoiseFactor);
//Paticles are sampled from the proposal distribution
for (int i = 0; i < X_particles.Length; i++)
{
//Motion model
StatesUpdate(ref X_particles[i], deltaDis + deltaDis * transNoiseFactor * normal.Sample(),
deltaAng + deltaDis * Deg2Rad * angNoiseFactor * normal.Sample());
Matrix <double> r = RotationMatrix(X_particles[i]);
//Initialize robot mean, cov, observation cov
Matrix <double> s = r * Q * r.Transpose();
Vector <double> m = X_particles[i];
Matrix <double> Qt = R;
Vector <double> X_old = m;
Matrix <double> P_old = s;
//Debug.Log("Particle " + i + " old: " + X_old.ToString());
#region Observation_update
//Observation update
//Interatively refine proposal distribution with observation
foreach (var m_obs in observedLandmarks)
{
int worldId = m_obs.Key;
if (!worldToLocalDictionary.ContainsKey(worldId))
{
continue;
}
Vector <double> z = m_obs.Value;
Vector <double> z_pred = ObervationEstimate(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]);
Vector <double> z_diff = z - z_pred;
z_diff[1] = ClampRad(z_diff[1]);
//Debug.Log("Measure measurement:" +z_diff.L2Norm());
Matrix <double> hx = Hx(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]); //2x3
Matrix <double> hm = Hm(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]); //2x2
Qt += hm * R * hm.Transpose(); //2x2
s = (hx.Transpose() * Qt.Inverse() * hx + s.Inverse()).Inverse(); //3x3
m += s * hx.Transpose() * Qt.Inverse() * z_diff; //3x1
}
//Sample from proposal distribution
MatrixNormal mn = new MatrixNormal(m.ToColumnMatrix(), s, Matrix <double> .Build.DenseIdentity(1, 1));
Matrix <double> sample = mn.Sample();
Vector <double> X_new = Vector <double> .Build.DenseOfArray(new double[3] {
sample[0, 0], sample[1, 0], sample[2, 0]
});
//Debug.Log("Mean " + i + " : " + m.ToString());
//Debug.Log("X_actual " + i + " : " + X_rob_actual.ToString());
//Update particle
X_particles[i] = X_new;
X_particles[i][2] = ClampRad(X_particles[i][2]);
#endregion
#region weight_update
//Compute sample weight from new sampled particles
//compute likelihood p(z|x)
double likelihood = 1;
foreach (var m_obs in observedLandmarks)
{
int worldId = m_obs.Key;
if (!worldToLocalDictionary.ContainsKey(worldId))
{
continue;
}
Vector <double> z = m_obs.Value;
Vector <double> z_pred = ObervationEstimate(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]);
Matrix <double> hm = Hm(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]); //2x2
Matrix <double> hx = Hx(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]); //2x3
//Independent
likelihood *= Gaussian_evaluation(z, z_pred, hm * R * hm.Transpose());
}
//Debug.Log(i + " likelihood: " + likelihood);
double prior = Gaussian_evaluation(X_new, X_old, P_old);
//Debug.Log(i + " prior: " + prior);
double prop = Gaussian_evaluation(X_new, m, s);
//Debug.Log(i + " prop: " + prop);
//Update weight
X_weight[i] *= likelihood * prior / (prop + 1e-5);
//Debug.Log("X weight: " + X_weight[i]);
#endregion
#region ekf_update
//EKF update for each associated landmarks
foreach (var m_obs in observedLandmarks)
{
int worldId = m_obs.Key;
if (!worldToLocalDictionary.ContainsKey(worldId))
{
continue;
}
Vector <double> z = m_obs.Value;
Vector <double> z_pred = ObervationEstimate(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]);
Vector <double> z_diff = z - z_pred;
z_diff[1] = ClampRad(z_diff[1]);
Matrix <double> hm = Hm(X_particles[i], X_m[i][worldToLocalDictionary[worldId]]); //2x2
Matrix <double> inov = hm * P_m[i][worldToLocalDictionary[worldId]] * hm.Transpose() + R; //2x2
//Kalman gain
Matrix <double> K = P_m[i][worldToLocalDictionary[worldId]] * hm.Transpose() * inov.Inverse();
Vector <double> X_inc = K * z_diff;
X_m[i][worldToLocalDictionary[worldId]] += X_inc;
P_m[i][worldToLocalDictionary[worldId]] = (Matrix <double> .Build.DenseIdentity(2, 2) - K * hm) * P_m[i][worldToLocalDictionary[worldId]];
}
#endregion
}
//Normalize
double count = 0;
double[] cdf = new double[numOfParticles];
double sum = X_weight.Sum();
//Debug.Log("Sum: " + sum);
for (int i = 0; i < X_particles.Length; i++)
{
X_weight[i] = X_weight[i] / sum;
count += X_weight[i];
cdf[i] = count;
}
fastVisualizer.Visualize(X_particles, X_rob_actual);
//Resample
Vector <double>[] new_X_particles = new Vector <double> [numOfParticles];
List <Vector <double> >[] new_X_m = new List <Vector <double> > [numOfParticles];
List <Matrix <double> >[] new_P_m = new List <Matrix <double> > [numOfParticles];
for (int i = 0; i < X_particles.Length; i++)
{
int idx = GetCDFIndex(cdf);
new_X_particles[i] = X_particles[idx].Clone();
new_X_m[i] = new List <Vector <double> >(X_m[idx]);
new_P_m[i] = new List <Matrix <double> >(P_m[idx]);
X_weight[i] = 1 / (double)numOfParticles;
}
X_particles = new_X_particles;
X_m = new_X_m;
P_m = new_P_m;
//Loop through all landmarks to check if new landmarks are observed
foreach (var m_obs in observedLandmarks)
{
int world_id = m_obs.Key;
//If it is a new landmark
if (!worldToLocalDictionary.ContainsKey(world_id))
{
// Register the new landmark
worldToLocalDictionary.Add(world_id, robotLandmarkIndex);
robotLandmarkIndex++;
for (int i = 0; i < X_particles.Length; i++)
{
//Debug.Log(X_m[i].ToString());
X_m[i].Add(Vector <double> .Build.DenseOfArray(new double[]
{ X_particles[i][0] + m_obs.Value[0] * Math.Cos(X_particles[i][2] + m_obs.Value[1]),
X_particles[i][1] + m_obs.Value[0] * Math.Sin(X_particles[i][2] + m_obs.Value[1]) }));
Matrix <double> Jz = Matrix <double> .Build.DenseOfArray(new double[2, 2]
{
{ Math.Cos(X_particles[i][2] + m_obs.Value[1]), -m_obs.Value[0] * Math.Sin(X_particles[i][2] + m_obs.Value[1]) },
{ Math.Sin(X_particles[i][2] + m_obs.Value[1]), m_obs.Value[0] * Math.Cos(X_particles[i][2] + m_obs.Value[1]) }
});
P_m[i].Add(Jz * R * Jz.Transpose());
fastVisualizer.VisualizeLandmarkRegisteration(i, X_particles[i][0] + m_obs.Value[0] * Math.Cos(X_particles[i][2] + m_obs.Value[1]),
X_particles[i][1] + m_obs.Value[0] * Math.Sin(X_particles[i][2] + m_obs.Value[1]));
}
}
}
fastVisualizer.Visualize(X_particles, X_rob_actual);
fastVisualizer.VisualizeMeasurement(observedLandmarks);
fastVisualizer.VisualizeLandmarkParticles(X_m);
}
