/// <summary>
/// Apply the motion model to the vehicle. It corresponds to a
/// 3D odometry model following the equation:
///
/// x = x + q dx q* + N(0, Q)
/// o = dq o dq* + N(0, Q')
///
/// where q is the midrotation quaternion (halfway between the old and new orientations) and N(a, b) is a normal function
/// with mean 'a' and covariance matrix 'b'.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="reading">Odometry reading (dx, dy, dz, dpitch, dyaw, droll).</param>
public void UpdateNoisy(GameTime time, double[] reading)
{
Update(time, reading);
// no input, static friction makes the robot stay put (if there is any static friction)
if (!(PerfectStill && reading.IsEqual(0)))
{
double[] noise = time.ElapsedGameTime.TotalSeconds.Multiply(
Util.RandomGaussianVector(new double[OdoSize],
MotionCovariance));
Pose = Pose.AddOdometry(noise);
}
WayPoints[WayPoints.Count - 1] = Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(Pose.State));
}