Nonlinear kinematic state estimation in rigid-link multibody systems by spherical simplex sigma point unscented Kalman filters

The design of state observers for multibody systems usually relies on dynamic models. Whenever the system links can be assumed rigid, a great improvement in the estimation accuracy can be achieved by employing kinematic constraint equations as the system model. Indeed, such geometrical equations do not involve forces and inertia characteristics and also the observer synthesis does not impose the knowledge of the external forces. These features reduce the overall uncertainty and noise. To this purpose, this paper proposes the general theory for the development of kinematic Kalman filters (KKFs) in multibody systems, suitable for both open and closed chain mechanisms and manipulators. Particular interest is paid to the sigma point unscented Kalman filters, which allow a more effective estimation than the Extended Kalman Filter in the presence of system nonlinearities and does not impose the computation of the Jacobian matrices. Experimental assessment of the developed theory is proposed through a planar multibody system.