/// <summary>
/// Obtain a measurement from the hidden state.
/// This method always detects the landmark (misdetection
/// probability is ignored).
/// </summary>
/// <param name="landmark">Landmark 3d location against which the measurement is performed.</param>
/// <returns>Pixel-range measurements.</returns>
public MeasurementT MeasureDetected(double[] landmark)
{
MeasurementT measurement = Measurer.MeasurePerfect(Pose, landmark);
double[] mlinear = measurement.ToLinear();
double[] noise = Util.RandomGaussianVector(new double[mlinear.Length], MeasurementCovariance);
return(measurement.FromLinear(mlinear.Add(noise)));
}
/// <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 virtual void Update(GameTime time, double[] reading)
{
Pose = Pose.AddOdometry(reading);
OdometryPose = OdometryPose.AddOdometry(reading);
double[] noise = time.ElapsedGameTime.TotalSeconds.Multiply(
Util.RandomGaussianVector(new double[OdoSize],
MotionCovariance));
OdometryPose = OdometryPose.AddOdometry(noise);
WayPoints.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(Pose.State)));
}
/// <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));
}