05. Global Kinematic Model
Now we’ve developed equations to determine the next state (state at t+1) from our state vector at t and our actuator values. Note that we’ve added a variable to our state called L_f which measures the distance between the front of the vehicle and its center of gravity. The larger the vehicle , the slower the turn rate.
\large x_{t+1} = x_t + v_t cos(\psi_t) * dt
\large y_{t+1} = y_t + v_t sin(\psi_t) * dt
\large \psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta * dt
\large v_{t+1} = v_t + a_t * dt
In the quiz below we will implement the global kinematic model to return the next state vector, given a current state input vector and an actuator vector.
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());
/**
* TODO: Implement the global kinematic model,
* to return the next state from the inputs.
*/
// NOTE: state is [x, y, psi, v] and actuators is [delta, a]
return next_state;
}
INSTRUCTOR NOTE:
Code and files for running this quiz locally can be found here .
[x, y, \psi, v] is the state of the vehicle, L_f is a physical characteristic of the vehicle, and [\delta, a] are the actuators, or control inputs, to our system.
This is a good kinematic model!
Now that we have this, we can use the model to write a simulation where we can develop a controller to follow a trajectory.