Iterative estimation of centripetal accelerations of inertial measurement units in kinematic chains

A method of compensating for centripetal accelerations of inertial measurement units (IMUs) disposed on separate but coupled rigid bodies, the method comprising:

• determining a centripetal acceleration of a rigid body at a pivot point between the rigid body and a previous rigid body, the rigid body having an IMU coupled thereto, and the previous rigid body having another IMU coupled thereto, wherein the centripetal acceleration of the rigid body is determined based on:
• an angular velocity of the previous rigid body;
• an angular acceleration of the previous rigid body;
• a centripetal acceleration of the previous rigid body;
• a global angular velocity skew matrix of the previous rigid body;
• a position of a pivot point between the rigid body and a next rigid body relative to the pivot point between the rigid body and the previous rigid body;
• and
• a matrix for rotation between the previous rigid body and the rigid body;
• determining a centripetal acceleration at the IMU of the rigid body, wherein the centripetal acceleration at the IMU of the rigid body is determined based on:
• the centripetal acceleration of the rigid body;
• a location of the IMU on the rigid body;
• an angular acceleration of the rigid body;
• and
• a global angular velocity skew matrix of the rigid body;
• determining a relative angle between the rigid body and the previous rigid body based on a measured acceleration at the IMU of the rigid body, the centripetal acceleration at the IMU of the rigid body, a measured acceleration at the IMU of the previous rigid body, and a centripetal acceleration at the IMU of the previous rigid body;
• determining a centripetal acceleration of a next rigid body at a pivot point between the next rigid body and the rigid body, the rigid body arranged between the previous rigid body and the next rigid body, the next rigid body having another IMU coupled thereto, wherein the centripetal acceleration of the next rigid body is determined based on:
• an angular velocity of the rigid body;
• the angular acceleration of the rigid body;
• the centripetal acceleration of the rigid body;
• the global angular velocity skew matrix of the rigid body;
• a position of the pivot point between the next rigid body and a following rigid body relative to the pivot point between the next rigid body and the rigid body, wherein the following rigid body is after the next rigid body in a series;
• and
• a matrix for rotation between the rigid body and the next rigid body;
• determining a centripetal acceleration at the IMU of the next rigid body, wherein the centripetal acceleration at the IMU of the next rigid body is determined based on:
• the centripetal acceleration of the next rigid body;
• a location of the IMU on the next rigid body;
• an angular acceleration of the next rigid body;
• and
• a global angular velocity skew matrix of the next rigid body;
• determining a relative angle between the next rigid body and the rigid body based on a measured acceleration at the IMU of the next rigid body, the centripetal acceleration at the IMU of the next rigid body, the measured acceleration at the IMU of the rigid body, and the centripetal acceleration at the IMU of the rigid body.