Inclination angle compensation systems and methods

1. A computer-implemented method of determining an inclination of a machine comprising:

receiving a measured inclination angular rate of a machine measured by an inclination angular rate sensor;

estimating an estimated inclination angle of the machine by integrating the measured inclination angular rate;

estimating a non-gravitational acceleration of the machine based on the estimated inclination angle and an acceleration output from a forward acceleration sensor;

receiving a measured inclination angle measured by an inclination angle sensor;

determining an inclination angle sensor acceleration based on the measured inclination angle;

calculating a corrected inclination angle of the machine based on the non-gravitational acceleration and the inclination angle sensor acceleration; and

determining an a posteriori estimated inclination angle of the machine based on the corrected inclination angle,

wherein the corrected inclination angle of the machine is calculated as:

θ

^

=

sin

-

1

⁡

(

A

i

-

A

non

-

g

g

)

,

where A i is an inclination angle sensor acceleration calculated based on the inclination angle output, A non-g is the non-gravitational acceleration and g is a conventional acceleration due to gravity.

2. The computer-implemented method according to claim 1 , wherein the non-gravitational acceleration of the machine is calculated as:

A non-g =A−{circumflex over (β)} A −g sin {circumflex over (θ)} − ,

where A is the acceleration output from the forward acceleration sensor, {circumflex over (β)} A is an acceleration bias estimate, g is a conventional acceleration due to gravity, and {circumflex over (θ)} − is the estimated inclination angle.

3. The computer-implemented method according to claim 1 , wherein the inclination angle sensor acceleration is determined as:

A i =g sin θ,

where g is a conventional acceleration due to gravity and θ is the inclination angle output.

4. The computer-implemented method according to claim 1 , wherein the estimated inclination angle is estimated using a predict phase of a Kalman filter.

5. The computer-implemented method according to claim 4 , further including:

outputting the corrected inclination angle to an update phase of the Kalman filter; and

generating the a posteriori estimated inclination angle with the Kalman filter based on the corrected inclination angle and the estimated inclination angle from the predict phase of the Kalman filter.

6. An inclination angle compensation system for determining an inclination angle of a machine comprising:

an inclination angle sensor configured to measure an inclination angle of a machine;

an inclination angular rate sensor configured to measure an inclination angular rate of the machine;

an inclination angle estimator configured to:

receive the measured inclination angular rate from the inclination angular rate sensor; and

estimate an estimated inclination angle by integrating the inclination angular rate;

a non-gravitational acceleration estimator configured to estimate a non-gravitational acceleration of the machine based on the estimated inclination angle and an acceleration output from a forward acceleration sensor; and

an inclination angle sensor corrector configured to:

receive a measured inclination angle output from an inclination angle sensor;

determine an inclination angle sensor acceleration based on the measured inclination angle;

calculate a corrected inclination angle of the machine based on the non-gravitational acceleration and the inclination angle sensor acceleration; and

determine an a posteriori estimated inclination angle of the machine based on the corrected inclination angle,

wherein the corrected inclination angle of the machine is calculated as:

θ

⋓

=

sin

-

1

⁡

(

A

i

-

A

non

⁢

-

⁢

g

g

)

,

where A i is an inclination angle sensor acceleration calculated based on the inclination angle output, A non-g is the non-gravitational acceleration, and g is a conventional acceleration due to gravity.

7. The system according to claim 6 , wherein the non-gravitational acceleration estimator is further configured to calculate the non-gravitational acceleration of the machine as:

A non-g =A−{circumflex over (β)} A −g sin {circumflex over (θ)} − ,

where A is the acceleration output from the forward acceleration sensor, {circumflex over (β)} A is an acceleration bias estimate, g is a conventional acceleration due to gravity, and {circumflex over (θ)} − is the estimated inclination angle.

8. The system according to claim 6 , wherein the inclination angle sensor corrector is further configured to determine the inclination angle sensor acceleration as:

A i =g sin θ,

where g is a conventional acceleration due to gravity and θ is the inclination angle output.

9. The system according to claim 6 , wherein the inclination angle estimator is further configured to estimate the estimated inclination angle by implementing a predict phase of a Kalman filter.

10. The system according to claim 9 , further including an inclination angle updater, wherein:

the inclination angle sensor corrector is further configured to output the corrected inclination angle to the inclination angle updater; and

the inclination angle updater is configured to generate the a posteriori estimated inclination angle by implementing an update phase of the Kalman filter based on the corrected inclination angle and the estimated inclination angle from the predict phase of the Kalman filter.

11. A non-transitory computer-readable storage device storing instructions for determining an inclination of a machine, the instructions causing one or more computer processors to perform operations comprising:

receiving a measured inclination angular rate of a machine measured by an inclination angular rate sensor;

estimating an estimated inclination angle of the machine by integrating the measured inclination angular rate;

estimating a non-gravitational acceleration of the machine based on the estimated inclination angle and an acceleration output from a forward acceleration sensor;

receiving a measured inclination angle measured by an inclination angle sensor;

determining an inclination angle sensor acceleration based on the measured inclination angle;

calculating a corrected inclination angle of the machine based on the non-gravitational acceleration and the inclination angle sensor acceleration; and

determining an a posteriori estimated inclination angle of the machine based on the corrected inclination angle,

wherein the corrected inclination angle of the machine is calculated as:

θ

⋓

=

sin

-

1

⁡

(

A

i

-

A

non

⁢

-

⁢

g

g

)

,

where A i is an inclination angle sensor acceleration calculated based on the inclination angle output, A non-g is the non-gravitational acceleration and g is a conventional acceleration due to gravity.

12. The non-transitory computer-readable storage device according to claim 11 , wherein the non-gravitational acceleration of the machine is calculated as:

A non-g =A−{circumflex over (β)} A −g sin {circumflex over (θ)} − ,

where A is the acceleration output from the forward acceleration sensor, {circumflex over (β)} A is an acceleration bias estimate, g is a conventional acceleration due to gravity, and {circumflex over (θ)} − is the estimated inclination angle.

13. The non-transitory computer-readable storage device according to claim 11 , wherein the estimated inclination angle is estimated using a predict phase of a Kalman filter.

14. The non-transitory computer-readable storage device according to claim 13 , the instructions further causing the one or more computer processors to perform operations including:

outputting the corrected inclination angle to an update phase of the Kalman filter; and

generating the a posteriori estimated inclination angle with the Kalman filter based on the corrected inclination angle and the estimated inclination angle from the predict phase of the Kalman filter.