Pseudo code with arbitrary values of a Simple Bicycle Model given steering change.
The x-axis points to the forward direction or the longitudinal direction
The y-axis, which represents the lateral direction is positive when it points to the right of the driver
The z-axis points to the ground satisfying the right hand rule.
SPEED = 10.0 // in km/h (velocity without direction) LENGTH= 20.0 //in meters x = 100 // in some sort of grid position y = 200 // in some sort of grid position theta = 20 // yaw in degrees of heading delta = 30 // angle rate change in degrees caused by steering
delta_in_radians = pi/180 * delta next_x = x + SPEED * cos(theta) // translational velocity + current state times cos angle of heading next_y = y + SPEED * sin(theta) // translational velocity + current state times sin angle of heading omega = SPEED/LENGTH * tan(delta_in_radians) //angular velocity divided by length to get an accurate revolution times the tangent of the rate of change next_theta = omega+theta next_theta < 0 ? next_theta +2*pi : next_thet