Commit 42a988eb by Jordan Ford

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

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