public void SetUp()
{
this.accDistType = new Mock <Normal>(1);
this.oriDistType = new Mock <Normal>(1);
this.source = new IMUSource(this.accDistType.Object, this.oriDistType.Object, this.bufferSize, new Vector3(new float[] { 0, 0, 0 }));
this.zeroOrientation = new Vector3(0, 0, 0);
this.standardAcceleration = new Vector3(1, 2, 3);
}
public void GetLastVelocityNoDataTest()
{
Vector3 v0 = new Vector3(1, 2, 3);
this.source = new IMUSource(this.accDistType.Object, this.oriDistType.Object, 5, new Vector3(0, 0, 0), v0);
Measurement <Vector3> res = this.source.GetLastVelocity();
this.AssertVectorAreEqual(v0, res.Data);
}
public void VelocityCorrectStdTest()
{
this.source = new IMUSource(new Normal(1), new Normal(1), 5);
Vector3 acc0 = new Vector3(0, 0, 0);
Vector3 acc1 = new Vector3(1, 2, 3);
this.source.AddMeasurements(0, acc0, this.zeroOrientation);
this.source.AddMeasurements(1000, acc1, this.zeroOrientation);
Measurement <Vector3> res = this.source.GetLastVelocity();
Assert.AreEqual(1, ((Normal)res.DistributionType).Stddev, 0.0001);
}
public void GetLastVelocityBufferLimitReached()
{
this.source = new IMUSource(this.accDistType.Object, this.oriDistType.Object, 3, new Vector3(0, 0, 0));
Vector3 v0 = new Vector3(0, 0, 0);
Vector3 v1 = new Vector3(1, 1, 1);
this.source.AddMeasurements(0, v0, this.zeroOrientation);
this.source.AddMeasurements(1000, v1, this.zeroOrientation);
this.source.AddMeasurements(2000, v0, this.zeroOrientation);
this.source.AddMeasurements(3000, v1, this.zeroOrientation);
this.source.AddMeasurements(4000, v0, this.zeroOrientation);
Measurement <Vector3> res = this.source.GetLastVelocity();
this.AssertVectorAreEqual(new Vector3(2, 2, 2), res.Data);
Assert.AreEqual(4000, res.TimeStamp);
}
public void GetLastOrientationBufferLimitReached()
{
this.source = new IMUSource(this.accDistType.Object, this.oriDistType.Object, 3, new Vector3(0, 0, 0));
Vector3 v0 = new Vector3(0, 0, 0);
Vector3 v1 = new Vector3(1, 1, 1);
Vector3 v2 = new Vector3(2, 2, 2);
Vector3 v3 = new Vector3(3, 3, 3);
Vector3 v4 = new Vector3(4, 4, 4);
this.source.AddMeasurements(0, v0, v0);
this.source.AddMeasurements(1, v0, v1);
this.source.AddMeasurements(2, v0, v2);
this.source.AddMeasurements(3, v0, v3);
this.source.AddMeasurements(4, v0, v4);
Measurement <Vector3> res = this.source.GetLastOrientation();
this.AssertVectorAreEqual(v4, res.Data);
Assert.AreEqual(4, res.TimeStamp);
}
public void TestGetDataClosestTo()
{
this.source = new IMUSource(this.accDistType.Object, this.oriDistType.Object, 10, new Vector3(new float[] { 0, 0, 0 }));
this.source.AddMeasurements(1, new Vector3(1, 1, 1), new Vector3(0, 0, 0));
this.source.AddMeasurements(1, new Vector3(1, 1, 1), new Vector3(0, 0, 0));
this.source.AddMeasurements(10, new Vector3(2, 2, 2), new Vector3(10, 10, 10));
this.source.AddMeasurements(10, new Vector3(2, 2, 2), new Vector3(10, 10, 10));
this.source.AddMeasurements(19, new Vector3(3, 3, 3), new Vector3(20, 20, 20));
this.source.AddMeasurements(19, new Vector3(3, 3, 3), new Vector3(20, 20, 20));
this.source.AddMeasurements(25, new Vector3(4, 4, 4), new Vector3(30, 30, 30));
this.source.AddMeasurements(25, new Vector3(4, 4, 4), new Vector3(30, 30, 30));
List <Measurement <Vector3> > resacc = this.source.GetAccelerationClosestTo(11, 10);
List <Measurement <Vector3> > resvel = this.source.GetVelocityClosestTo(15, 10);
List <Measurement <Vector3> > resori = this.source.GetOrientationClosestTo(21, 10);
Assert.AreEqual(new[] { 10, 10 }, resacc.Select <Measurement <Vector3>, float>(m => m.TimeStamp).ToArray());
Assert.AreEqual(new[] { 19, 19 }, resvel.Select <Measurement <Vector3>, float>(m => m.TimeStamp).ToArray());
Assert.AreEqual(new[] { 19, 19 }, resori.Select <Measurement <Vector3>, float>(m => m.TimeStamp).ToArray());
}
public void ZeroBufferSizeInitTest()
{
this.source = new IMUSource(this.accDistType.Object, this.oriDistType.Object, 0);
Assert.AreEqual(this.classDefaultBufferSize, this.source.GetMeasurementBufferSize());
}
/// <summary>
/// Method called when creating a UserController.
/// </summary>
public override void Init()
{
this.imuSource = new IMUSource(new Normal(this.accelerationStd), new Normal(this.orientationStd), this.bufferSize, new Vector3(-0.3213f, -9.81f + 0.0435f, -0.1763f));
}
