...
- Camera measurement error correction. DONE (See #CAMERA MEASUREMENT ERROR CORRECTION)
- Steer input - curvature radius identification. DONE (See #STEER STEER INPUT - CURVATURE RADIUS RELATIONSHIP)
- Steer controller design. WIP
...
- Set the variable RECORD_OBJECT_DATA = 1 in the file CPS.h in the computer responsible for the camera that you want to configure and compile it. For information about compiling the CPS, check the original lab documentation.
- Run CPS.exe. The RECORD_OBJECT_DATA mode was designed to take pictures of the cars symbols so it will ask you the car number and the section number, just put a negative number. The only thing you must insert correctly is the camera number that you want to configure.
- Determine the four points to record. Considering the error trend, the four points should be chosen to form the broadest rectangle of interest, that is the rectangle with the biggest area contained in the camera view where the car can be tracked. To clarify, an example of how to choose the points is found in point_selection.png. Walls, obstacles and the end of the sections limit the rectangle. The console gives you information about the pixel coordinates of the top left corner of the small squared boxes, in the figure it is located close to (x2, y2). You can move that box with keys "a", "s", "d" and "w". Write down the pixel coordinates of those four points.
- Now measure the global position of those four points with a measure tape and record them. You should now have written down 4 pixel coordinates (x1, y1, x2, y2) and 8 global coordinates, 2 for each point of the rectangle. Pay attention when writing down the global coordinates. Since the x and y axises of the local and the global coordinate system are inverted, it is easy to make confusion.
- Open the folder "Desktop/camera_programs/Andrea CPS/calib_data". There 5 files called "error_camx.txt", where "x" is the number of the camera. Open the one that refers to the camera you are configuring. The format of the file is very easy to understand and consistent with this explanation. Write there the pixel coordinates of x1, y1, x2, y2 and the global coordinates of those points.
- Now you can set RECORD_OBJECT_DATA to 0 and you are good to go.
Work In Progress. A filter Kalman has been implemented on the car in hopes of removing the remaining part of the noise. The model still needs to be refined though in order to test its true usefulness.
Results
The error correction gave a visible improvement to the steer control performance. The car stays much closer to the given path. The error in camera 5 and 2 is constantly below 10cm and rarely above 5cm. Moreover the measured position "leaps" are reduced. I was not able to measure a leap above 15cm.
...
HEADING FILTERING
...
Leaps affect the measured heading too since it is computed from positions. I have thus implemented a model-based filter to eliminate heading jumps. In short, the filter detects when a leap occurs and use the model heading instead of using the measurement when that happens. The algorithm is the following:
- recompute measured_heading // see discussion later to understand why this must be done
- compute model_heading = prev_heading + tan(prev_steer) * prev_speed * 0.1 / wheelbase // previous_steer is in degrees, 0.1 is the car clock in seconds
- compute measured_Dpos = sqrt((prev_x1 - curr_x1)^2 + (prev_x2 - curr_x2)^2)
- compute model_Dpos = prev_speed * 0.1
- if |measure_heading - model_heading| > 30
- keep model_heading
- else if |measure_heading - model_heading| > 20 and |measure_Dpos - model_Dpos| > 0.35 * model_Dpos // this is the uncertain case
- keep model_heading
- else
- keep measured_heading
CPS MEASURED 2D SPEED
Work In Progress. The issues with CPS measured 2D speed detected in this testing are still unresolved. This is not currently a priority since all the encoders are working and can be used for the model identification. These issues will be deepen in case the 2D speed will actually be used for the prediction.
...