06. Solution: Global Kinematic Model
The solution below uses the Eigen library, state and actuator vector inputs, and the equations for the next state (below), to implement the Global Kinematic Model.
x_{t+1} = x_t + v_t * cos(\psi_t) * dt
y_{t+1} = y_t + v_t * sin(\psi_t) * dt
\psi_{t+1} = \psi_t + \frac {v_t} { L_f} * \delta * dt
v_{t+1} = v_t + a_t * dt
Start Quiz:
// In this quiz you'll implement the global kinematic model.
#include <math.h>
#include <iostream>
#include "Dense"
using Eigen::VectorXd;
//
// Helper functions
//
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }
const double Lf = 2;
// Return the next state.
VectorXd globalKinematic(const VectorXd &state,
const VectorXd &actuators, double dt);
int main() {
// [x, y, psi, v]
VectorXd state(4);
// [delta, v]
VectorXd actuators(2);
state << 0, 0, deg2rad(45), 1;
actuators << deg2rad(5), 1;
// should be [0.212132, 0.212132, 0.798488, 1.3]
auto next_state = globalKinematic(state, actuators, 0.3);
std::cout << next_state << std::endl;
}
VectorXd globalKinematic(const VectorXd &state,
const VectorXd &actuators, double dt) {
// Create a new vector for the next state.
VectorXd next_state(state.size());
// NOTE: state is [x, y, psi, v]
auto x = state(0);
auto y = state(1);
auto psi = state(2);
auto v = state(3);
// NOTE: actuators is [delta, a]
auto delta = actuators(0);
auto a = actuators(1);
// Recall the equations for the model:
// x_[t+1] = x[t] + v[t] * cos(psi[t]) * dt
// y_[t+1] = y[t] + v[t] * sin(psi[t]) * dt
// psi_[t+1] = psi[t] + v[t] / Lf * delta[t] * dt
// v_[t+1] = v[t] + a[t] * dt
next_state(0) = x + v * cos(psi) * dt;
next_state(1) = y + v * sin(psi) * dt;
next_state(2) = psi + v / Lf * delta * dt;
next_state(3) = v + a * dt;
return next_state;
}