Iterative Feedback Control of Plate-Ball Orientation and Position

This MATLAB function applies the algorithm in 'A Framework for the Stabilization of General Nonholonomic Systems With an Application to the Plate-Ball Mechanism' by Giuseppe Oriolo and Marilena Vendittelli in IEEE TRANSACTIONS ON ROBOTICS, VOL. 21, NO. 2, APRIL 2005

It steers a sphere with a possibly unknown radius that rolls without slipping on the plane to a desired position and orientation by using iterative feedback control.

inputs:
State = [u,v,phi,x,y], (u,v) = (lat, log) contact point, phi = angle between the x-xis and the plane of the meridian through the contact point. u is limited to +/-pi/2 and v is limited to +/-pi

The actual sphere radius is rhoA, while the assumed radius is rho.