public IEnumerable <KeyValuePair <DateTime, Tuple <double, double>[]> > BatchRun(IEnumerable <KeyValuePair <DateTime, double> > measurements)
{
//kfs = KalmanFilterWrapperFactory.BuildMany((1, 300), (1, 2)).ToArray();
//dfs = kfs.Select(_ => DiscreteFactory.Build(_.Size)).ToArray();
//List<Tuple<DateTime, Tuple<double[], double[]>>> u = new List<Tuple<DateTime, Tuple<double[],double[]>>>();
DateTime dt = measurements.First().Key;
List <Matrix <double> > nweights =
Enumerable.Range(0, Filters.Count()).Select(_ => Matrix <double> .Build.DenseOfRowArrays(new double[] { 0.00000000001d })).ToList();
foreach (var meas in measurements.ToMatrices())
{
TimeSpan ts = meas.Key - dt;
dt = meas.Key;
var prd = PredictWeighted(ts, dt, nweights);
var eval = Evaluate(prd).ToArray();
Update(ts, meas.Value);
var weights = GetDifferences(meas.Value);
nweights = Normaliser.Normalise(weights.ToList()).ToList();
yield return(new KeyValuePair <DateTime, Tuple <double, double>[]>(dt, eval));
}
}
public IObservable <KeyValuePair <DateTime, Tuple <double, double>[]> > Run(IObservable <KeyValuePair <DateTime, double?> > measurements)
{
//kfs = KalmanFilterWrapperFactory.BuildMany((1, 300), (1, 2)).ToArray();
//dfs = kfs.Select(_ => DiscreteFactory.Build(_.Size)).ToArray();
//List<Tuple<DateTime, Tuple<double[], double[]>>> u = new List<Tuple<DateTime, Tuple<double[],double[]>>>();
//DateTime dt = measurements.First().Item1;
List <Matrix <double> > nweights =
Enumerable.Range(0, Filters.Count()).Select(_ => Matrix <double> .Build.DenseOfRowArrays(new double[] { 0.00000000001d })).ToList();
return(measurements.IncrementalTimeOffsets().Select(meas =>
{
var prd = PredictWeighted(meas.Key.Item2, meas.Key.Item1, nweights);
var eval = Evaluate(prd).ToArray();
if (meas.Value != null)
{
var mtrx = meas.Value == null ? null : Matrix <double> .Build.DenseOfColumnArrays(new double[] { (double)meas.Value });
Update(meas.Key.Item2, mtrx);
var weights = GetDifferences(mtrx);
nweights = Normaliser.Normalise(weights.ToList()).ToList();
}
return new KeyValuePair <DateTime, Tuple <double, double>[]>(meas.Key.Item1, eval);
}));
}
private static Boolean IsNoOp(Normaliser normaliser)
{
Double actual = normaliser.Normalise <Linear>(1d);
Double expected = Math.Pow(10, normaliser.Exponent);
Assert.Equal(expected, actual);
return(true);
}
private static Boolean IsMultiply(Normaliser normaliser)
{
Double value = 2d;
Double actual = normaliser.Normalise <Linear>(value);
Double lower = Math.Pow(10, normaliser.Exponent + 1);
Double upper = Math.Pow(10, normaliser.Exponent + 4);
Assert.InRange(actual, lower, upper);
return(true);
}
//public void Update(double[] likelihood, double[] prior)
//{
// var posterior = prior * likelihood;
// return Normaliser.Normalise(posterior);
//}
public void Update(ref List <Particle> particles, Point robot)
{
foreach (var p in particles)
{
var dX = p.X - robot.X;
var dY = p.Y - robot.Y;
p.Weight = 1 / (Math.Sqrt(dX * dX + dY * dY));
}
particles = particles.Zip(Normaliser.Normalise(particles.Select(_ => _.Weight).ToList()), (a, b) => { a.Weight = b; return(a); }).ToList();
//particles = particles.Zip(Normaliser.Normalise(weights), (a, b) => { a.Weight = b; return a; }).ToList();
}