Commit 42a988eb by Jordan Ford

Now using 4th order Runge-Kutta to forward integrate the dynamics in LQR.

parent cac349a7
...@@ -9,7 +9,8 @@ ...@@ -9,7 +9,8 @@
namespace track { namespace track {
State dynamicsBicycleModel(const State& state, const Control& control, const VehicleParams& params) { State dynamics(const State& state, const Control& control,
const VehicleParams& params) {
State dState; State dState;
dState.x() = state.velocity() * std::cos(state.theta()); dState.x() = state.velocity() * std::cos(state.theta());
dState.y() = state.velocity() * std::sin(state.theta()); dState.y() = state.velocity() * std::sin(state.theta());
...@@ -21,13 +22,10 @@ State dynamicsBicycleModel(const State& state, const Control& control, const Veh ...@@ -21,13 +22,10 @@ State dynamicsBicycleModel(const State& state, const Control& control, const Veh
return dState; return dState;
} }
Trajectory trackLQR(const State& initialState, Trajectory trackLQR(const State& initialState, const Control& initialControl,
const Control& initialControl,
const Trajectory& referenceTrajectory, const Trajectory& referenceTrajectory,
const LQRParams& lqrParams, const LQRParams& lqrParams,
const VehicleParams& vehicleParams, const VehicleParams& vehicleParams, const double epsTime) {
const double epsTime) {
assert(checkTrajectoryValid(referenceTrajectory)); assert(checkTrajectoryValid(referenceTrajectory));
assert(referenceTrajectory[0].time() == initialState.time()); assert(referenceTrajectory[0].time() == initialState.time());
...@@ -81,8 +79,8 @@ Trajectory trackLQR(const State& initialState, ...@@ -81,8 +79,8 @@ Trajectory trackLQR(const State& initialState,
xLon << l, targetState.velocity() - state.velocity(); xLon << l, targetState.velocity() - state.velocity();
// Re-linearize the model for lateral control. // Re-linearize the model for lateral control.
aLat << 0, state.velocity(), 0, 0, 0, -state.velocity() / vehicleParams.wheelbase, 0, aLat << 0, state.velocity(), 0, 0, 0,
0, 0; -state.velocity() / vehicleParams.wheelbase, 0, 0, 0;
// Recompute the lateral control gain. // Recompute the lateral control gain.
Matrix3d pLat = track::care(aLat, bLat, lqrParams.qLat, rLatMatrix); Matrix3d pLat = track::care(aLat, bLat, lqrParams.qLat, rLatMatrix);
...@@ -93,18 +91,16 @@ Trajectory trackLQR(const State& initialState, ...@@ -93,18 +91,16 @@ Trajectory trackLQR(const State& initialState,
controls.deltaDot = -kLat * xLat; controls.deltaDot = -kLat * xLat;
controls.vDot = -kLon * xLon; controls.vDot = -kLon * xLon;
// TODO: Use RK4 to forward integrate. // Use RK4 to forward integrate with constant controls for one timestep.
/*
state.x += state.velocity * (std::cos(state.theta)) * epsTime; // clang-format off
state.y += state.velocity * (std::sin(state.theta)) * epsTime; State k1(epsTime * dynamics(state, controls, vehicleParams).vec());
state.theta += (std::tan(state.delta) / vehicleParams.wheelbase) * epsTime; State k2(epsTime * dynamics(State(state.vec() + k1.vec() / 2.0), controls, vehicleParams).vec());
state.delta = controls.deltaDot * epsTime; State k3(epsTime * dynamics(State(state.vec() + k2.vec() / 2.0), controls, vehicleParams).vec());
state.velocity += controls.vDot * epsTime; State k4(epsTime * dynamics(State(state.vec() + k3.vec()), controls, vehicleParams).vec());
state.time += epsTime;
*/ state.vec() += (1.0 / 6.0) * (k1.vec() + 2 * k2.vec() + 2 * k3.vec() + k4.vec());
// clang-format on
State dState = dynamicsBicycleModel(state, controls, vehicleParams);
state.vec() += dState.vec()*epsTime;
} }
return trackTrajectory; return trackTrajectory;
......
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