Attitude estimation method and apparatus

Abstract

From a first estimation value of attitude of equipment at a first time, a second estimation value of attitude of the equipment at a second time is derived. For this purpose, a predicted value of the attitude at the second time is generated by applying the first estimation value to a state transition model indicating transition of attitude of the equipment. Then, a difference between the predicted value of the attitude and a true value of the attitude of the equipment is calculated based on data outputted from a sensor in the equipment to thereby generate an estimated attitude error between the predicted attitude and true attitude of the equipment. Further, a specific attitude component is extracted from a plurality of attitude components of the estimated attitude error. Lastly, the second estimation value is calculated based on the predicted value of the attitude and the specific attitude component.

What is claimed is: 
     
         1 . A method of estimating attitude of equipment having a plurality of sensors including an acceleration sensor, the method comprising:
 generating a first estimation value of the attitude of the equipment at a first time;   determining whether the attitude of the equipment is stable or not, based on acceleration data outputted from the acceleration sensor;   predicting the attitude of the equipment at a second time after a predetermined period from the first time to thereby generate a predicted value of the attitude, by applying the first estimation value to a state transition model indicating transition of the attitude of the equipment;   estimating a difference between the predicted value of the attitude and a true value of the attitude of the equipment based on data outputted from at least one sensor included in the plurality of the sensors to thereby generate an estimated attitude error between the predicted attitude and true attitude of the equipment;   calculating a second estimation value of the attitude of the equipment at the second time based on the predicted value of the attitude and the estimated attitude error when the attitude of the equipment is determined to be stable;   extracting a specific attitude component from a plurality of attitude components of the estimated attitude error when the attitude of the equipment is determined to be not stable; and   calculating a second estimation value of the attitude of the equipment at the second time based on the predicted value of the attitude and the specific attitude component extracted from the estimated attitude error when the attitude of the equipment is determined to be not stable.   
     
     
         2 . The method according to  claim 1 , wherein the plurality of attitude components of the estimated attitude error comprise a tilt component representing a tilt from an axis extending in a direction of gravity acceleration and a rotation component around the axis, and the specific attitude component is the rotation component obtained by removing the tilt component from the estimated attitude error. 
     
     
         3 . The method according to  claim 1 , wherein the plurality of attitude components are obtained by changing reference attitude of the equipment by the estimated attitude error. 
     
     
         4 . The method according to  claim 1 , wherein the attitude of the equipment is represented by quaternion. 
     
     
         5 . The method according to  claim 1 ,
 wherein the plurality of sensors further include a magnetic sensor and an angular velocity sensor,   wherein the method further comprises: generating an input observation value including, as its elements, magnetic data outputted from the magnetic sensor and acceleration data outputted from the acceleration sensor when the attitude of the equipment is determined to be stable; and generating an input observation value including, as its elements, magnetic data outputted from the magnetic sensor and precluding acceleration data outputted from the acceleration sensor when the attitude of the equipment is determined to be not stable,   wherein the attitude of the equipment at the second time is predicted by applying the first estimation value at the first time and angular velocity data outputted from the angular velocity sensor to the state transition model, and   wherein the estimated attitude error is generated by: applying the predicted value of the attitude at the second time to an observation model presenting relation between elements of the observation value and a value of the attitude so as to estimate the elements of the observation value at the second time, thereby generating an estimated observation value having the estimated elements; calculating difference between the estimated observation value and the input observation value to generate an observation residual representing the calculated difference; and generating the estimated attitude error based on the observation residual and the predicted value of the attitude of the equipment.   
     
     
         6 . The method according to  claim 1 , wherein the attitude of the equipment is determined to be stable in case that an absolute value of difference between a magnitude of the acceleration data outputted from the acceleration sensor and a magnitude of the gravity acceleration is not more than a predetermined value. 
     
     
         7 . A method of estimating attitude of equipment having a sensor, comprising:
 generating a first estimation value of the attitude of the equipment at a first time;   predicting the attitude of the equipment at a second time after a predetermined period from the first time to thereby generate a predicted value of the attitude, by applying the first estimation value to a state transition model indicating transition of the attitude of the equipment;   estimating a difference between the predicted value of the attitude and a true value of the attitude of the equipment based on data outputted from the sensor to thereby generate an estimated attitude error between the predicted attitude and true attitude of the equipment;   extracting a specific attitude component from a plurality of attitude components of the estimated attitude error; and   calculating a second estimation value of the attitude of the equipment at the second time based on the predicted value of the attitude and the specific attitude component extracted from the estimated attitude error.   
     
     
         8 . The method according to  claim 7 , wherein the sensor comprises a magnetic sensor for outputting magnetic data representing geomagnetism. 
     
     
         9 . The method according to  claim 7 , wherein the plurality of attitude components of the estimated attitude error comprise a tilt component representing a tilt from an axis extending in a direction of gravity acceleration and a rotation component around the axis, and the specific attitude component is the rotation component obtained by removing the tilt component from the estimated attitude error. 
     
     
         10 . The method according to  claim 7 , wherein the plurality of attitude components are obtained by changing reference attitude of the equipment by the estimated attitude error. 
     
     
         11 . An apparatus for estimating attitude of equipment having a plurality of sensors including an acceleration sensor, the apparatus comprising one or more processor configured to:
 generate a first estimation value of the attitude of the equipment at a first time;   determine whether the attitude of the equipment is stable or not, based on acceleration data outputted from the acceleration sensor;   predict the attitude of the equipment at a second time after a predetermined period from the first time to thereby generate a predicted value of the attitude, by applying the first estimation value to a state transition model indicating transition of the attitude of the equipment;   estimate a difference between the predicted value of the attitude and a true value of the attitude of the equipment based on data outputted from at least one sensor included in the plurality of the sensors to thereby generate an estimated attitude error between the predicted attitude and true attitude of the equipment;   calculate a second estimation value of the attitude of the equipment at the second time based on the predicted value of the attitude and the estimated attitude error when the attitude of the equipment is determined to be stable;   extract a specific attitude component from a plurality of attitude components of the estimated attitude error when the attitude of the equipment is determined to be not stable; and   calculate a second estimation value of the attitude of the equipment at the second time based on the predicted value of the attitude and the specific attitude component extracted from the estimated attitude error when the attitude of the equipment is determined to be not stable.   
     
     
         12 . An apparatus for estimating attitude of equipment having a sensor, the apparatus comprising one or more processor configured to:
 generate a first estimation value of the attitude of the equipment at a first time;   predict the attitude of the equipment at a second time after a predetermined period from the first time to thereby generate a predicted value of the attitude, by applying the first estimation value to a state transition model indicating transition of the attitude of the equipment;   estimate a difference between the predicted value of the attitude and a true value of the attitude of the equipment based on data outputted from the sensor to thereby generate an estimated attitude error between the predicted attitude and true attitude of the equipment;   extract a specific attitude component from a plurality of attitude components of the estimated attitude error; and   calculate a second estimation value of the attitude of the equipment at the second time based on the predicted value of the attitude and the specific attitude component extracted from the estimated attitude error.