Commit 193d1711 by Jordan Ford

Commented the demo code much better.

parent 42a988eb
......@@ -34,6 +34,8 @@ void plotControls(const Trajectory& traj, const std::string& title) {
}
int main() {
// Define a reference trajectory that looks like stair steps.
Trajectory refTraj;
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State{10, 0, 0, 0, 1.0, 10.0});
......@@ -45,26 +47,36 @@ int main() {
refTraj.emplace_back(State{40, 3, 0, 0, 1.0, 43.0});
refTraj.emplace_back(State{40, 4, 0, 0, 1.0, 44.0});
State initialState{0, .5, 0, 0, 2.0, 0.0};
Control initialControl{0, 0};
// Choose an initial state that is slightly offset
// from the beginning of the reference trajectory.
State initialState (0, .5, 0, 0, 2.0, 0.0);
// Choose zero initial control input.
Control initialControl {0, 0};
VehicleParams vehicleParams{2.0}; // vehicle wheelbase length
// Describe the vehicle.
VehicleParams vehicleParams {2.0}; // vehicle wheelbase length
// Select LQR cost matrices.
LQRParams lqrParams;
lqrParams.qLat << 10000, 0, 0, // d
0, 1, 0, // theta* - theta
0, 0, 1000; // delta
//
lqrParams.rLat = 1; // deltaDot
//
lqrParams.qLon << 1, 0, // l
0, 1; // v* - v
//
lqrParams.rLon = 1; // vDot
lqrParams.rLat = 1; // Penalize steering rate.
lqrParams.rLon = 1; // Penalize acceleration.
// Penalize lateral offset heavily (1000).
// Penalize angular error lightly (1).
// Penalize steering moderately (10).
lqrParams.qLat << 1000, 0, 0, 0, 1, 0, 0, 0, 10;
// Penalize longitudinal offset moderately (10).
// Penalize velocity error lightly (10).
lqrParams.qLon << 10, 0, 0, 1;
// Use the LQR trajectory tracker to smooth the reference trajectory.
Trajectory smoothTraj = trackLQR(initialState, initialControl, refTraj,
lqrParams, vehicleParams, 0.01);
// Plot the smoothed trajectory.
plotStates(smoothTraj, "Tracked Trajectory");
plotStates(refTraj, "Tracked Trajectory");
plt::xlim(0, 40);
......@@ -73,6 +85,7 @@ int main() {
plt::ylabel("Y");
plt::show();
// Plot the vehicle controls.
plotControls(smoothTraj, "LQR Smoothed Trajectory Controls");
plt::show();
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment