Forward Kinematics

Forward Velocity Kinematics

../_images/forward_kinematics.png

Covariance Matrix of 2D-Twist

../_images/twist_covariance.png

Implemention (Core M7)

void ForwardKinematic(float right_linvel, float left_linvel, float wheel_distance)
{
    Robot_LinVel = (right_linvel + left_linvel)*0.5;
    Robot_AngVel = (right_linvel - left_linvel)/wheel_distance;
}

Runtime Test

runstarttime = micros();
//***************************************************************
ForwardKinematic(estimated_rightvel, estimated_leftvel, 0.39377);
//***************************************************************
runtime = micros() - runstarttime;

Runtime: 0.003 ms