CSE 298 – Homework 4
Sonar Sensor Model:
1. In this problem, you will generate a sensor model for sonar measurements.
2. Assume that your sonar has a maximum range of 10 meters.
3. Write a function
r = DetectWall( robot, a_w, b_w )
that takes as an argument the robot struct and the slope/intercept (a_w,b_w) of the wall in the
world frame, and returns an ARRAY of range measurements from the sonars. In effect, this
function returns “ideal” sonar measurements. Note that the order of sonars should be from
[pi/15:pi/15:2*pi] in the robot frame. Also, only sonars that are within 10 meters of the wall
should return an appropriate value. If the true sonar range would be > 10 meters, you should
return a -1 in the respective array index.
4. We will now perturb the true wall measurements to model both random noise and outlier
a. Assume that the range measurements are corrupted with random zeromean Gaussian
noise with standard deviation = 0.01ρ, where ρ is the actual range to the target.
b. Further assume that 25% of the sensor measurements suffer from multipath, resulting
in a range measurement error of +20-50%. You may assume that these measurements
are uniformly distributed.
5. You are to implement a function rHat = FireSonar( r ) that:
a. Take the ARRAY of true ranges from DetectWall as input.
b. Tests to see if each range was affected by multi-path error. If so, the appropriate
random error is added to the actual range measurement and returned from the sensor
model as rHat.
c. Otherwise, the range measurement is corrupted with appropriate random Gaussian
noise and returned as rHat.
d. Under no circumstances should the range returned exceed the maximum detection
range of the sensor. For ranges >10 m, the sonar will return a value of -1 (indicating that
no target was detected).
6. Helpful functions:
Least Squares Line Fitting:
1. For this problem, we will be using the least-squares line fitter from page 20 of the Robust
Estimation lecture notes.
2. Implement the following functions in Matlab
[ a_r, b_r] = LS_Line( rho, alpha );
[ a_w, b_w ] = Transform_Line( robot, a_r, b_r );
LS_Line takes an array of range measurements rho and an array of bearing angles alpha and
returns the least-squares solution [a_r, b_r] that corresponds to the slope and y-intercept line
parameters in the robot frame. The function Transform_Line takes these parameters and robot
struct as inputs and transforms these to parameters in the world frame. This will be useful for
plotting lines in the world frame. NOTE: You may solve this using the solution in the notes or
pseudo-inverse operation. However, you MAY NOT solve this using a built-in Matlab LS function.
Least-Squares & RANSAC Wallfollowers:
1. You are to implement a least-squares wallfollower LS_WallFollower( kP, kD, y_wall ) based upon
the LS_Line function above. kP and kD correspond to the proportional and derivative gains for
your controller. This will control the relative position and orientation of your robot with respect
to the wall described in Figure 1. y_wall denotes the y-intercept of the wall. You may assume
that it is always parallel to the x-axis.
2. To regulate the position and orientation of the robot, you will implement the PD controller
discussed in class during the feedback control lectures. You will need to choose appropriate
gains for the controller design.
3. You will first estimate the relative position and orientation of the robot with respect to the wall.
These inputs will be fed to the PD Controller which in turn will generate an output (ω) that will
be used to control the orientation of the robot.
4. To estimate the position/orientation with respect to the wall, you will:
a. Determine the range from each sonar to the wall using DetectWall and pass the
appropriate range array to FireSonar where they will be corrupted with noise.
b. From the corrupted range measurements, form an estimate of the position and
orientation for the wall relative to the robot by employing your LS_Line function. From
this, you can infer the relative distance and orientation of the wall for use in your PD
controller. The output of your PD controller will yield ω. Since v is constant, you can
move the robot appropriately. Note that robot motion is “perfect”, and updates occur at
c. The initial robot position will be taken as input from the user mouse (see the ginput
function). The initial orientation will be generated at random from θ [-π/4, π/4].
5. Repeat steps 1-4 by implementing a RANSAC_WallFollower(kP,kV) that will filter outliers from the
a. Determine the maximum number of sample trials n_max that you need to run RANSAC to
ensure you have a 99.9% probability of choosing the correct line (you may assume a 25%
outlier rate, although this is technically not quite correct).
b. Use the subset of sonar measurements that yields the largest consensus set after n_max
trials to fit the wall. In your figures, sonar measurements that were used above should be
drawn green, while outliers are drawn red.
6. Also, for both the LS and RANSAC wall followers, redraw the new wall as shown in Figure 2 where
the actual wall (blue), RANSAC wall (green) and LS wall (red) are all shown.