I am using Constant Turn Rate and Veloctity (CTRV) model to predict the position of the vehicle.The CTRV model assumes a circular path between two consecutive time steps of the car as depicted in the figure, where the vehicle moves from point A to point B. Also, the yaw rate and velocity are assumed constant between two timesteps.
The displacement between the two points is denoted by S and R is the radius of the curvature. The five input parameters of the CTRV model are [ x, y, yaw angle, velocity and yaw rate]. The previous x, y position of the car is known to me. The laser data of the surrounding environment is available for each timestep. With this laser data, I have already calculated the Rotation matrix and translation vector between two time steps using ICP.
Now my question is how to calculate the input parameters of the CTRV model with the available data?
The followings things are clear to me: -
With the help of rotation angle I can calculate: - Theta = Displacement (S) / Radius (R)
The velocity can be calculated using the timesteps :- Velocity = Displacement (S) / t
The yaw rate can be calculated using :- Velocity = Yaw rate * Radius (R)
Considering the first equation I am not sure how to calculate the displacement S if Radius is not known to me.
Pardon me if my queries are a bit naive. I am still in the process of learning. Any help from your side will be much appreciated :)