I am trying to develop a digital map based on the vehicle inertial sensor information. The Map output will be a road course shape on which the vehicle traveled. The map will be based on a fixed reference point. I have speed, yaw rate and time related information.
what I tried to do is this----
yaw_angle=m_f32_yaw_rate_s* delta_time*0.0174532;
linear_distance=delta_time*m_f32_speed_s;
distance_x_direction= linear_distance*cos(yaw_angle);
distance_y_direction= linear_distance*sin(yaw_angle);
total_yaw_angle=total_yaw_angle+yaw_angle;
total_y_direction=distance_y_direction+total_y_direction;
distance_x_direction_increment=distance_x_direction_increment+distance_x_direction;
temp_keep_2.y=-temp_keep_2.y*cos(total_yaw_angle)-temp_keep_2.x*sin(total_yaw_angle)-total_y_direction;
temp_keep_2.x=temp_keep_2.y*sin(total_yaw_angle)-temp_keep_2.x*cos(total_yaw_angle)-distance_x_direction_increment;
coordinate_virtual_dm_source .push_back(temp_keep_2);
The value temp_keep_2 contains the X and Y which corrosponds to the Vertical and perpendicula distance from a fixed point, like (0,0).
But I think it is wrong. I am not getting expected values.
Any ideas?