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