As mentioned before the state transition can be described with the following differential equation:
Using the assumptions of the CTRV model the integral can be solved more easily. The derivative of the velocity is 0, because we assume
constant velocity magnitude. The integral of the fourth row can be solved to
psi_dot at time
psi_dot is a
known state variable. And since we also assume a constant turn rate, the second derivative of the yaw angle is also 0. This leaves us with the
Once again the assumption of a constant velocity makes our lives easier.
v(t) is now a constant and doesn’t need to be integrated.
Finally the yaw angle for this time can be expressed with its state at time
k plus the change. This leads to:
Solving the integrals for the x and y position change leaves us with the final state transition formula:
However this solution has a problem. The yaw rate can not be equal to zero, because we would then divide by zero in the first two rows of
x_dot. The yaw rate is zero when the car drives a straight line. But instead of solving the integral again with yaw rate set to zero,
the solution can be found with simple trigonometry.
yaw_rate = 0:
That’s it for the deterministic part of the motion model. To account for uncertainty we also have to add some noise (like every other party song wants you to…).
This is done by considering the noise vector
nu contains the longitudinal acceleration noise as well as the
yaw rate acceleration noise. Both are drawn from a normal distribution with zero mean (white noise) and with their respective
All that’s left is to express how the respective noise term influences each state variable. This leads to the final state transition model for the CTRV model:
yaw_rate != 0:
yaw_rate = 0: