System and method for determining machine state using sensor fusion

1. A method of controlling movement of a machine, comprising:

receiving, with at least one processor, from each of a plurality of Inertial Measurement Units (IMU's) mounted on different components of the machine, a time series of signals indicative of acceleration and angular rate of motion measurements for each of the components of the machine on which one or more of the plurality of IMU's are mounted;

for each one of the IMU's on a separate component of the machine, fusing the signals received from the IMU with a separate Kalman filter module of the at least one processor, the fusing of the signals from each one of the plurality of IMU's comprising:

combining an acceleration measurement and an angular rate of motion measurement from the IMU to estimate an output joint angle for the component of the machine on which the IMU is mounted during a timestep of a series of timesteps;

generating an a priori estimate of the output joint angle for the component in a subsequent timestep based on an estimate of the output joint angle from a previous timestep;

combining the a priori estimate of the output joint angle with an estimate of the accuracy of the a priori estimate and a current measurement value received from the IMU to produce a refined a posteriori estimate of the output joint angle;

fusing the refined a posteriori estimates of the output joint angles for a plurality of the components of the machine with each other and in reference to a machine reference frame to determine best estimates of current output joint angles for each of the plurality of components of the machine with reference to the machine reference frame;

solving a kinematic equation using the best estimates of current output joint angles for the components of the machine and structural design information characterizing the machine;

determining from the solution of the kinematic equation a real time value for at least one of position, velocity, and acceleration of each machine component of interest at successive timesteps of the series of timesteps; and

applying the determined real time value in an implementation of a controlled operational movement of each machine component of interest.

2. The method of claim 1 , wherein the separate Kalman filter modules for each of the plurality of IMU's on the separate components of the machine perform an estimation of bias of measurements received from a gyroscope associated with each IMU, and compensate for the bias.

3. The method of claim 1 , further including determining from successive positions of each machine component of interest whether a machine component is being moved in an arc during a swinging motion of the machine, and compensating for centripetal acceleration of a machine component that is being moved in an arc in acceleration measurements received from an IMU mounted on the machine component before inputting the acceleration measurements to a Kalman filter module.

4. The method of claim 1 , further including determining the accuracy of an a posteriori estimate of an output joint angle based on an estimate of the accuracy of an actual measured value received from an IMU.

5. The method of claim 4 , further including determining a weight to be associated with each successive a priori estimate of an output joint angle for a machine component relative to a weight to be associated with each successive a posteriori estimate based on successive actual measured values received from each IMU, and assigning a Kalman gain representative of the relative weights.

6. The method of claim 5 , further including assigning the Kalman gain by retrieving from a predetermined gain schedule a state covariance matrix representative of the predicted variability in the a priori estimates of each output joint angle for a machine component, and an estimated measurements covariance matrix representative of the predicted variability in the actual measurements received from each IMU.

7. The method of claim 1 , further including determining from the solution of the kinematic equation for each machine component a location and angular orientation relative to a global reference frame of each IMU positioned on the machine component.

8. The method of claim 7 , further including applying an offset to a determined location and angular orientation of an IMU positioned on a machine component to solve for at least one of a location or angular rotation of another point of interest on the machine component.

9. The method of claim 1 , further including:

receiving, with the at least one processor, a signal from a non-IMU sensor located on the machine, the signal from the non-IMU sensor being indicative of at least one of position, velocity, and acceleration of a machine component; and

fusing the signal received from the non-IMU sensor with a signal received from an IMU mounted on a machine component using the separate Kalman filter module also used to fuse the signals received from the IMU.

10. A system for controlling movement of a machine, comprising:

a plurality of Inertial Measurement Units (IMU's) mounted on separate components of the machine, the IMU's each being configured to generate a time series of signals indicative of acceleration and angular rate of motion measurements for the component of the machine on which the IMU is mounted; and

at least one processor in communication with each of the IMU's, each of the at least one processor being configured to:

fuse the signals received from the IMU with a separate Kalman filter module of the at least one processor, the fusing of the signals from each one of the plurality of IMU's comprising:

combining an acceleration measurement and an angular rate of motion measurement from the IMU to estimate an output joint angle for the component of the machine on which the IMU is mounted during a timestep of a series of timesteps;

generating an a priori estimate of the output joint angle for the component in a subsequent timestep based on an estimate of the output joint angle from a previous timestep; and

combining the a priori estimate of the output joint angle with an estimate of the accuracy of the a priori estimate and a current measurement value received from the IMU to produce a refined a posteriori estimate of the output joint angle; and

fuse the refined a posteriori estimates of the output joint angles for a plurality of the components of the machine with each other and in reference to a machine reference frame to determine best estimates of current output joint angles for each of the plurality of components of the machine with reference to the machine reference frame;

solve a kinematic equation using the best estimates of current output joint angles for the components of the machine and structural design information characterizing the machine;

determine from the solution of the kinematic equation a real time value for at least one of position, velocity, and acceleration of each machine component of interest at successive timesteps of the series of timesteps; and

apply the determined real time value in an implementation of a controlled operational movement of each machine component of interest.

11. The system of claim 10 , wherein the separate Kalman filter modules for each of the plurality of IMU's on the separate components of the machine perform an estimation of bias of measurements received from a gyroscope associated with each IMU, and compensate for the bias.

12. The system of claim 10 , wherein the at least one processor is further configured to determine from successive positions of each machine component of interest whether a machine component is being moved in an arc during a swinging motion of the machine, and compensate for centripetal acceleration of a machine component that is being moved in an arc in acceleration measurements received from an IMU mounted on the machine component before inputting the acceleration measurements to a Kalman filter module.

13. The system of claim 10 , wherein the at least one processor is further configured to determine the accuracy of an a posteriori estimate of an output joint angle based on an estimate of the accuracy of an actual measured value received from an IMU.

14. The system of claim 13 , wherein the at least one processor is further configured to determine a weight to be associated with each successive a priori estimate of an output joint angle for a machine component relative to a weight to be associated with each successive a posteriori estimate based on successive actual measured values received from each IMU, and assign a Kalman gain representative of the relative weights.

15. The system of claim 14 , wherein the at least one processor is further configured to assign the Kalman gain by retrieving from a predetermined gain schedule a state covariance matrix representative of the predicted variability in the a priori estimates of each output joint angle for a machine component, and an estimated measurements covariance matrix representative of the predicted variability in the actual measurements received from each IMU.

16. The system of claim 10 , wherein the at least one processor is further configured to determine from the solution of the kinematic equation for each machine component a location and angular orientation relative to a global reference frame of each IMU positioned on the machine component.

17. The system of claim 16 , wherein the at least one processor is further configured to apply an offset to a determined location and angular orientation of an IMU positioned on a machine component to solve for at least one of a location or angular rotation of another point of interest on the machine component.

18. The system of claim 10 , further including:

at least one non-IMU sensor mounted on the machine; and

the at least one processor being further configured to:

receive a signal from the at least one non-IMU sensor, the signal from the at least one non-IMU sensor being indicative of at least one of position, velocity, and acceleration of a machine component; and

fuse the signal received from the non-IMU sensor with a signal received from an IMU mounted on a machine component using the separate Kalman filter module also used to fuse the signals received from the IMU.

19. A non-transitory computer-readable storage device storing instructions for enabling a processor to execute a method of controlling movement of a machine, the method comprising:

receiving, with the processor, from each of a plurality of Inertial Measurement Units (IMU's) mounted on different components of the machine, a time series of signals indicative of acceleration and angular rate of motion measurements for each of the components of the machine on which one or more of the plurality of IMU's are mounted;

for each one of the IMU's on a separate component of the machine, fusing the signals received from the IMU with a separate Kalman filter module of the processor, the fusing of the signals from each one of the plurality of IMU's comprising:

combining an acceleration measurement and an angular rate of motion measurement from the IMU to estimate an output joint angle for the component of the machine on which the IMU is mounted during a timestep of a series of timesteps;

generating an a priori estimate of the output joint angle for the component in a subsequent timestep based on an estimate of the output joint angle from a previous timestep;

combining the a priori estimate of the output joint angle with an estimate of the accuracy of the a priori estimate and a current measurement value received from the IMU to produce a refined a posteriori estimate of the output joint angle;

fusing the refined a posteriori estimates of the output joint angles for a plurality of the components of the machine with each other and in reference to a machine reference frame to determine best estimates of current output joint angles for each of the plurality of components of the machine with reference to the machine reference frame;

solving a kinematic equation using the best estimates of current output joint angles for the components of the machine and structural design information characterizing the machine;

determining from the solution of the kinematic equation a real time value for at least one of position, velocity, and acceleration of each machine component of interest at successive timesteps of the series of timesteps; and

applying the determined real time value in an implementation of a controlled operational movement of each machine component of interest.

20. The non-transitory computer-readable storage device of claim 19 , wherein the method further comprises:

performing an estimation of bias of measurements received from a gyroscope associated with each IMU by the Kalman filter module for the IMU on a component of the machine;

compensating for the bias in the gyroscope measurements with the processor;

determining from successive positions of each machine component of interest whether a machine component is being moved in an arc during a swinging motion of the machine; and

compensating for centripetal acceleration of a machine component that is being moved in an arc in acceleration measurements received from an IMU mounted on the machine component before inputting the acceleration measurements to the associated Kalman filter module.