Acceleration corrected attitude estimation systems and methods
Techniques are disclosed for systems and methods to provide accurate attitude estimation for a mobile structure. An attitude estimation system includes a logic device in communication with a gyroscope, an accelerometer, and/or a speed sensor. Sensor signals provided by the various sensors are used to determine an estimated absolute coordinate frame relative to the mobile structure, which can be referenced to the Earth's gravity. Angular velocities provided by the gyroscope are transformed to the estimated absolute coordinate frame and used to determine a stabilized attitude estimate for the mobile structure. The stabilized attitude estimate may be displayed to a user, used to calculate a heading or route for the mobile structure, and/or used to adjust a steering actuator, a propulsion system thrust, and/or other operational systems of the mobile structure.
1. A system comprising:
a logic device configured to receive one or more sensor signals from one or more sensors mounted on a mobile structure, and determine a stabilized attitude estimate for the mobile structure, wherein the logic device is adapted to:
receive an angular velocity, an acceleration, and a speed of the mobile structure;
determine a motion compensated acceleration of the mobile structure from the acceleration of the mobile structure and, at least in part, the angular velocity and/or speed;
determine an estimated absolute coordinate frame based on the motion compensated acceleration and, at least in part, the angular velocity, and/or the speed;
determine the stabilized attitude estimate based, at least in part, on the angular velocity and the estimated absolute coordinate frame; and
control operation of the mobile structure based, at least in part, on the stabilized attitude estimate.
2. The system of claim 1 , wherein the speed of the mobile structure comprises a calculated speed based, at least in part, on a measured speed and/or a global positioning satellite-derived speed.
3. The system of claim 1 , wherein:
the angular velocity comprises a bias corrected angular velocity based, at least in part, on one or more bias correction factors; and
the one or more bias correction factors are based, at least in part, on a prior estimated absolute coordinate frame and a prior stabilized attitude estimate.
4. The system of claim 3 , wherein the logic device is adapted to:
determine, from the angular velocity and/or speed, that the mobile structure is in a bias accumulation state; and
update the one or more bias correction factors based, at least in part, on the estimated absolute coordinate frame and the stabilized attitude estimate.
5. The system of claim 1 , wherein the estimated absolute coordinate frame corresponds to an estimated absolute roll and/or pitch of the mobile structure, and wherein the logic device is adapted to:
determine the motion compensated acceleration of the mobile structure by removing a centripetal acceleration of the mobile structure from the acceleration of the mobile structure; and
determine the estimated absolute roll and/or pitch based, at least in part, on the motion compensated acceleration of the mobile structure.
6. The system of claim 5 , wherein the determining the estimated absolute roll and/or pitch of the mobile structure comprises:
determining the estimated absolute roll, wherein the estimated absolute roll is based, at least in part, on a ratio of a lateral component of the motion compensated acceleration to a vertical component of the motion compensated acceleration;
transforming the motion compensated acceleration to a rotated reference frame by rotating a coordinate frame of the motion compensated acceleration about a longitudinal axis of the coordinate frame, wherein a magnitude of the rotation is substantially equal to the estimated absolute roll; and
determining the estimated absolute pitch, wherein the estimated absolute pitch is based, at least in part, on a ratio of a longitudinal component of the transformed motion compensated acceleration to a vertical component of the transformed motion compensated acceleration.
7. The system of claim 5 , wherein the logic device is adapted to:
determine a longitudinal component of the motion compensated acceleration based, at least in part, on a time derivative of the speed; and
determine lateral and vertical components of the motion compensated acceleration based, at least in part, on respective products of the speed with yaw and pitch components of the angular velocity.
8. The system of claim 7 , wherein the yaw component of the angular velocity comprises a computed yaw rate based, at least in part, on a deadband filtered yaw component of the angular velocity and/or a global positioning satellite-derived yaw component of the angular velocity.
9. The system of claim 1 , wherein the logic device is adapted to:
determine an estimated absolute angular velocity based, at least in part, on the estimated absolute coordinate frame and a prior stabilized attitude estimate; and
determine the stabilized attitude estimate based on the estimated absolute angular velocity.
10. The system of claim 1 , wherein:
the mobile structure comprises a watercraft, aircraft, robot, drone, and/or a type of vehicle;
the estimated absolute coordinate frame corresponds to a horizontal plane local to the mobile structure and comprises an undefined yaw reference;
the stabilized attitude estimate corresponds to a stabilized roll and pitch estimate referenced to the horizontal plane; and
the logic device is adapted to transform the angular velocity to the estimated absolute coordinate frame and determine the stabilized attitude estimate based, at least in part, on the transformed angular velocity.
11. A method comprising:
receiving one or more sensor signals from one or more sensors mounted on a mobile structure, representing an angular velocity, an acceleration, and a speed of the mobile structure;
determining a motion compensated acceleration of the mobile structure from the acceleration of the mobile structure and, at least in part, the angular velocity and/or speed;
determining an estimated absolute coordinate frame based on the motion compensated acceleration and, at least in part, an angular velocity, and/or a speed of the mobile structure;
determining a stabilized attitude estimate for the mobile structure based, at least in part, on the angular velocity and the estimated absolute coordinate frame; and
controlling operation of the mobile structure based, at least in part, on the stabilized attitude estimate.
12. The method of claim 11 , wherein the speed of the mobile structure comprises a calculated speed based, at least in part, on a measured speed and/or a global positioning satellite-derived speed.
13. The method of claim 11 , wherein:
the angular velocity comprises a bias corrected angular velocity based, at least in part, on one or more bias correction factors; and
the one or more bias correction factors are based, at least in part, on a prior estimated absolute coordinate frame and a prior stabilized attitude estimate.
14. The method of claim 13 , further comprising:
determining, from the angular velocity and/or speed, that the mobile structure is in a bias accumulation state; and
updating the one or more bias correction factors based, at least in part, on the estimated absolute coordinate frame and the stabilized attitude estimate.
15. The method of claim 11 , wherein the estimated absolute coordinate frame corresponds to an estimated absolute roll and pitch of the mobile structure, the method further comprising:
determining the motion compensated acceleration of the mobile structure by removing a centripetal acceleration of the mobile structure from the acceleration of the mobile structure; and
determining the estimated absolute roll and pitch based, at least in part, on the motion compensated acceleration of the mobile structure.
16. The method of claim 15 , wherein the determining the estimated absolute roll and pitch of the mobile structure comprises:
determining the estimated absolute roll, wherein the estimated absolute roll is based, at least in part, on a ratio of a lateral component of the motion compensated acceleration to a vertical component of the motion compensated acceleration;
transforming the motion compensated acceleration to a rotated reference frame by rotating a coordinate frame of the motion compensated acceleration about a longitudinal component of the coordinate frame, wherein a magnitude of the rotation is substantially equal to the estimated absolute roll; and
determining the estimated absolute pitch, wherein the estimated absolute pitch is based, at least in part, on a ratio of a longitudinal component of the transformed motion compensated acceleration to a vertical component of the transformed motion compensated acceleration.
17. The method of claim 15 , further comprising:
determining a longitudinal component of the motion compensated acceleration based, at least in part, on a time derivative of the speed; and
determining lateral and vertical components of the motion compensated acceleration based, at least in part, on respective products of the speed with yaw and pitch components of the angular velocity.
18. The method of claim 17 , wherein the yaw component of the angular velocity comprises a computed yaw rate based, at least in part, on a deadband filtered yaw component of the angular velocity and/or a global positioning satellite-derived yaw component of the angular velocity.
19. The method of claim 11 , further comprising:
determining an estimated absolute angular velocity based, at least in part, on the estimated absolute coordinate frame and a prior stabilized attitude estimate; and
determining the stabilized attitude estimate based on the estimated absolute angular velocity.
20. The method of claim 11 , further comprising:
transforming the angular velocity to the estimated absolute coordinate frame; and
determining the stabilized attitude estimate based, at least in part, on the transformed angular velocity.