/// <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)); }