As mentioned before the state transition can be described with the following differential equation:

Deterministic Part

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 k times dt, where 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 following:

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.

If yaw_rate = 0:

Stochastic Part

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. 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 standard deviations.


Final Solution

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:

If yaw_rate != 0:

If yaw_rate = 0:

Back to UKF