Digital Map design

[blackmagic01021]

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?